2739 files changed, 130731 insertions, 62181 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_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/acpi_platform.c b/drivers/acpi/acpi_platform.c
index 1284138e42ab..4bf75597f732 100644
--- a/drivers/acpi/acpi_platform.c
+++ b/drivers/acpi/acpi_platform.c
@@ -102,7 +102,7 @@ struct platform_device *acpi_create_platform_device(struct acpi_device *adev)
 	pdevinfo.id = -1;
 	pdevinfo.res = resources;
 	pdevinfo.num_res = count;
-	pdevinfo.acpi_node.companion = adev;
+	pdevinfo.fwnode = acpi_fwnode_handle(adev);
 	pdevinfo.dma_mask = DMA_BIT_MASK(32);
 	pdev = platform_device_register_full(&pdevinfo);
 	if (IS_ERR(pdev))
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index d98ba4355819..63d43677f644 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)
 {
@@ -530,8 +531,8 @@ static int acpi_battery_get_state(struct acpi_battery *battery)
 		battery->rate_now != ACPI_BATTERY_VALUE_UNKNOWN &&
 		(s16)(battery->rate_now) < 0) {
 		battery->rate_now = abs((s16)battery->rate_now);
-		printk_once(KERN_WARNING FW_BUG "battery: (dis)charge rate"
-			" invalid.\n");
+		printk_once(KERN_WARNING FW_BUG
+			    "battery: (dis)charge rate invalid.\n");
 	}
 
 	if (test_bit(ACPI_BATTERY_QUIRK_PERCENTAGE_CAPACITY, &battery->flags)
@@ -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/blacklist.c b/drivers/acpi/blacklist.c
index 9b693d54c743..1d1791935c31 100644
--- a/drivers/acpi/blacklist.c
+++ b/drivers/acpi/blacklist.c
@@ -215,6 +215,14 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 	},
 	{
 	.callback = dmi_disable_osi_vista,
+	.ident = "VGN-SR19XN",
+	.matches = {
+		     DMI_MATCH(DMI_SYS_VENDOR, "Sony Corporation"),
+		     DMI_MATCH(DMI_PRODUCT_NAME, "VGN-SR19XN"),
+		},
+	},
+	{
+	.callback = dmi_disable_osi_vista,
 	.ident = "Toshiba Satellite L355",
 	.matches = {
 		     DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
diff --git a/drivers/acpi/dock.c b/drivers/acpi/dock.c
index d9339b442a4e..a688aa243f6c 100644
--- a/drivers/acpi/dock.c
+++ b/drivers/acpi/dock.c
@@ -615,7 +615,7 @@ void acpi_dock_add(struct acpi_device *adev)
 	memset(&pdevinfo, 0, sizeof(pdevinfo));
 	pdevinfo.name = "dock";
 	pdevinfo.id = dock_station_count;
-	pdevinfo.acpi_node.companion = adev;
+	pdevinfo.fwnode = acpi_fwnode_handle(adev);
 	pdevinfo.data = &ds;
 	pdevinfo.size_data = sizeof(ds);
 	dd = platform_device_register_full(&pdevinfo);
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index a8dd2f763382..220d6406c9e9 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -137,6 +137,50 @@ static int EC_FLAGS_CLEAR_ON_RESUME; /* Needs acpi_ec_clear() on boot/resume */
 static int EC_FLAGS_QUERY_HANDSHAKE; /* Needs QR_EC issued when SCI_EVT set */
 
 /* --------------------------------------------------------------------------
+ *                           Logging/Debugging
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Splitters used by the developers to track the boundary of the EC
+ * handling processes.
+ */
+#ifdef DEBUG
+#define EC_DBG_SEP	" "
+#define EC_DBG_DRV	"+++++"
+#define EC_DBG_STM	"====="
+#define EC_DBG_REQ	"*****"
+#define EC_DBG_EVT	"#####"
+#else
+#define EC_DBG_SEP	""
+#define EC_DBG_DRV
+#define EC_DBG_STM
+#define EC_DBG_REQ
+#define EC_DBG_EVT
+#endif
+
+#define ec_log_raw(fmt, ...) \
+	pr_info(fmt "\n", ##__VA_ARGS__)
+#define ec_dbg_raw(fmt, ...) \
+	pr_debug(fmt "\n", ##__VA_ARGS__)
+#define ec_log(filter, fmt, ...) \
+	ec_log_raw(filter EC_DBG_SEP fmt EC_DBG_SEP filter, ##__VA_ARGS__)
+#define ec_dbg(filter, fmt, ...) \
+	ec_dbg_raw(filter EC_DBG_SEP fmt EC_DBG_SEP filter, ##__VA_ARGS__)
+
+#define ec_log_drv(fmt, ...) \
+	ec_log(EC_DBG_DRV, fmt, ##__VA_ARGS__)
+#define ec_dbg_drv(fmt, ...) \
+	ec_dbg(EC_DBG_DRV, fmt, ##__VA_ARGS__)
+#define ec_dbg_stm(fmt, ...) \
+	ec_dbg(EC_DBG_STM, fmt, ##__VA_ARGS__)
+#define ec_dbg_req(fmt, ...) \
+	ec_dbg(EC_DBG_REQ, fmt, ##__VA_ARGS__)
+#define ec_dbg_evt(fmt, ...) \
+	ec_dbg(EC_DBG_EVT, fmt, ##__VA_ARGS__)
+#define ec_dbg_ref(ec, fmt, ...) \
+	ec_dbg_raw("%lu: " fmt, ec->reference_count, ## __VA_ARGS__)
+
+/* --------------------------------------------------------------------------
  *                           Device Flags
  * -------------------------------------------------------------------------- */
 
@@ -159,14 +203,14 @@ static inline u8 acpi_ec_read_status(struct acpi_ec *ec)
 {
 	u8 x = inb(ec->command_addr);
 
-	pr_debug("EC_SC(R) = 0x%2.2x "
-		 "SCI_EVT=%d BURST=%d CMD=%d IBF=%d OBF=%d\n",
-		 x,
-		 !!(x & ACPI_EC_FLAG_SCI),
-		 !!(x & ACPI_EC_FLAG_BURST),
-		 !!(x & ACPI_EC_FLAG_CMD),
-		 !!(x & ACPI_EC_FLAG_IBF),
-		 !!(x & ACPI_EC_FLAG_OBF));
+	ec_dbg_raw("EC_SC(R) = 0x%2.2x "
+		   "SCI_EVT=%d BURST=%d CMD=%d IBF=%d OBF=%d",
+		   x,
+		   !!(x & ACPI_EC_FLAG_SCI),
+		   !!(x & ACPI_EC_FLAG_BURST),
+		   !!(x & ACPI_EC_FLAG_CMD),
+		   !!(x & ACPI_EC_FLAG_IBF),
+		   !!(x & ACPI_EC_FLAG_OBF));
 	return x;
 }
 
@@ -175,20 +219,20 @@ static inline u8 acpi_ec_read_data(struct acpi_ec *ec)
 	u8 x = inb(ec->data_addr);
 
 	ec->curr->timestamp = jiffies;
-	pr_debug("EC_DATA(R) = 0x%2.2x\n", x);
+	ec_dbg_raw("EC_DATA(R) = 0x%2.2x", x);
 	return x;
 }
 
 static inline void acpi_ec_write_cmd(struct acpi_ec *ec, u8 command)
 {
-	pr_debug("EC_SC(W) = 0x%2.2x\n", command);
+	ec_dbg_raw("EC_SC(W) = 0x%2.2x", command);
 	outb(command, ec->command_addr);
 	ec->curr->timestamp = jiffies;
 }
 
 static inline void acpi_ec_write_data(struct acpi_ec *ec, u8 data)
 {
-	pr_debug("EC_DATA(W) = 0x%2.2x\n", data);
+	ec_dbg_raw("EC_DATA(W) = 0x%2.2x", data);
 	outb(data, ec->data_addr);
 	ec->curr->timestamp = jiffies;
 }
@@ -240,7 +284,7 @@ static inline void acpi_ec_enable_gpe(struct acpi_ec *ec, bool open)
 		 * software need to manually trigger a pseudo GPE event on
 		 * EN=1 writes.
 		 */
-		pr_debug("***** Polling quirk *****\n");
+		ec_dbg_raw("Polling quirk");
 		advance_transaction(ec);
 	}
 }
@@ -299,7 +343,7 @@ static void acpi_ec_set_storm(struct acpi_ec *ec, u8 flag)
 {
 	if (!test_bit(flag, &ec->flags)) {
 		acpi_ec_disable_gpe(ec, false);
-		pr_debug("+++++ Polling enabled +++++\n");
+		ec_dbg_drv("Polling enabled");
 		set_bit(flag, &ec->flags);
 	}
 }
@@ -309,7 +353,7 @@ static void acpi_ec_clear_storm(struct acpi_ec *ec, u8 flag)
 	if (test_bit(flag, &ec->flags)) {
 		clear_bit(flag, &ec->flags);
 		acpi_ec_enable_gpe(ec, false);
-		pr_debug("+++++ Polling disabled +++++\n");
+		ec_dbg_drv("Polling disabled");
 	}
 }
 
@@ -335,7 +379,7 @@ static bool acpi_ec_submit_flushable_request(struct acpi_ec *ec)
 static void acpi_ec_submit_query(struct acpi_ec *ec)
 {
 	if (!test_and_set_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
-		pr_debug("***** Event started *****\n");
+		ec_dbg_req("Event started");
 		schedule_work(&ec->work);
 	}
 }
@@ -344,7 +388,7 @@ static void acpi_ec_complete_query(struct acpi_ec *ec)
 {
 	if (ec->curr->command == ACPI_EC_COMMAND_QUERY) {
 		clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags);
-		pr_debug("***** Event stopped *****\n");
+		ec_dbg_req("Event stopped");
 	}
 }
 
@@ -366,8 +410,8 @@ static void advance_transaction(struct acpi_ec *ec)
 	u8 status;
 	bool wakeup = false;
 
-	pr_debug("===== %s (%d) =====\n",
-		 in_interrupt() ? "IRQ" : "TASK", smp_processor_id());
+	ec_dbg_stm("%s (%d)", in_interrupt() ? "IRQ" : "TASK",
+		   smp_processor_id());
 	/*
 	 * By always clearing STS before handling all indications, we can
 	 * ensure a hardware STS 0->1 change after this clearing can always
@@ -390,8 +434,8 @@ static void advance_transaction(struct acpi_ec *ec)
 				if (t->rlen == t->ri) {
 					t->flags |= ACPI_EC_COMMAND_COMPLETE;
 					if (t->command == ACPI_EC_COMMAND_QUERY)
-						pr_debug("***** Command(%s) hardware completion *****\n",
-							 acpi_ec_cmd_string(t->command));
+						ec_dbg_req("Command(%s) hardware completion",
+							   acpi_ec_cmd_string(t->command));
 					wakeup = true;
 				}
 			} else
@@ -410,8 +454,8 @@ static void advance_transaction(struct acpi_ec *ec)
 			acpi_ec_complete_query(ec);
 			t->rdata[t->ri++] = 0x00;
 			t->flags |= ACPI_EC_COMMAND_COMPLETE;
-			pr_debug("***** Command(%s) software completion *****\n",
-				 acpi_ec_cmd_string(t->command));
+			ec_dbg_req("Command(%s) software completion",
+				   acpi_ec_cmd_string(t->command));
 			wakeup = true;
 		} else if ((status & ACPI_EC_FLAG_IBF) == 0) {
 			acpi_ec_write_cmd(ec, t->command);
@@ -502,21 +546,21 @@ static int acpi_ec_transaction_unlocked(struct acpi_ec *ec,
 		ret = -EINVAL;
 		goto unlock;
 	}
+	ec_dbg_ref(ec, "Increase command");
 	/* following two actions should be kept atomic */
 	ec->curr = t;
-	pr_debug("***** Command(%s) started *****\n",
-		 acpi_ec_cmd_string(t->command));
+	ec_dbg_req("Command(%s) started", acpi_ec_cmd_string(t->command));
 	start_transaction(ec);
 	spin_unlock_irqrestore(&ec->lock, tmp);
 	ret = ec_poll(ec);
 	spin_lock_irqsave(&ec->lock, tmp);
 	if (t->irq_count == ec_storm_threshold)
 		acpi_ec_clear_storm(ec, EC_FLAGS_COMMAND_STORM);
-	pr_debug("***** Command(%s) stopped *****\n",
-		 acpi_ec_cmd_string(t->command));
+	ec_dbg_req("Command(%s) stopped", acpi_ec_cmd_string(t->command));
 	ec->curr = NULL;
 	/* Disable GPE for command processing (IBF=0/OBF=1) */
 	acpi_ec_complete_request(ec);
+	ec_dbg_ref(ec, "Decrease command");
 unlock:
 	spin_unlock_irqrestore(&ec->lock, tmp);
 	return ret;
@@ -676,11 +720,13 @@ static void acpi_ec_start(struct acpi_ec *ec, bool resuming)
 
 	spin_lock_irqsave(&ec->lock, flags);
 	if (!test_and_set_bit(EC_FLAGS_STARTED, &ec->flags)) {
-		pr_debug("+++++ Starting EC +++++\n");
+		ec_dbg_drv("Starting EC");
 		/* Enable GPE for event processing (SCI_EVT=1) */
-		if (!resuming)
+		if (!resuming) {
 			acpi_ec_submit_request(ec);
-		pr_debug("EC started\n");
+			ec_dbg_ref(ec, "Increase driver");
+		}
+		ec_log_drv("EC started");
 	}
 	spin_unlock_irqrestore(&ec->lock, flags);
 }
@@ -702,17 +748,19 @@ static void acpi_ec_stop(struct acpi_ec *ec, bool suspending)
 
 	spin_lock_irqsave(&ec->lock, flags);
 	if (acpi_ec_started(ec)) {
-		pr_debug("+++++ Stopping EC +++++\n");
+		ec_dbg_drv("Stopping EC");
 		set_bit(EC_FLAGS_STOPPED, &ec->flags);
 		spin_unlock_irqrestore(&ec->lock, flags);
 		wait_event(ec->wait, acpi_ec_stopped(ec));
 		spin_lock_irqsave(&ec->lock, flags);
 		/* Disable GPE for event processing (SCI_EVT=1) */
-		if (!suspending)
+		if (!suspending) {
 			acpi_ec_complete_request(ec);
+			ec_dbg_ref(ec, "Decrease driver");
+		}
 		clear_bit(EC_FLAGS_STARTED, &ec->flags);
 		clear_bit(EC_FLAGS_STOPPED, &ec->flags);
-		pr_debug("EC stopped\n");
+		ec_log_drv("EC stopped");
 	}
 	spin_unlock_irqrestore(&ec->lock, flags);
 }
@@ -824,12 +872,12 @@ static void acpi_ec_run(void *cxt)
 
 	if (!handler)
 		return;
-	pr_debug("##### Query(0x%02x) started #####\n", handler->query_bit);
+	ec_dbg_evt("Query(0x%02x) started", handler->query_bit);
 	if (handler->func)
 		handler->func(handler->data);
 	else if (handler->handle)
 		acpi_evaluate_object(handler->handle, NULL, NULL, NULL);
-	pr_debug("##### Query(0x%02x) stopped #####\n", handler->query_bit);
+	ec_dbg_evt("Query(0x%02x) stopped", handler->query_bit);
 	acpi_ec_put_query_handler(handler);
 }
 
@@ -861,8 +909,8 @@ static int acpi_ec_query(struct acpi_ec *ec, u8 *data)
 		if (value == handler->query_bit) {
 			/* have custom handler for this bit */
 			handler = acpi_ec_get_query_handler(handler);
-			pr_debug("##### Query(0x%02x) scheduled #####\n",
-				 handler->query_bit);
+			ec_dbg_evt("Query(0x%02x) scheduled",
+				   handler->query_bit);
 			status = acpi_os_execute((handler->func) ?
 				OSL_NOTIFY_HANDLER : OSL_GPE_HANDLER,
 				acpi_ec_run, handler);
@@ -1099,6 +1147,9 @@ static int acpi_ec_add(struct acpi_device *device)
 
 	ret = ec_install_handlers(ec);
 
+	/* Reprobe devices depending on the EC */
+	acpi_walk_dep_device_list(ec->handle);
+
 	/* EC is fully operational, allow queries */
 	clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags);
 
diff --git a/drivers/acpi/glue.c b/drivers/acpi/glue.c
index f774c65ecb8b..39c485b0c25c 100644
--- a/drivers/acpi/glue.c
+++ b/drivers/acpi/glue.c
@@ -168,7 +168,7 @@ int acpi_bind_one(struct device *dev, struct acpi_device *acpi_dev)
 	unsigned int node_id;
 	int retval = -EINVAL;
 
-	if (ACPI_COMPANION(dev)) {
+	if (has_acpi_companion(dev)) {
 		if (acpi_dev) {
 			dev_warn(dev, "ACPI companion already set\n");
 			return -EINVAL;
@@ -220,7 +220,7 @@ int acpi_bind_one(struct device *dev, struct acpi_device *acpi_dev)
 	list_add(&physical_node->node, physnode_list);
 	acpi_dev->physical_node_count++;
 
-	if (!ACPI_COMPANION(dev))
+	if (!has_acpi_companion(dev))
 		ACPI_COMPANION_SET(dev, acpi_dev);
 
 	acpi_physnode_link_name(physical_node_name, node_id);
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/pmic/intel_pmic_crc.c b/drivers/acpi/pmic/intel_pmic_crc.c
index ef7d8ff95abe..42df46a86c25 100644
--- a/drivers/acpi/pmic/intel_pmic_crc.c
+++ b/drivers/acpi/pmic/intel_pmic_crc.c
@@ -207,5 +207,5 @@ static int __init intel_crc_pmic_opregion_driver_init(void)
 }
 module_init(intel_crc_pmic_opregion_driver_init);
 
-MODULE_DESCRIPTION("CrystalCove ACPI opration region driver");
+MODULE_DESCRIPTION("CrystalCove ACPI operation region driver");
 MODULE_LICENSE("GPL");
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/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/scan.c b/drivers/acpi/scan.c
index bbca7830e18a..69bc0d888c01 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -114,7 +114,12 @@ int acpi_scan_add_handler_with_hotplug(struct acpi_scan_handler *handler,
 	return 0;
 }
 
-/*
+/**
+ * create_pnp_modalias - Create hid/cid(s) string for modalias and uevent
+ * @acpi_dev: ACPI device object.
+ * @modalias: Buffer to print into.
+ * @size: Size of the buffer.
+ *
  * Creates hid/cid(s) string needed for modalias and uevent
  * e.g. on a device with hid:IBM0001 and cid:ACPI0001 you get:
  * char *modalias: "acpi:IBM0001:ACPI0001"
@@ -122,68 +127,98 @@ int acpi_scan_add_handler_with_hotplug(struct acpi_scan_handler *handler,
  *         -EINVAL: output error
  *         -ENOMEM: output is truncated
 */
-static int create_modalias(struct acpi_device *acpi_dev, char *modalias,
-			   int size)
+static int create_pnp_modalias(struct acpi_device *acpi_dev, char *modalias,
+			       int size)
 {
 	int len;
 	int count;
 	struct acpi_hardware_id *id;
 
-	if (list_empty(&acpi_dev->pnp.ids))
-		return 0;
-
 	/*
-	 * If the device has PRP0001 we expose DT compatible modalias
-	 * instead in form of of:NnameTCcompatible.
+	 * Since we skip PRP0001 from the modalias below, 0 should be returned
+	 * if PRP0001 is the only ACPI/PNP ID in the device's list.
 	 */
-	if (acpi_dev->data.of_compatible) {
-		struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER };
-		const union acpi_object *of_compatible, *obj;
-		int i, nval;
-		char *c;
-
-		acpi_get_name(acpi_dev->handle, ACPI_SINGLE_NAME, &buf);
-		/* DT strings are all in lower case */
-		for (c = buf.pointer; *c != '\0'; c++)
-			*c = tolower(*c);
-
-		len = snprintf(modalias, size, "of:N%sT", (char *)buf.pointer);
-		ACPI_FREE(buf.pointer);
-
-		of_compatible = acpi_dev->data.of_compatible;
-		if (of_compatible->type == ACPI_TYPE_PACKAGE) {
-			nval = of_compatible->package.count;
-			obj = of_compatible->package.elements;
-		} else { /* Must be ACPI_TYPE_STRING. */
-			nval = 1;
-			obj = of_compatible;
-		}
-		for (i = 0; i < nval; i++, obj++) {
-			count = snprintf(&modalias[len], size, "C%s",
-					 obj->string.pointer);
-			if (count < 0)
-				return -EINVAL;
-			if (count >= size)
-				return -ENOMEM;
-
-			len += count;
-			size -= count;
-		}
-	} else {
-		len = snprintf(modalias, size, "acpi:");
-		size -= len;
+	count = 0;
+	list_for_each_entry(id, &acpi_dev->pnp.ids, list)
+		if (strcmp(id->id, "PRP0001"))
+			count++;
 
-		list_for_each_entry(id, &acpi_dev->pnp.ids, list) {
-			count = snprintf(&modalias[len], size, "%s:", id->id);
-			if (count < 0)
-				return -EINVAL;
-			if (count >= size)
-				return -ENOMEM;
-			len += count;
-			size -= count;
-		}
+	if (!count)
+		return 0;
+
+	len = snprintf(modalias, size, "acpi:");
+	if (len <= 0)
+		return len;
+
+	size -= len;
+
+	list_for_each_entry(id, &acpi_dev->pnp.ids, list) {
+		if (!strcmp(id->id, "PRP0001"))
+			continue;
+
+		count = snprintf(&modalias[len], size, "%s:", id->id);
+		if (count < 0)
+			return -EINVAL;
+
+		if (count >= size)
+			return -ENOMEM;
+
+		len += count;
+		size -= count;
+	}
+	modalias[len] = '\0';
+	return len;
+}
+
+/**
+ * create_of_modalias - Creates DT compatible string for modalias and uevent
+ * @acpi_dev: ACPI device object.
+ * @modalias: Buffer to print into.
+ * @size: Size of the buffer.
+ *
+ * Expose DT compatible modalias as of:NnameTCcompatible.  This function should
+ * only be called for devices having PRP0001 in their list of ACPI/PNP IDs.
+ */
+static int create_of_modalias(struct acpi_device *acpi_dev, char *modalias,
+			      int size)
+{
+	struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER };
+	const union acpi_object *of_compatible, *obj;
+	int len, count;
+	int i, nval;
+	char *c;
+
+	acpi_get_name(acpi_dev->handle, ACPI_SINGLE_NAME, &buf);
+	/* DT strings are all in lower case */
+	for (c = buf.pointer; *c != '\0'; c++)
+		*c = tolower(*c);
+
+	len = snprintf(modalias, size, "of:N%sT", (char *)buf.pointer);
+	ACPI_FREE(buf.pointer);
+
+	if (len <= 0)
+		return len;
+
+	of_compatible = acpi_dev->data.of_compatible;
+	if (of_compatible->type == ACPI_TYPE_PACKAGE) {
+		nval = of_compatible->package.count;
+		obj = of_compatible->package.elements;
+	} else { /* Must be ACPI_TYPE_STRING. */
+		nval = 1;
+		obj = of_compatible;
 	}
+	for (i = 0; i < nval; i++, obj++) {
+		count = snprintf(&modalias[len], size, "C%s",
+				 obj->string.pointer);
+		if (count < 0)
+			return -EINVAL;
 
+		if (count >= size)
+			return -ENOMEM;
+
+		len += count;
+		size -= count;
+	}
 	modalias[len] = '\0';
 	return len;
 }
@@ -194,7 +229,8 @@ static int create_modalias(struct acpi_device *acpi_dev, char *modalias,
  *
  * Check if the given device has an ACPI companion and if that companion has
  * a valid list of PNP IDs, and if the device is the first (primary) physical
- * device associated with it.
+ * device associated with it.  Return the companion pointer if that's the case
+ * or NULL otherwise.
  *
  * If multiple physical devices are attached to a single ACPI companion, we need
  * to be careful.  The usage scenario for this kind of relationship is that all
@@ -208,88 +244,129 @@ static int create_modalias(struct acpi_device *acpi_dev, char *modalias,
  * resources available from it but they will be matched normally using functions
  * provided by their bus types (and analogously for their modalias).
  */
-static bool acpi_companion_match(const struct device *dev)
+static struct acpi_device *acpi_companion_match(const struct device *dev)
 {
 	struct acpi_device *adev;
-	bool ret;
+	struct mutex *physical_node_lock;
 
 	adev = ACPI_COMPANION(dev);
 	if (!adev)
-		return false;
+		return NULL;
 
 	if (list_empty(&adev->pnp.ids))
-		return false;
+		return NULL;
 
-	mutex_lock(&adev->physical_node_lock);
+	physical_node_lock = &adev->physical_node_lock;
+	mutex_lock(physical_node_lock);
 	if (list_empty(&adev->physical_node_list)) {
-		ret = false;
+		adev = NULL;
 	} else {
 		const struct acpi_device_physical_node *node;
 
 		node = list_first_entry(&adev->physical_node_list,
 					struct acpi_device_physical_node, node);
-		ret = node->dev == dev;
+		if (node->dev != dev)
+			adev = NULL;
 	}
-	mutex_unlock(&adev->physical_node_lock);
+	mutex_unlock(physical_node_lock);
 
-	return ret;
+	return adev;
 }
 
-/*
- * Creates uevent modalias field for ACPI enumerated devices.
- * Because the other buses does not support ACPI HIDs & CIDs.
- * e.g. for a device with hid:IBM0001 and cid:ACPI0001 you get:
- * "acpi:IBM0001:ACPI0001"
- */
-int acpi_device_uevent_modalias(struct device *dev, struct kobj_uevent_env *env)
+static int __acpi_device_uevent_modalias(struct acpi_device *adev,
+					 struct kobj_uevent_env *env)
 {
 	int len;
 
-	if (!acpi_companion_match(dev))
+	if (!adev)
 		return -ENODEV;
 
+	if (list_empty(&adev->pnp.ids))
+		return 0;
+
 	if (add_uevent_var(env, "MODALIAS="))
 		return -ENOMEM;
-	len = create_modalias(ACPI_COMPANION(dev), &env->buf[env->buflen - 1],
-				sizeof(env->buf) - env->buflen);
-	if (len <= 0)
+
+	len = create_pnp_modalias(adev, &env->buf[env->buflen - 1],
+				  sizeof(env->buf) - env->buflen);
+	if (len < 0)
+		return len;
+
+	env->buflen += len;
+	if (!adev->data.of_compatible)
+		return 0;
+
+	if (len > 0 && add_uevent_var(env, "MODALIAS="))
+		return -ENOMEM;
+
+	len = create_of_modalias(adev, &env->buf[env->buflen - 1],
+				 sizeof(env->buf) - env->buflen);
+	if (len < 0)
 		return len;
+
 	env->buflen += len;
+
 	return 0;
 }
-EXPORT_SYMBOL_GPL(acpi_device_uevent_modalias);
 
 /*
- * Creates modalias sysfs attribute for ACPI enumerated devices.
+ * Creates uevent modalias field for ACPI enumerated devices.
  * Because the other buses does not support ACPI HIDs & CIDs.
  * e.g. for a device with hid:IBM0001 and cid:ACPI0001 you get:
  * "acpi:IBM0001:ACPI0001"
  */
-int acpi_device_modalias(struct device *dev, char *buf, int size)
+int acpi_device_uevent_modalias(struct device *dev, struct kobj_uevent_env *env)
 {
-	int len;
+	return __acpi_device_uevent_modalias(acpi_companion_match(dev), env);
+}
+EXPORT_SYMBOL_GPL(acpi_device_uevent_modalias);
 
-	if (!acpi_companion_match(dev))
+static int __acpi_device_modalias(struct acpi_device *adev, char *buf, int size)
+{
+	int len, count;
+
+	if (!adev)
 		return -ENODEV;
 
-	len = create_modalias(ACPI_COMPANION(dev), buf, size -1);
-	if (len <= 0)
+	if (list_empty(&adev->pnp.ids))
+		return 0;
+
+	len = create_pnp_modalias(adev, buf, size - 1);
+	if (len < 0) {
+		return len;
+	} else if (len > 0) {
+		buf[len++] = '\n';
+		size -= len;
+	}
+	if (!adev->data.of_compatible)
 		return len;
-	buf[len++] = '\n';
+
+	count = create_of_modalias(adev, buf + len, size - 1);
+	if (count < 0) {
+		return count;
+	} else if (count > 0) {
+		len += count;
+		buf[len++] = '\n';
+	}
+
 	return len;
 }
+
+/*
+ * Creates modalias sysfs attribute for ACPI enumerated devices.
+ * Because the other buses does not support ACPI HIDs & CIDs.
+ * e.g. for a device with hid:IBM0001 and cid:ACPI0001 you get:
+ * "acpi:IBM0001:ACPI0001"
+ */
+int acpi_device_modalias(struct device *dev, char *buf, int size)
+{
+	return __acpi_device_modalias(acpi_companion_match(dev), buf, size);
+}
 EXPORT_SYMBOL_GPL(acpi_device_modalias);
 
 static ssize_t
 acpi_device_modalias_show(struct device *dev, struct device_attribute *attr, char *buf) {
-	struct acpi_device *acpi_dev = to_acpi_device(dev);
-	int len;
-
-	len = create_modalias(acpi_dev, buf, 1024);
-	if (len <= 0)
-		return len;
-	buf[len++] = '\n';
-	return len;
+	return __acpi_device_modalias(to_acpi_device(dev), buf, 1024);
 }
 static DEVICE_ATTR(modalias, 0444, acpi_device_modalias_show, NULL);
 
@@ -894,8 +971,51 @@ static void acpi_device_remove_files(struct acpi_device *dev)
 			ACPI Bus operations
    -------------------------------------------------------------------------- */
 
+/**
+ * acpi_of_match_device - Match device object using the "compatible" property.
+ * @adev: ACPI device object to match.
+ * @of_match_table: List of device IDs to match against.
+ *
+ * If @dev has an ACPI companion which has the special PRP0001 device ID in its
+ * list of identifiers and a _DSD object with the "compatible" property, use
+ * that property to match against the given list of identifiers.
+ */
+static bool acpi_of_match_device(struct acpi_device *adev,
+				 const struct of_device_id *of_match_table)
+{
+	const union acpi_object *of_compatible, *obj;
+	int i, nval;
+
+	if (!adev)
+		return false;
+
+	of_compatible = adev->data.of_compatible;
+	if (!of_match_table || !of_compatible)
+		return false;
+
+	if (of_compatible->type == ACPI_TYPE_PACKAGE) {
+		nval = of_compatible->package.count;
+		obj = of_compatible->package.elements;
+	} else { /* Must be ACPI_TYPE_STRING. */
+		nval = 1;
+		obj = of_compatible;
+	}
+	/* Now we can look for the driver DT compatible strings */
+	for (i = 0; i < nval; i++, obj++) {
+		const struct of_device_id *id;
+
+		for (id = of_match_table; id->compatible[0]; id++)
+			if (!strcasecmp(obj->string.pointer, id->compatible))
+				return true;
+	}
+
+	return false;
+}
+
 static const struct acpi_device_id *__acpi_match_device(
-	struct acpi_device *device, const struct acpi_device_id *ids)
+	struct acpi_device *device,
+	const struct acpi_device_id *ids,
+	const struct of_device_id *of_ids)
 {
 	const struct acpi_device_id *id;
 	struct acpi_hardware_id *hwid;
@@ -904,14 +1024,27 @@ static const struct acpi_device_id *__acpi_match_device(
 	 * If the device is not present, it is unnecessary to load device
 	 * driver for it.
 	 */
-	if (!device->status.present)
+	if (!device || !device->status.present)
 		return NULL;
 
-	for (id = ids; id->id[0]; id++)
-		list_for_each_entry(hwid, &device->pnp.ids, list)
+	list_for_each_entry(hwid, &device->pnp.ids, list) {
+		/* First, check the ACPI/PNP IDs provided by the caller. */
+		for (id = ids; id->id[0]; id++)
 			if (!strcmp((char *) id->id, hwid->id))
 				return id;
 
+		/*
+		 * Next, check the special "PRP0001" ID and try to match the
+		 * "compatible" property if found.
+		 *
+		 * The id returned by the below is not valid, but the only
+		 * caller passing non-NULL of_ids here is only interested in
+		 * whether or not the return value is NULL.
+		 */
+		if (!strcmp("PRP0001", hwid->id)
+		    && acpi_of_match_device(device, of_ids))
+			return id;
+	}
 	return NULL;
 }
 
@@ -929,68 +1062,26 @@ static const struct acpi_device_id *__acpi_match_device(
 const struct acpi_device_id *acpi_match_device(const struct acpi_device_id *ids,
 					       const struct device *dev)
 {
-	struct acpi_device *adev;
-	acpi_handle handle = ACPI_HANDLE(dev);
-
-	if (!ids || !handle || acpi_bus_get_device(handle, &adev))
-		return NULL;
-
-	if (!acpi_companion_match(dev))
-		return NULL;
-
-	return __acpi_match_device(adev, ids);
+	return __acpi_match_device(acpi_companion_match(dev), ids, NULL);
 }
 EXPORT_SYMBOL_GPL(acpi_match_device);
 
 int acpi_match_device_ids(struct acpi_device *device,
 			  const struct acpi_device_id *ids)
 {
-	return __acpi_match_device(device, ids) ? 0 : -ENOENT;
+	return __acpi_match_device(device, ids, NULL) ? 0 : -ENOENT;
 }
 EXPORT_SYMBOL(acpi_match_device_ids);
 
-/* Performs match against special "PRP0001" shoehorn ACPI ID */
-static bool acpi_of_driver_match_device(struct device *dev,
-					const struct device_driver *drv)
-{
-	const union acpi_object *of_compatible, *obj;
-	struct acpi_device *adev;
-	int i, nval;
-
-	adev = ACPI_COMPANION(dev);
-	if (!adev)
-		return false;
-
-	of_compatible = adev->data.of_compatible;
-	if (!drv->of_match_table || !of_compatible)
-		return false;
-
-	if (of_compatible->type == ACPI_TYPE_PACKAGE) {
-		nval = of_compatible->package.count;
-		obj = of_compatible->package.elements;
-	} else { /* Must be ACPI_TYPE_STRING. */
-		nval = 1;
-		obj = of_compatible;
-	}
-	/* Now we can look for the driver DT compatible strings */
-	for (i = 0; i < nval; i++, obj++) {
-		const struct of_device_id *id;
-
-		for (id = drv->of_match_table; id->compatible[0]; id++)
-			if (!strcasecmp(obj->string.pointer, id->compatible))
-				return true;
-	}
-
-	return false;
-}
-
 bool acpi_driver_match_device(struct device *dev,
 			      const struct device_driver *drv)
 {
 	if (!drv->acpi_match_table)
-		return acpi_of_driver_match_device(dev, drv);
+		return acpi_of_match_device(ACPI_COMPANION(dev),
+					    drv->of_match_table);
 
-	return !!acpi_match_device(drv->acpi_match_table, dev);
+	return !!__acpi_match_device(acpi_companion_match(dev),
+				     drv->acpi_match_table, drv->of_match_table);
 }
 EXPORT_SYMBOL_GPL(acpi_driver_match_device);
 
@@ -1031,20 +1122,7 @@ static int acpi_bus_match(struct device *dev, struct device_driver *drv)
 
 static int acpi_device_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
-	struct acpi_device *acpi_dev = to_acpi_device(dev);
-	int len;
-
-	if (list_empty(&acpi_dev->pnp.ids))
-		return 0;
-
-	if (add_uevent_var(env, "MODALIAS="))
-		return -ENOMEM;
-	len = create_modalias(acpi_dev, &env->buf[env->buflen - 1],
-			      sizeof(env->buf) - env->buflen);
-	if (len <= 0)
-		return len;
-	env->buflen += len;
-	return 0;
+	return __acpi_device_uevent_modalias(to_acpi_device(dev), env);
 }
 
 static void acpi_device_notify(acpi_handle handle, u32 event, void *data)
@@ -1062,10 +1140,10 @@ static void acpi_device_notify_fixed(void *data)
 	acpi_device_notify(NULL, ACPI_FIXED_HARDWARE_EVENT, device);
 }
 
-static acpi_status acpi_device_fixed_event(void *data)
+static u32 acpi_device_fixed_event(void *data)
 {
 	acpi_os_execute(OSL_NOTIFY_HANDLER, acpi_device_notify_fixed, data);
-	return AE_OK;
+	return ACPI_INTERRUPT_HANDLED;
 }
 
 static int acpi_device_install_notify_handler(struct acpi_device *device)
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 7f251dd1a687..2f0d4db40a9e 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -629,6 +629,7 @@ static int acpi_freeze_begin(void)
 
 static int acpi_freeze_prepare(void)
 {
+	acpi_enable_wakeup_devices(ACPI_STATE_S0);
 	acpi_enable_all_wakeup_gpes();
 	acpi_os_wait_events_complete();
 	enable_irq_wake(acpi_gbl_FADT.sci_interrupt);
@@ -637,6 +638,7 @@ static int acpi_freeze_prepare(void)
 
 static void acpi_freeze_restore(void)
 {
+	acpi_disable_wakeup_devices(ACPI_STATE_S0);
 	disable_irq_wake(acpi_gbl_FADT.sci_interrupt);
 	acpi_enable_all_runtime_gpes();
 }
@@ -806,21 +808,6 @@ static void acpi_sleep_hibernate_setup(void)
 static inline void acpi_sleep_hibernate_setup(void) {}
 #endif /* !CONFIG_HIBERNATION */
 
-int acpi_suspend(u32 acpi_state)
-{
-	suspend_state_t states[] = {
-		[1] = PM_SUSPEND_STANDBY,
-		[3] = PM_SUSPEND_MEM,
-		[5] = PM_SUSPEND_MAX
-	};
-
-	if (acpi_state < 6 && states[acpi_state])
-		return pm_suspend(states[acpi_state]);
-	if (acpi_state == 4)
-		return hibernate();
-	return -EINVAL;
-}
-
 static void acpi_power_off_prepare(void)
 {
 	/* Prepare to power off the system */
diff --git a/drivers/acpi/sleep.h b/drivers/acpi/sleep.h
index 0143540a2519..c797ffa568d5 100644
--- a/drivers/acpi/sleep.h
+++ b/drivers/acpi/sleep.h
@@ -1,6 +1,4 @@
 
-extern int acpi_suspend(u32 state);
-
 extern void acpi_enable_wakeup_devices(u8 sleep_state);
 extern void acpi_disable_wakeup_devices(u8 sleep_state);
 
diff --git a/drivers/acpi/sysfs.c b/drivers/acpi/sysfs.c
index 13e577c80201..0876d77b3206 100644
--- a/drivers/acpi/sysfs.c
+++ b/drivers/acpi/sysfs.c
@@ -527,7 +527,7 @@ static ssize_t counter_show(struct kobject *kobj,
 	    acpi_irq_not_handled;
 	all_counters[num_gpes + ACPI_NUM_FIXED_EVENTS + COUNT_GPE].count =
 	    acpi_gpe_count;
-	size = sprintf(buf, "%8d", all_counters[index].count);
+	size = sprintf(buf, "%8u", all_counters[index].count);
 
 	/* "gpe_all" or "sci" */
 	if (index >= num_gpes + ACPI_NUM_FIXED_EVENTS)
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index 26eb70c8f518..cc79d3fedfb2 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -82,9 +82,15 @@ module_param(allow_duplicates, bool, 0644);
  * For Windows 8 systems: used to decide if video module
  * should skip registering backlight interface of its own.
  */
-static int use_native_backlight_param = -1;
+enum {
+	NATIVE_BACKLIGHT_NOT_SET = -1,
+	NATIVE_BACKLIGHT_OFF,
+	NATIVE_BACKLIGHT_ON,
+};
+
+static int use_native_backlight_param = NATIVE_BACKLIGHT_NOT_SET;
 module_param_named(use_native_backlight, use_native_backlight_param, int, 0444);
-static bool use_native_backlight_dmi = true;
+static int use_native_backlight_dmi = NATIVE_BACKLIGHT_NOT_SET;
 
 static int register_count;
 static struct mutex video_list_lock;
@@ -237,15 +243,16 @@ static void acpi_video_switch_brightness(struct work_struct *work);
 
 static bool acpi_video_use_native_backlight(void)
 {
-	if (use_native_backlight_param != -1)
+	if (use_native_backlight_param != NATIVE_BACKLIGHT_NOT_SET)
 		return use_native_backlight_param;
-	else
+	else if (use_native_backlight_dmi != NATIVE_BACKLIGHT_NOT_SET)
 		return use_native_backlight_dmi;
+	return acpi_osi_is_win8();
 }
 
 bool acpi_video_verify_backlight_support(void)
 {
-	if (acpi_osi_is_win8() && acpi_video_use_native_backlight() &&
+	if (acpi_video_use_native_backlight() &&
 	    backlight_device_registered(BACKLIGHT_RAW))
 		return false;
 	return acpi_video_backlight_support();
@@ -414,7 +421,13 @@ static int __init video_set_bqc_offset(const struct dmi_system_id *d)
 
 static int __init video_disable_native_backlight(const struct dmi_system_id *d)
 {
-	use_native_backlight_dmi = false;
+	use_native_backlight_dmi = NATIVE_BACKLIGHT_OFF;
+	return 0;
+}
+
+static int __init video_enable_native_backlight(const struct dmi_system_id *d)
+{
+	use_native_backlight_dmi = NATIVE_BACKLIGHT_ON;
 	return 0;
 }
 
@@ -559,6 +572,17 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "XPS L521X"),
 		},
 	},
+
+	/* Non win8 machines which need native backlight nevertheless */
+	{
+	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1187004 */
+	 .callback = video_enable_native_backlight,
+	 .ident = "Lenovo Ideapad Z570",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "102434U"),
+		},
+	},
 	{}
 };
 
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index 27c43499977a..c42feb2bacd0 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -174,14 +174,6 @@ static struct dmi_system_id video_detect_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 5737"),
 		},
 	},
-	{
-	.callback = video_detect_force_vendor,
-	.ident = "Lenovo IdeaPad Z570",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "Ideapad Z570"),
-		},
-	},
 	{ },
 };
 
diff --git a/drivers/amba/tegra-ahb.c b/drivers/amba/tegra-ahb.c
index c6dc3548e5d1..b0b688c481e8 100644
--- a/drivers/amba/tegra-ahb.c
+++ b/drivers/amba/tegra-ahb.c
@@ -25,49 +25,50 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
+#include <linux/of.h>
 
 #include <soc/tegra/ahb.h>
 
 #define DRV_NAME "tegra-ahb"
 
-#define AHB_ARBITRATION_DISABLE		0x00
-#define AHB_ARBITRATION_PRIORITY_CTRL	0x04
+#define AHB_ARBITRATION_DISABLE		0x04
+#define AHB_ARBITRATION_PRIORITY_CTRL	0x08
 #define   AHB_PRIORITY_WEIGHT(x)	(((x) & 0x7) << 29)
 #define   PRIORITY_SELECT_USB BIT(6)
 #define   PRIORITY_SELECT_USB2 BIT(18)
 #define   PRIORITY_SELECT_USB3 BIT(17)
 
-#define AHB_GIZMO_AHB_MEM		0x0c
+#define AHB_GIZMO_AHB_MEM		0x10
 #define   ENB_FAST_REARBITRATE BIT(2)
 #define   DONT_SPLIT_AHB_WR     BIT(7)
 
-#define AHB_GIZMO_APB_DMA		0x10
-#define AHB_GIZMO_IDE			0x18
-#define AHB_GIZMO_USB			0x1c
-#define AHB_GIZMO_AHB_XBAR_BRIDGE	0x20
-#define AHB_GIZMO_CPU_AHB_BRIDGE	0x24
-#define AHB_GIZMO_COP_AHB_BRIDGE	0x28
-#define AHB_GIZMO_XBAR_APB_CTLR		0x2c
-#define AHB_GIZMO_VCP_AHB_BRIDGE	0x30
-#define AHB_GIZMO_NAND			0x3c
-#define AHB_GIZMO_SDMMC4		0x44
-#define AHB_GIZMO_XIO			0x48
-#define AHB_GIZMO_BSEV			0x60
-#define AHB_GIZMO_BSEA			0x70
-#define AHB_GIZMO_NOR			0x74
-#define AHB_GIZMO_USB2			0x78
-#define AHB_GIZMO_USB3			0x7c
+#define AHB_GIZMO_APB_DMA		0x14
+#define AHB_GIZMO_IDE			0x1c
+#define AHB_GIZMO_USB			0x20
+#define AHB_GIZMO_AHB_XBAR_BRIDGE	0x24
+#define AHB_GIZMO_CPU_AHB_BRIDGE	0x28
+#define AHB_GIZMO_COP_AHB_BRIDGE	0x2c
+#define AHB_GIZMO_XBAR_APB_CTLR		0x30
+#define AHB_GIZMO_VCP_AHB_BRIDGE	0x34
+#define AHB_GIZMO_NAND			0x40
+#define AHB_GIZMO_SDMMC4		0x48
+#define AHB_GIZMO_XIO			0x4c
+#define AHB_GIZMO_BSEV			0x64
+#define AHB_GIZMO_BSEA			0x74
+#define AHB_GIZMO_NOR			0x78
+#define AHB_GIZMO_USB2			0x7c
+#define AHB_GIZMO_USB3			0x80
 #define   IMMEDIATE	BIT(18)
 
-#define AHB_GIZMO_SDMMC1		0x80
-#define AHB_GIZMO_SDMMC2		0x84
-#define AHB_GIZMO_SDMMC3		0x88
-#define AHB_MEM_PREFETCH_CFG_X		0xd8
-#define AHB_ARBITRATION_XBAR_CTRL	0xdc
-#define AHB_MEM_PREFETCH_CFG3		0xe0
-#define AHB_MEM_PREFETCH_CFG4		0xe4
-#define AHB_MEM_PREFETCH_CFG1		0xec
-#define AHB_MEM_PREFETCH_CFG2		0xf0
+#define AHB_GIZMO_SDMMC1		0x84
+#define AHB_GIZMO_SDMMC2		0x88
+#define AHB_GIZMO_SDMMC3		0x8c
+#define AHB_MEM_PREFETCH_CFG_X		0xdc
+#define AHB_ARBITRATION_XBAR_CTRL	0xe0
+#define AHB_MEM_PREFETCH_CFG3		0xe4
+#define AHB_MEM_PREFETCH_CFG4		0xe8
+#define AHB_MEM_PREFETCH_CFG1		0xf0
+#define AHB_MEM_PREFETCH_CFG2		0xf4
 #define   PREFETCH_ENB	BIT(31)
 #define   MST_ID(x)	(((x) & 0x1f) << 26)
 #define   AHBDMA_MST_ID	MST_ID(5)
@@ -77,10 +78,20 @@
 #define   ADDR_BNDRY(x)	(((x) & 0xf) << 21)
 #define   INACTIVITY_TIMEOUT(x)	(((x) & 0xffff) << 0)
 
-#define AHB_ARBITRATION_AHB_MEM_WRQUE_MST_ID	0xf8
+#define AHB_ARBITRATION_AHB_MEM_WRQUE_MST_ID	0xfc
 
 #define AHB_ARBITRATION_XBAR_CTRL_SMMU_INIT_DONE BIT(17)
 
+/*
+ * INCORRECT_BASE_ADDR_LOW_BYTE: Legacy kernel DT files for Tegra SoCs
+ * prior to Tegra124 generally use a physical base address ending in
+ * 0x4 for the AHB IP block.  According to the TRM, the low byte
+ * should be 0x0.  During device probing, this macro is used to detect
+ * whether the passed-in physical address is incorrect, and if so, to
+ * correct it.
+ */
+#define INCORRECT_BASE_ADDR_LOW_BYTE		0x4
+
 static struct platform_driver tegra_ahb_driver;
 
 static const u32 tegra_ahb_gizmo[] = {
@@ -257,6 +268,15 @@ static int tegra_ahb_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	/* Correct the IP block base address if necessary */
+	if (res &&
+	    (res->start & INCORRECT_BASE_ADDR_LOW_BYTE) ==
+	    INCORRECT_BASE_ADDR_LOW_BYTE) {
+		dev_warn(&pdev->dev, "incorrect AHB base address in DT data - enabling workaround\n");
+		res->start -= INCORRECT_BASE_ADDR_LOW_BYTE;
+	}
+
 	ahb->regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(ahb->regs))
 		return PTR_ERR(ahb->regs);
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_isapnp.c b/drivers/ata/pata_isapnp.c
index b33d1f99b3a4..994f168b54a8 100644
--- a/drivers/ata/pata_isapnp.c
+++ b/drivers/ata/pata_isapnp.c
@@ -128,20 +128,8 @@ static struct pnp_driver isapnp_driver = {
 	.remove		= isapnp_remove_one,
 };
 
-static int __init isapnp_init(void)
-{
-	return pnp_register_driver(&isapnp_driver);
-}
-
-static void __exit isapnp_exit(void)
-{
-	pnp_unregister_driver(&isapnp_driver);
-}
-
+module_pnp_driver(isapnp_driver);
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for ISA PnP ATA");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
-
-module_init(isapnp_init);
-module_exit(isapnp_exit);
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_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 4c06f6281d74..ff614be55d0f 100644
--- a/drivers/ata/sata_svw.c
+++ b/drivers/ata/sata_svw.c
@@ -487,10 +487,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/atm/nicstar.c b/drivers/atm/nicstar.c
index b7e1cc0a97c8..ddc4ceb85fc5 100644
--- a/drivers/atm/nicstar.c
+++ b/drivers/atm/nicstar.c
@@ -73,9 +73,6 @@
 #undef GENERAL_DEBUG
 #undef EXTRA_DEBUG
 
-#undef NS_USE_DESTRUCTORS	/* For now keep this undefined unless you know
-				   you're going to use only raw ATM */
-
 /* Do not touch these */
 
 #ifdef TX_DEBUG
@@ -138,11 +135,6 @@ static void process_tsq(ns_dev * card);
 static void drain_scq(ns_dev * card, scq_info * scq, int pos);
 static void process_rsq(ns_dev * card);
 static void dequeue_rx(ns_dev * card, ns_rsqe * rsqe);
-#ifdef NS_USE_DESTRUCTORS
-static void ns_sb_destructor(struct sk_buff *sb);
-static void ns_lb_destructor(struct sk_buff *lb);
-static void ns_hb_destructor(struct sk_buff *hb);
-#endif /* NS_USE_DESTRUCTORS */
 static void recycle_rx_buf(ns_dev * card, struct sk_buff *skb);
 static void recycle_iovec_rx_bufs(ns_dev * card, struct iovec *iov, int count);
 static void recycle_iov_buf(ns_dev * card, struct sk_buff *iovb);
@@ -2169,9 +2161,6 @@ static void dequeue_rx(ns_dev * card, ns_rsqe * rsqe)
 			} else {
 				skb_put(skb, len);
 				dequeue_sm_buf(card, skb);
-#ifdef NS_USE_DESTRUCTORS
-				skb->destructor = ns_sb_destructor;
-#endif /* NS_USE_DESTRUCTORS */
 				ATM_SKB(skb)->vcc = vcc;
 				__net_timestamp(skb);
 				vcc->push(vcc, skb);
@@ -2190,9 +2179,6 @@ static void dequeue_rx(ns_dev * card, ns_rsqe * rsqe)
 				} else {
 					skb_put(sb, len);
 					dequeue_sm_buf(card, sb);
-#ifdef NS_USE_DESTRUCTORS
-					sb->destructor = ns_sb_destructor;
-#endif /* NS_USE_DESTRUCTORS */
 					ATM_SKB(sb)->vcc = vcc;
 					__net_timestamp(sb);
 					vcc->push(vcc, sb);
@@ -2208,9 +2194,6 @@ static void dequeue_rx(ns_dev * card, ns_rsqe * rsqe)
 					atomic_inc(&vcc->stats->rx_drop);
 				} else {
 					dequeue_lg_buf(card, skb);
-#ifdef NS_USE_DESTRUCTORS
-					skb->destructor = ns_lb_destructor;
-#endif /* NS_USE_DESTRUCTORS */
 					skb_push(skb, NS_SMBUFSIZE);
 					skb_copy_from_linear_data(sb, skb->data,
 								  NS_SMBUFSIZE);
@@ -2322,9 +2305,6 @@ static void dequeue_rx(ns_dev * card, ns_rsqe * rsqe)
 					     card->index);
 #endif /* EXTRA_DEBUG */
 				ATM_SKB(hb)->vcc = vcc;
-#ifdef NS_USE_DESTRUCTORS
-				hb->destructor = ns_hb_destructor;
-#endif /* NS_USE_DESTRUCTORS */
 				__net_timestamp(hb);
 				vcc->push(vcc, hb);
 				atomic_inc(&vcc->stats->rx);
@@ -2337,68 +2317,6 @@ static void dequeue_rx(ns_dev * card, ns_rsqe * rsqe)
 
 }
 
-#ifdef NS_USE_DESTRUCTORS
-
-static void ns_sb_destructor(struct sk_buff *sb)
-{
-	ns_dev *card;
-	u32 stat;
-
-	card = (ns_dev *) ATM_SKB(sb)->vcc->dev->dev_data;
-	stat = readl(card->membase + STAT);
-	card->sbfqc = ns_stat_sfbqc_get(stat);
-	card->lbfqc = ns_stat_lfbqc_get(stat);
-
-	do {
-		sb = __dev_alloc_skb(NS_SMSKBSIZE, GFP_KERNEL);
-		if (sb == NULL)
-			break;
-		NS_PRV_BUFTYPE(sb) = BUF_SM;
-		skb_queue_tail(&card->sbpool.queue, sb);
-		skb_reserve(sb, NS_AAL0_HEADER);
-		push_rxbufs(card, sb);
-	} while (card->sbfqc < card->sbnr.min);
-}
-
-static void ns_lb_destructor(struct sk_buff *lb)
-{
-	ns_dev *card;
-	u32 stat;
-
-	card = (ns_dev *) ATM_SKB(lb)->vcc->dev->dev_data;
-	stat = readl(card->membase + STAT);
-	card->sbfqc = ns_stat_sfbqc_get(stat);
-	card->lbfqc = ns_stat_lfbqc_get(stat);
-
-	do {
-		lb = __dev_alloc_skb(NS_LGSKBSIZE, GFP_KERNEL);
-		if (lb == NULL)
-			break;
-		NS_PRV_BUFTYPE(lb) = BUF_LG;
-		skb_queue_tail(&card->lbpool.queue, lb);
-		skb_reserve(lb, NS_SMBUFSIZE);
-		push_rxbufs(card, lb);
-	} while (card->lbfqc < card->lbnr.min);
-}
-
-static void ns_hb_destructor(struct sk_buff *hb)
-{
-	ns_dev *card;
-
-	card = (ns_dev *) ATM_SKB(hb)->vcc->dev->dev_data;
-
-	while (card->hbpool.count < card->hbnr.init) {
-		hb = __dev_alloc_skb(NS_HBUFSIZE, GFP_KERNEL);
-		if (hb == NULL)
-			break;
-		NS_PRV_BUFTYPE(hb) = BUF_NONE;
-		skb_queue_tail(&card->hbpool.queue, hb);
-		card->hbpool.count++;
-	}
-}
-
-#endif /* NS_USE_DESTRUCTORS */
-
 static void recycle_rx_buf(ns_dev * card, struct sk_buff *skb)
 {
 	if (unlikely(NS_PRV_BUFTYPE(skb) == BUF_NONE)) {
@@ -2427,9 +2345,6 @@ static void recycle_iov_buf(ns_dev * card, struct sk_buff *iovb)
 static void dequeue_sm_buf(ns_dev * card, struct sk_buff *sb)
 {
 	skb_unlink(sb, &card->sbpool.queue);
-#ifdef NS_USE_DESTRUCTORS
-	if (card->sbfqc < card->sbnr.min)
-#else
 	if (card->sbfqc < card->sbnr.init) {
 		struct sk_buff *new_sb;
 		if ((new_sb = dev_alloc_skb(NS_SMSKBSIZE)) != NULL) {
@@ -2440,7 +2355,6 @@ static void dequeue_sm_buf(ns_dev * card, struct sk_buff *sb)
 		}
 	}
 	if (card->sbfqc < card->sbnr.init)
-#endif /* NS_USE_DESTRUCTORS */
 	{
 		struct sk_buff *new_sb;
 		if ((new_sb = dev_alloc_skb(NS_SMSKBSIZE)) != NULL) {
@@ -2455,9 +2369,6 @@ static void dequeue_sm_buf(ns_dev * card, struct sk_buff *sb)
 static void dequeue_lg_buf(ns_dev * card, struct sk_buff *lb)
 {
 	skb_unlink(lb, &card->lbpool.queue);
-#ifdef NS_USE_DESTRUCTORS
-	if (card->lbfqc < card->lbnr.min)
-#else
 	if (card->lbfqc < card->lbnr.init) {
 		struct sk_buff *new_lb;
 		if ((new_lb = dev_alloc_skb(NS_LGSKBSIZE)) != NULL) {
@@ -2468,7 +2379,6 @@ static void dequeue_lg_buf(ns_dev * card, struct sk_buff *lb)
 		}
 	}
 	if (card->lbfqc < card->lbnr.init)
-#endif /* NS_USE_DESTRUCTORS */
 	{
 		struct sk_buff *new_lb;
 		if ((new_lb = dev_alloc_skb(NS_LGSKBSIZE)) != NULL) {
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..21d13038534e 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -12,6 +12,7 @@
 
 #include <linux/device.h>
 #include <linux/err.h>
+#include <linux/fwnode.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -805,8 +806,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 +823,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 +851,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 +1108,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;
@@ -2133,3 +2145,53 @@ define_dev_printk_level(dev_notice, KERN_NOTICE);
 define_dev_printk_level(_dev_info, KERN_INFO);
 
 #endif
+
+static inline bool fwnode_is_primary(struct fwnode_handle *fwnode)
+{
+	return fwnode && !IS_ERR(fwnode->secondary);
+}
+
+/**
+ * set_primary_fwnode - Change the primary firmware node of a given device.
+ * @dev: Device to handle.
+ * @fwnode: New primary firmware node of the device.
+ *
+ * Set the device's firmware node pointer to @fwnode, but if a secondary
+ * firmware node of the device is present, preserve it.
+ */
+void set_primary_fwnode(struct device *dev, struct fwnode_handle *fwnode)
+{
+	if (fwnode) {
+		struct fwnode_handle *fn = dev->fwnode;
+
+		if (fwnode_is_primary(fn))
+			fn = fn->secondary;
+
+		fwnode->secondary = fn;
+		dev->fwnode = fwnode;
+	} else {
+		dev->fwnode = fwnode_is_primary(dev->fwnode) ?
+			dev->fwnode->secondary : NULL;
+	}
+}
+EXPORT_SYMBOL_GPL(set_primary_fwnode);
+
+/**
+ * set_secondary_fwnode - Change the secondary firmware node of a given device.
+ * @dev: Device to handle.
+ * @fwnode: New secondary firmware node of the device.
+ *
+ * If a primary firmware node of the device is present, set its secondary
+ * pointer to @fwnode.  Otherwise, set the device's firmware node pointer to
+ * @fwnode.
+ */
+void set_secondary_fwnode(struct device *dev, struct fwnode_handle *fwnode)
+{
+	if (fwnode)
+		fwnode->secondary = ERR_PTR(-ENODEV);
+
+	if (fwnode_is_primary(dev->fwnode))
+		dev->fwnode->secondary = fwnode;
+	else
+		dev->fwnode = fwnode;
+}
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index cdc779cf79a3..e843fdbe4925 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -298,6 +298,12 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 		goto probe_failed;
 	}
 
+	if (dev->pm_domain && dev->pm_domain->activate) {
+		ret = dev->pm_domain->activate(dev);
+		if (ret)
+			goto probe_failed;
+	}
+
 	if (dev->bus->probe) {
 		ret = dev->bus->probe(dev);
 		if (ret)
@@ -308,6 +314,9 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 			goto probe_failed;
 	}
 
+	if (dev->pm_domain && dev->pm_domain->sync)
+		dev->pm_domain->sync(dev);
+
 	driver_bound(dev);
 	ret = 1;
 	pr_debug("bus: '%s': %s: bound device %s to driver %s\n",
@@ -319,22 +328,28 @@ probe_failed:
 	driver_sysfs_remove(dev);
 	dev->driver = NULL;
 	dev_set_drvdata(dev, NULL);
+	if (dev->pm_domain && dev->pm_domain->dismiss)
+		dev->pm_domain->dismiss(dev);
 
-	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
@@ -525,6 +540,9 @@ static void __device_release_driver(struct device *dev)
 		devres_release_all(dev);
 		dev->driver = NULL;
 		dev_set_drvdata(dev, NULL);
+		if (dev->pm_domain && dev->pm_domain->dismiss)
+			dev->pm_domain->dismiss(dev);
+
 		klist_remove(&dev->p->knode_driver);
 		if (dev->bus)
 			blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
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..2804aed3f416 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;
@@ -219,6 +219,7 @@ static bool pages_correctly_reserved(unsigned long start_pfn)
 /*
  * MEMORY_HOTPLUG depends on SPARSEMEM in mm/Kconfig, so it is
  * OK to have direct references to sparsemem variables in here.
+ * Must already be protected by mem_hotplug_begin().
  */
 static int
 memory_block_action(unsigned long phys_index, unsigned long action, int online_type)
@@ -228,23 +229,23 @@ memory_block_action(unsigned long phys_index, unsigned long action, int online_t
 	struct page *first_page;
 	int ret;
 
-	start_pfn = phys_index << PFN_SECTION_SHIFT;
+	start_pfn = section_nr_to_pfn(phys_index);
 	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;
@@ -286,6 +287,7 @@ static int memory_subsys_online(struct device *dev)
 	if (mem->online_type < 0)
 		mem->online_type = MMOP_ONLINE_KEEP;
 
+	/* Already under protection of mem_hotplug_begin() */
 	ret = memory_block_change_state(mem, MEM_ONLINE, MEM_OFFLINE);
 
 	/* clear online_type */
@@ -328,17 +330,19 @@ store_mem_state(struct device *dev,
 		goto err;
 	}
 
+	/*
+	 * Memory hotplug needs to hold mem_hotplug_begin() for probe to find
+	 * the correct memory block to online before doing device_online(dev),
+	 * which will take dev->mutex.  Take the lock early to prevent an
+	 * inversion, memory_subsys_online() callbacks will be implemented by
+	 * assuming it's already protected.
+	 */
+	mem_hotplug_begin();
+
 	switch (online_type) {
 	case MMOP_ONLINE_KERNEL:
 	case MMOP_ONLINE_MOVABLE:
 	case MMOP_ONLINE_KEEP:
-		/*
-		 * mem->online_type is not protected so there can be a
-		 * race here.  However, when racing online, the first
-		 * will succeed and the second will just return as the
-		 * block will already be online.  The online type
-		 * could be either one, but that is expected.
-		 */
 		mem->online_type = online_type;
 		ret = device_online(&mem->dev);
 		break;
@@ -349,6 +353,7 @@ store_mem_state(struct device *dev,
 		ret = -EINVAL; /* should never happen */
 	}
 
+	mem_hotplug_done();
 err:
 	unlock_device_hotplug();
 
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..ebf034b97278 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
@@ -454,7 +463,7 @@ struct platform_device *platform_device_register_full(
 		goto err_alloc;
 
 	pdev->dev.parent = pdevinfo->parent;
-	ACPI_COMPANION_SET(&pdev->dev, pdevinfo->acpi_node.companion);
+	pdev->dev.fwnode = pdevinfo->fwnode;
 
 	if (pdevinfo->dma_mask) {
 		/*
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index 45937f88e77c..2327613d4539 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -68,7 +68,36 @@ static struct generic_pm_domain *pm_genpd_lookup_name(const char *domain_name)
 	return genpd;
 }
 
-struct generic_pm_domain *dev_to_genpd(struct device *dev)
+/*
+ * Get the generic PM domain for a particular struct device.
+ * This validates the struct device pointer, the PM domain pointer,
+ * and checks that the PM domain pointer is a real generic PM domain.
+ * Any failure results in NULL being returned.
+ */
+struct generic_pm_domain *pm_genpd_lookup_dev(struct device *dev)
+{
+	struct generic_pm_domain *genpd = NULL, *gpd;
+
+	if (IS_ERR_OR_NULL(dev) || IS_ERR_OR_NULL(dev->pm_domain))
+		return NULL;
+
+	mutex_lock(&gpd_list_lock);
+	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
+		if (&gpd->domain == dev->pm_domain) {
+			genpd = gpd;
+			break;
+		}
+	}
+	mutex_unlock(&gpd_list_lock);
+
+	return genpd;
+}
+
+/*
+ * This should only be used where we are certain that the pm_domain
+ * attached to the device is a genpd domain.
+ */
+static struct generic_pm_domain *dev_to_genpd(struct device *dev)
 {
 	if (IS_ERR_OR_NULL(dev->pm_domain))
 		return ERR_PTR(-EINVAL);
@@ -173,8 +202,8 @@ static int genpd_power_on(struct generic_pm_domain *genpd)
 	genpd->power_on_latency_ns = elapsed_ns;
 	genpd->max_off_time_changed = true;
 	genpd_recalc_cpu_exit_latency(genpd);
-	pr_warn("%s: Power-%s latency exceeded, new value %lld ns\n",
-		genpd->name, "on", elapsed_ns);
+	pr_debug("%s: Power-%s latency exceeded, new value %lld ns\n",
+		 genpd->name, "on", elapsed_ns);
 
 	return ret;
 }
@@ -199,8 +228,8 @@ static int genpd_power_off(struct generic_pm_domain *genpd)
 
 	genpd->power_off_latency_ns = elapsed_ns;
 	genpd->max_off_time_changed = true;
-	pr_warn("%s: Power-%s latency exceeded, new value %lld ns\n",
-		genpd->name, "off", elapsed_ns);
+	pr_debug("%s: Power-%s latency exceeded, new value %lld ns\n",
+		 genpd->name, "off", elapsed_ns);
 
 	return ret;
 }
@@ -1513,9 +1542,7 @@ int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 
 	dev_dbg(dev, "%s()\n", __func__);
 
-	if (IS_ERR_OR_NULL(genpd) || IS_ERR_OR_NULL(dev)
-	    ||  IS_ERR_OR_NULL(dev->pm_domain)
-	    ||  pd_to_genpd(dev->pm_domain) != genpd)
+	if (!genpd || genpd != pm_genpd_lookup_dev(dev))
 		return -EINVAL;
 
 	/* The above validation also means we have existing domain_data. */
@@ -2093,21 +2120,10 @@ EXPORT_SYMBOL_GPL(of_genpd_get_from_provider);
  */
 static void genpd_dev_pm_detach(struct device *dev, bool power_off)
 {
-	struct generic_pm_domain *pd = NULL, *gpd;
+	struct generic_pm_domain *pd;
 	int ret = 0;
 
-	if (!dev->pm_domain)
-		return;
-
-	mutex_lock(&gpd_list_lock);
-	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
-		if (&gpd->domain == dev->pm_domain) {
-			pd = gpd;
-			break;
-		}
-	}
-	mutex_unlock(&gpd_list_lock);
-
+	pd = pm_genpd_lookup_dev(dev);
 	if (!pd)
 		return;
 
@@ -2130,6 +2146,17 @@ static void genpd_dev_pm_detach(struct device *dev, bool power_off)
 	genpd_queue_power_off_work(pd);
 }
 
+static void genpd_dev_pm_sync(struct device *dev)
+{
+	struct generic_pm_domain *pd;
+
+	pd = dev_to_genpd(dev);
+	if (IS_ERR(pd))
+		return;
+
+	genpd_queue_power_off_work(pd);
+}
+
 /**
  * genpd_dev_pm_attach - Attach a device to its PM domain using DT.
  * @dev: Device to attach.
@@ -2196,6 +2223,7 @@ int genpd_dev_pm_attach(struct device *dev)
 	}
 
 	dev->pm_domain->detach = genpd_dev_pm_detach;
+	dev->pm_domain->sync = genpd_dev_pm_sync;
 	pm_genpd_poweron(pd);
 
 	return 0;
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 9717d5f20139..3d874eca7104 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -23,7 +23,7 @@
 #include <linux/mutex.h>
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
-#include <linux/resume-trace.h>
+#include <linux/pm-trace.h>
 #include <linux/interrupt.h>
 #include <linux/sched.h>
 #include <linux/async.h>
@@ -1017,6 +1017,9 @@ static int __device_suspend_noirq(struct device *dev, pm_message_t state, bool a
 	char *info = NULL;
 	int error = 0;
 
+	TRACE_DEVICE(dev);
+	TRACE_SUSPEND(0);
+
 	if (async_error)
 		goto Complete;
 
@@ -1057,6 +1060,7 @@ static int __device_suspend_noirq(struct device *dev, pm_message_t state, bool a
 
 Complete:
 	complete_all(&dev->power.completion);
+	TRACE_SUSPEND(error);
 	return error;
 }
 
@@ -1078,7 +1082,7 @@ static int device_suspend_noirq(struct device *dev)
 {
 	reinit_completion(&dev->power.completion);
 
-	if (pm_async_enabled && dev->power.async_suspend) {
+	if (is_async(dev)) {
 		get_device(dev);
 		async_schedule(async_suspend_noirq, dev);
 		return 0;
@@ -1157,6 +1161,9 @@ static int __device_suspend_late(struct device *dev, pm_message_t state, bool as
 	char *info = NULL;
 	int error = 0;
 
+	TRACE_DEVICE(dev);
+	TRACE_SUSPEND(0);
+
 	__pm_runtime_disable(dev, false);
 
 	if (async_error)
@@ -1198,6 +1205,7 @@ static int __device_suspend_late(struct device *dev, pm_message_t state, bool as
 		async_error = error;
 
 Complete:
+	TRACE_SUSPEND(error);
 	complete_all(&dev->power.completion);
 	return error;
 }
@@ -1219,7 +1227,7 @@ static int device_suspend_late(struct device *dev)
 {
 	reinit_completion(&dev->power.completion);
 
-	if (pm_async_enabled && dev->power.async_suspend) {
+	if (is_async(dev)) {
 		get_device(dev);
 		async_schedule(async_suspend_late, dev);
 		return 0;
@@ -1338,6 +1346,9 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 	int error = 0;
 	DECLARE_DPM_WATCHDOG_ON_STACK(wd);
 
+	TRACE_DEVICE(dev);
+	TRACE_SUSPEND(0);
+
 	dpm_wait_for_children(dev, async);
 
 	if (async_error)
@@ -1444,6 +1455,7 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 	if (error)
 		async_error = error;
 
+	TRACE_SUSPEND(error);
 	return error;
 }
 
@@ -1465,7 +1477,7 @@ static int device_suspend(struct device *dev)
 {
 	reinit_completion(&dev->power.completion);
 
-	if (pm_async_enabled && dev->power.async_suspend) {
+	if (is_async(dev)) {
 		get_device(dev);
 		async_schedule(async_suspend, dev);
 		return 0;
diff --git a/drivers/base/power/trace.c b/drivers/base/power/trace.c
index d94a1f5121cf..a311cfa4c5bd 100644
--- a/drivers/base/power/trace.c
+++ b/drivers/base/power/trace.c
@@ -7,7 +7,7 @@
  * devices may be working.
  */
 
-#include <linux/resume-trace.h>
+#include <linux/pm-trace.h>
 #include <linux/export.h>
 #include <linux/rtc.h>
 
@@ -154,7 +154,7 @@ EXPORT_SYMBOL(set_trace_device);
  * it's not any guarantee, but it's a high _likelihood_ that
  * the match is valid).
  */
-void generate_resume_trace(const void *tracedata, unsigned int user)
+void generate_pm_trace(const void *tracedata, unsigned int user)
 {
 	unsigned short lineno = *(unsigned short *)tracedata;
 	const char *file = *(const char **)(tracedata + 2);
@@ -164,7 +164,7 @@ void generate_resume_trace(const void *tracedata, unsigned int user)
 	file_hash_value = hash_string(lineno, file, FILEHASH);
 	set_magic_time(user_hash_value, file_hash_value, dev_hash_value);
 }
-EXPORT_SYMBOL(generate_resume_trace);
+EXPORT_SYMBOL(generate_pm_trace);
 
 extern char __tracedata_start, __tracedata_end;
 static int show_file_hash(unsigned int value)
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index aab7158d2afe..77262009f89d 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -843,7 +843,6 @@ static int print_wakeup_source_stats(struct seq_file *m,
 	unsigned long active_count;
 	ktime_t active_time;
 	ktime_t prevent_sleep_time;
-	int ret;
 
 	spin_lock_irqsave(&ws->lock, flags);
 
@@ -866,17 +865,16 @@ static int print_wakeup_source_stats(struct seq_file *m,
 		active_time = ktime_set(0, 0);
 	}
 
-	ret = seq_printf(m, "%-12s\t%lu\t\t%lu\t\t%lu\t\t%lu\t\t"
-			"%lld\t\t%lld\t\t%lld\t\t%lld\t\t%lld\n",
-			ws->name, active_count, ws->event_count,
-			ws->wakeup_count, ws->expire_count,
-			ktime_to_ms(active_time), ktime_to_ms(total_time),
-			ktime_to_ms(max_time), ktime_to_ms(ws->last_time),
-			ktime_to_ms(prevent_sleep_time));
+	seq_printf(m, "%-12s\t%lu\t\t%lu\t\t%lu\t\t%lu\t\t%lld\t\t%lld\t\t%lld\t\t%lld\t\t%lld\n",
+		   ws->name, active_count, ws->event_count,
+		   ws->wakeup_count, ws->expire_count,
+		   ktime_to_ms(active_time), ktime_to_ms(total_time),
+		   ktime_to_ms(max_time), ktime_to_ms(ws->last_time),
+		   ktime_to_ms(prevent_sleep_time));
 
 	spin_unlock_irqrestore(&ws->lock, flags);
 
-	return ret;
+	return 0;
 }
 
 /**
diff --git a/drivers/base/property.c b/drivers/base/property.c
index c45845874d4f..1d0b116cae95 100644
--- a/drivers/base/property.c
+++ b/drivers/base/property.c
@@ -10,10 +10,102 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/property.h>
-#include <linux/export.h>
 #include <linux/acpi.h>
+#include <linux/export.h>
+#include <linux/kernel.h>
 #include <linux/of.h>
+#include <linux/property.h>
+
+/**
+ * device_add_property_set - Add a collection of properties to a device object.
+ * @dev: Device to add properties to.
+ * @pset: Collection of properties to add.
+ *
+ * Associate a collection of device properties represented by @pset with @dev
+ * as its secondary firmware node.
+ */
+void device_add_property_set(struct device *dev, struct property_set *pset)
+{
+	if (pset)
+		pset->fwnode.type = FWNODE_PDATA;
+
+	set_secondary_fwnode(dev, &pset->fwnode);
+}
+EXPORT_SYMBOL_GPL(device_add_property_set);
+
+static inline bool is_pset(struct fwnode_handle *fwnode)
+{
+	return fwnode && fwnode->type == FWNODE_PDATA;
+}
+
+static inline struct property_set *to_pset(struct fwnode_handle *fwnode)
+{
+	return is_pset(fwnode) ?
+		container_of(fwnode, struct property_set, fwnode) : NULL;
+}
+
+static struct property_entry *pset_prop_get(struct property_set *pset,
+					    const char *name)
+{
+	struct property_entry *prop;
+
+	if (!pset || !pset->properties)
+		return NULL;
+
+	for (prop = pset->properties; prop->name; prop++)
+		if (!strcmp(name, prop->name))
+			return prop;
+
+	return NULL;
+}
+
+static int pset_prop_read_array(struct property_set *pset, const char *name,
+				enum dev_prop_type type, void *val, size_t nval)
+{
+	struct property_entry *prop;
+	unsigned int item_size;
+
+	prop = pset_prop_get(pset, name);
+	if (!prop)
+		return -ENODATA;
+
+	if (prop->type != type)
+		return -EPROTO;
+
+	if (!val)
+		return prop->nval;
+
+	if (prop->nval < nval)
+		return -EOVERFLOW;
+
+	switch (type) {
+	case DEV_PROP_U8:
+		item_size = sizeof(u8);
+		break;
+	case DEV_PROP_U16:
+		item_size = sizeof(u16);
+		break;
+	case DEV_PROP_U32:
+		item_size = sizeof(u32);
+		break;
+	case DEV_PROP_U64:
+		item_size = sizeof(u64);
+		break;
+	case DEV_PROP_STRING:
+		item_size = sizeof(const char *);
+		break;
+	default:
+		return -EINVAL;
+	}
+	memcpy(val, prop->value.raw_data, nval * item_size);
+	return 0;
+}
+
+static inline struct fwnode_handle *dev_fwnode(struct device *dev)
+{
+	return IS_ENABLED(CONFIG_OF) && dev->of_node ?
+		&dev->of_node->fwnode : dev->fwnode;
+}
 
 /**
  * device_property_present - check if a property of a device is present
@@ -24,10 +116,7 @@
  */
 bool device_property_present(struct device *dev, const char *propname)
 {
-	if (IS_ENABLED(CONFIG_OF) && dev->of_node)
-		return of_property_read_bool(dev->of_node, propname);
-
-	return !acpi_dev_prop_get(ACPI_COMPANION(dev), propname, NULL);
+	return fwnode_property_present(dev_fwnode(dev), propname);
 }
 EXPORT_SYMBOL_GPL(device_property_present);
 
@@ -43,32 +132,22 @@ bool fwnode_property_present(struct fwnode_handle *fwnode, const char *propname)
 	else if (is_acpi_node(fwnode))
 		return !acpi_dev_prop_get(acpi_node(fwnode), propname, NULL);
 
-	return false;
+	return !!pset_prop_get(to_pset(fwnode), propname);
 }
 EXPORT_SYMBOL_GPL(fwnode_property_present);
 
-#define OF_DEV_PROP_READ_ARRAY(node, propname, type, val, nval) \
-	(val) ? of_property_read_##type##_array((node), (propname), (val), (nval)) \
-	      : of_property_count_elems_of_size((node), (propname), sizeof(type))
-
-#define DEV_PROP_READ_ARRAY(_dev_, _propname_, _type_, _proptype_, _val_, _nval_) \
-	IS_ENABLED(CONFIG_OF) && _dev_->of_node ? \
-		(OF_DEV_PROP_READ_ARRAY(_dev_->of_node, _propname_, _type_, \
-					_val_, _nval_)) : \
-		acpi_dev_prop_read(ACPI_COMPANION(_dev_), _propname_, \
-				   _proptype_, _val_, _nval_)
-
 /**
  * device_property_read_u8_array - return a u8 array property of a device
  * @dev: Device to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Function reads an array of u8 properties with @propname from the device
  * firmware description and stores them to @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -77,7 +156,7 @@ EXPORT_SYMBOL_GPL(fwnode_property_present);
 int device_property_read_u8_array(struct device *dev, const char *propname,
 				  u8 *val, size_t nval)
 {
-	return DEV_PROP_READ_ARRAY(dev, propname, u8, DEV_PROP_U8, val, nval);
+	return fwnode_property_read_u8_array(dev_fwnode(dev), propname, val, nval);
 }
 EXPORT_SYMBOL_GPL(device_property_read_u8_array);
 
@@ -85,13 +164,14 @@ EXPORT_SYMBOL_GPL(device_property_read_u8_array);
  * device_property_read_u16_array - return a u16 array property of a device
  * @dev: Device to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Function reads an array of u16 properties with @propname from the device
  * firmware description and stores them to @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -100,7 +180,7 @@ EXPORT_SYMBOL_GPL(device_property_read_u8_array);
 int device_property_read_u16_array(struct device *dev, const char *propname,
 				   u16 *val, size_t nval)
 {
-	return DEV_PROP_READ_ARRAY(dev, propname, u16, DEV_PROP_U16, val, nval);
+	return fwnode_property_read_u16_array(dev_fwnode(dev), propname, val, nval);
 }
 EXPORT_SYMBOL_GPL(device_property_read_u16_array);
 
@@ -108,13 +188,14 @@ EXPORT_SYMBOL_GPL(device_property_read_u16_array);
  * device_property_read_u32_array - return a u32 array property of a device
  * @dev: Device to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Function reads an array of u32 properties with @propname from the device
  * firmware description and stores them to @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -123,7 +204,7 @@ EXPORT_SYMBOL_GPL(device_property_read_u16_array);
 int device_property_read_u32_array(struct device *dev, const char *propname,
 				   u32 *val, size_t nval)
 {
-	return DEV_PROP_READ_ARRAY(dev, propname, u32, DEV_PROP_U32, val, nval);
+	return fwnode_property_read_u32_array(dev_fwnode(dev), propname, val, nval);
 }
 EXPORT_SYMBOL_GPL(device_property_read_u32_array);
 
@@ -131,13 +212,14 @@ EXPORT_SYMBOL_GPL(device_property_read_u32_array);
  * device_property_read_u64_array - return a u64 array property of a device
  * @dev: Device to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Function reads an array of u64 properties with @propname from the device
  * firmware description and stores them to @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -146,7 +228,7 @@ EXPORT_SYMBOL_GPL(device_property_read_u32_array);
 int device_property_read_u64_array(struct device *dev, const char *propname,
 				   u64 *val, size_t nval)
 {
-	return DEV_PROP_READ_ARRAY(dev, propname, u64, DEV_PROP_U64, val, nval);
+	return fwnode_property_read_u64_array(dev_fwnode(dev), propname, val, nval);
 }
 EXPORT_SYMBOL_GPL(device_property_read_u64_array);
 
@@ -154,13 +236,14 @@ EXPORT_SYMBOL_GPL(device_property_read_u64_array);
  * device_property_read_string_array - return a string array property of device
  * @dev: Device to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Function reads an array of string properties with @propname from the device
  * firmware description and stores them to @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO or %-EILSEQ if the property is not an array of strings,
@@ -169,10 +252,7 @@ EXPORT_SYMBOL_GPL(device_property_read_u64_array);
 int device_property_read_string_array(struct device *dev, const char *propname,
 				      const char **val, size_t nval)
 {
-	return IS_ENABLED(CONFIG_OF) && dev->of_node ?
-		of_property_read_string_array(dev->of_node, propname, val, nval) :
-		acpi_dev_prop_read(ACPI_COMPANION(dev), propname,
-				   DEV_PROP_STRING, val, nval);
+	return fwnode_property_read_string_array(dev_fwnode(dev), propname, val, nval);
 }
 EXPORT_SYMBOL_GPL(device_property_read_string_array);
 
@@ -193,13 +273,14 @@ EXPORT_SYMBOL_GPL(device_property_read_string_array);
 int device_property_read_string(struct device *dev, const char *propname,
 				const char **val)
 {
-	return IS_ENABLED(CONFIG_OF) && dev->of_node ?
-		of_property_read_string(dev->of_node, propname, val) :
-		acpi_dev_prop_read(ACPI_COMPANION(dev), propname,
-				   DEV_PROP_STRING, val, 1);
+	return fwnode_property_read_string(dev_fwnode(dev), propname, val);
 }
 EXPORT_SYMBOL_GPL(device_property_read_string);
 
+#define OF_DEV_PROP_READ_ARRAY(node, propname, type, val, nval) \
+	(val) ? of_property_read_##type##_array((node), (propname), (val), (nval)) \
+	      : of_property_count_elems_of_size((node), (propname), sizeof(type))
+
 #define FWNODE_PROP_READ_ARRAY(_fwnode_, _propname_, _type_, _proptype_, _val_, _nval_) \
 ({ \
 	int _ret_; \
@@ -210,7 +291,8 @@ EXPORT_SYMBOL_GPL(device_property_read_string);
 		_ret_ = acpi_dev_prop_read(acpi_node(_fwnode_), _propname_, \
 					   _proptype_, _val_, _nval_); \
 	else \
-		_ret_ = -ENXIO; \
+		_ret_ = pset_prop_read_array(to_pset(_fwnode_), _propname_, \
+					     _proptype_, _val_, _nval_); \
 	_ret_; \
 })
 
@@ -218,13 +300,14 @@ EXPORT_SYMBOL_GPL(device_property_read_string);
  * fwnode_property_read_u8_array - return a u8 array property of firmware node
  * @fwnode: Firmware node to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Read an array of u8 properties with @propname from @fwnode and stores them to
  * @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -243,13 +326,14 @@ EXPORT_SYMBOL_GPL(fwnode_property_read_u8_array);
  * fwnode_property_read_u16_array - return a u16 array property of firmware node
  * @fwnode: Firmware node to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Read an array of u16 properties with @propname from @fwnode and store them to
  * @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -268,13 +352,14 @@ EXPORT_SYMBOL_GPL(fwnode_property_read_u16_array);
  * fwnode_property_read_u32_array - return a u32 array property of firmware node
  * @fwnode: Firmware node to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Read an array of u32 properties with @propname from @fwnode store them to
  * @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -293,13 +378,14 @@ EXPORT_SYMBOL_GPL(fwnode_property_read_u32_array);
  * fwnode_property_read_u64_array - return a u64 array property firmware node
  * @fwnode: Firmware node to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Read an array of u64 properties with @propname from @fwnode and store them to
  * @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of numbers,
@@ -318,13 +404,14 @@ EXPORT_SYMBOL_GPL(fwnode_property_read_u64_array);
  * fwnode_property_read_string_array - return string array property of a node
  * @fwnode: Firmware node to get the property of
  * @propname: Name of the property
- * @val: The values are stored here
+ * @val: The values are stored here or %NULL to return the number of values
  * @nval: Size of the @val array
  *
  * Read an string list property @propname from the given firmware node and store
  * them to @val if found.
  *
- * Return: %0 if the property was found (success),
+ * Return: number of values if @val was %NULL,
+ *         %0 if the property was found (success),
  *	   %-EINVAL if given arguments are not valid,
  *	   %-ENODATA if the property does not have a value,
  *	   %-EPROTO if the property is not an array of strings,
@@ -336,13 +423,16 @@ int fwnode_property_read_string_array(struct fwnode_handle *fwnode,
 				      size_t nval)
 {
 	if (is_of_node(fwnode))
-		return of_property_read_string_array(of_node(fwnode), propname,
-						     val, nval);
+		return val ?
+			of_property_read_string_array(of_node(fwnode), propname,
+						      val, nval) :
+			of_property_count_strings(of_node(fwnode), propname);
 	else if (is_acpi_node(fwnode))
 		return acpi_dev_prop_read(acpi_node(fwnode), propname,
 					  DEV_PROP_STRING, val, nval);
 
-	return -ENXIO;
+	return pset_prop_read_array(to_pset(fwnode), propname,
+				    DEV_PROP_STRING, val, nval);
 }
 EXPORT_SYMBOL_GPL(fwnode_property_read_string_array);
 
@@ -365,7 +455,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/bcma/Kconfig b/drivers/bcma/Kconfig
index 0ee48be23837..fc6ffcfa8061 100644
--- a/drivers/bcma/Kconfig
+++ b/drivers/bcma/Kconfig
@@ -26,6 +26,7 @@ config BCMA_HOST_PCI_POSSIBLE
 config BCMA_HOST_PCI
 	bool "Support for BCMA on PCI-host bus"
 	depends on BCMA_HOST_PCI_POSSIBLE
+	select BCMA_DRIVER_PCI
 	default y
 
 config BCMA_DRIVER_PCI_HOSTMODE
@@ -44,6 +45,22 @@ config BCMA_HOST_SOC
 
 	  If unsure, say N
 
+config BCMA_DRIVER_PCI
+	bool "BCMA Broadcom PCI core driver"
+	depends on BCMA && PCI
+	default y
+	help
+	  BCMA bus may have many versions of PCIe core. This driver
+	  supports:
+	  1) PCIe core working in clientmode
+	  2) PCIe Gen 2 clientmode core
+
+	  In general PCIe (Gen 2) clientmode core is required on PCIe
+	  hosted buses. It's responsible for initialization and basic
+	  hardware management.
+	  This driver is also prerequisite for a hostmode PCIe core
+	  support.
+
 config BCMA_DRIVER_MIPS
 	bool "BCMA Broadcom MIPS core driver"
 	depends on BCMA && MIPS
diff --git a/drivers/bcma/Makefile b/drivers/bcma/Makefile
index 838b4b9d352f..f32af9b76bcd 100644
--- a/drivers/bcma/Makefile
+++ b/drivers/bcma/Makefile
@@ -3,8 +3,8 @@ bcma-y					+= driver_chipcommon.o driver_chipcommon_pmu.o
 bcma-y					+= driver_chipcommon_b.o
 bcma-$(CONFIG_BCMA_SFLASH)		+= driver_chipcommon_sflash.o
 bcma-$(CONFIG_BCMA_NFLASH)		+= driver_chipcommon_nflash.o
-bcma-y					+= driver_pci.o
-bcma-y					+= driver_pcie2.o
+bcma-$(CONFIG_BCMA_DRIVER_PCI)		+= driver_pci.o
+bcma-$(CONFIG_BCMA_DRIVER_PCI)		+= driver_pcie2.o
 bcma-$(CONFIG_BCMA_DRIVER_PCI_HOSTMODE)	+= driver_pci_host.o
 bcma-$(CONFIG_BCMA_DRIVER_MIPS)		+= driver_mips.o
 bcma-$(CONFIG_BCMA_DRIVER_GMAC_CMN)	+= driver_gmac_cmn.o
diff --git a/drivers/bcma/bcma_private.h b/drivers/bcma/bcma_private.h
index ac6c5fca906d..15f2b2e242ea 100644
--- a/drivers/bcma/bcma_private.h
+++ b/drivers/bcma/bcma_private.h
@@ -26,6 +26,7 @@ bool bcma_wait_value(struct bcma_device *core, u16 reg, u32 mask, u32 value,
 		     int timeout);
 void bcma_prepare_core(struct bcma_bus *bus, struct bcma_device *core);
 void bcma_init_bus(struct bcma_bus *bus);
+void bcma_unregister_cores(struct bcma_bus *bus);
 int bcma_bus_register(struct bcma_bus *bus);
 void bcma_bus_unregister(struct bcma_bus *bus);
 int __init bcma_bus_early_register(struct bcma_bus *bus);
@@ -42,6 +43,9 @@ int bcma_bus_scan(struct bcma_bus *bus);
 int bcma_sprom_get(struct bcma_bus *bus);
 
 /* driver_chipcommon.c */
+void bcma_core_chipcommon_early_init(struct bcma_drv_cc *cc);
+void bcma_core_chipcommon_init(struct bcma_drv_cc *cc);
+void bcma_chipco_bcm4331_ext_pa_lines_ctl(struct bcma_drv_cc *cc, bool enable);
 #ifdef CONFIG_BCMA_DRIVER_MIPS
 void bcma_chipco_serial_init(struct bcma_drv_cc *cc);
 extern struct platform_device bcma_pflash_dev;
@@ -52,6 +56,8 @@ int bcma_core_chipcommon_b_init(struct bcma_drv_cc_b *ccb);
 void bcma_core_chipcommon_b_free(struct bcma_drv_cc_b *ccb);
 
 /* driver_chipcommon_pmu.c */
+void bcma_pmu_early_init(struct bcma_drv_cc *cc);
+void bcma_pmu_init(struct bcma_drv_cc *cc);
 u32 bcma_pmu_get_alp_clock(struct bcma_drv_cc *cc);
 u32 bcma_pmu_get_cpu_clock(struct bcma_drv_cc *cc);
 
@@ -100,7 +106,35 @@ static inline void __exit bcma_host_soc_unregister_driver(void)
 #endif /* CONFIG_BCMA_HOST_SOC && CONFIG_OF */
 
 /* driver_pci.c */
+#ifdef CONFIG_BCMA_DRIVER_PCI
 u32 bcma_pcie_read(struct bcma_drv_pci *pc, u32 address);
+void bcma_core_pci_early_init(struct bcma_drv_pci *pc);
+void bcma_core_pci_init(struct bcma_drv_pci *pc);
+void bcma_core_pci_up(struct bcma_drv_pci *pc);
+void bcma_core_pci_down(struct bcma_drv_pci *pc);
+#else
+static inline void bcma_core_pci_early_init(struct bcma_drv_pci *pc)
+{
+	WARN_ON(pc->core->bus->hosttype == BCMA_HOSTTYPE_PCI);
+}
+static inline void bcma_core_pci_init(struct bcma_drv_pci *pc)
+{
+	/* Initialization is required for PCI hosted bus */
+	WARN_ON(pc->core->bus->hosttype == BCMA_HOSTTYPE_PCI);
+}
+#endif
+
+/* driver_pcie2.c */
+#ifdef CONFIG_BCMA_DRIVER_PCI
+void bcma_core_pcie2_init(struct bcma_drv_pcie2 *pcie2);
+void bcma_core_pcie2_up(struct bcma_drv_pcie2 *pcie2);
+#else
+static inline void bcma_core_pcie2_init(struct bcma_drv_pcie2 *pcie2)
+{
+	/* Initialization is required for PCI hosted bus */
+	WARN_ON(pcie2->core->bus->hosttype == BCMA_HOSTTYPE_PCI);
+}
+#endif
 
 extern int bcma_chipco_watchdog_register(struct bcma_drv_cc *cc);
 
@@ -117,6 +151,39 @@ static inline void bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
 }
 #endif /* CONFIG_BCMA_DRIVER_PCI_HOSTMODE */
 
+/**************************************************
+ * driver_mips.c
+ **************************************************/
+
+#ifdef CONFIG_BCMA_DRIVER_MIPS
+unsigned int bcma_core_mips_irq(struct bcma_device *dev);
+void bcma_core_mips_early_init(struct bcma_drv_mips *mcore);
+void bcma_core_mips_init(struct bcma_drv_mips *mcore);
+#else
+static inline unsigned int bcma_core_mips_irq(struct bcma_device *dev)
+{
+	return 0;
+}
+static inline void bcma_core_mips_early_init(struct bcma_drv_mips *mcore)
+{
+}
+static inline void bcma_core_mips_init(struct bcma_drv_mips *mcore)
+{
+}
+#endif
+
+/**************************************************
+ * driver_gmac_cmn.c
+ **************************************************/
+
+#ifdef CONFIG_BCMA_DRIVER_GMAC_CMN
+void bcma_core_gmac_cmn_init(struct bcma_drv_gmac_cmn *gc);
+#else
+static inline void bcma_core_gmac_cmn_init(struct bcma_drv_gmac_cmn *gc)
+{
+}
+#endif
+
 #ifdef CONFIG_BCMA_DRIVER_GPIO
 /* driver_gpio.c */
 int bcma_gpio_init(struct bcma_drv_cc *cc);
diff --git a/drivers/bcma/driver_gpio.c b/drivers/bcma/driver_gpio.c
index 598a6cd9028a..74ccb02e0f10 100644
--- a/drivers/bcma/driver_gpio.c
+++ b/drivers/bcma/driver_gpio.c
@@ -17,6 +17,8 @@
 
 #include "bcma_private.h"
 
+#define BCMA_GPIO_MAX_PINS	32
+
 static inline struct bcma_drv_cc *bcma_gpio_get_cc(struct gpio_chip *chip)
 {
 	return container_of(chip, struct bcma_drv_cc, gpio);
@@ -76,7 +78,7 @@ static void bcma_gpio_free(struct gpio_chip *chip, unsigned gpio)
 	bcma_chipco_gpio_pullup(cc, 1 << gpio, 0);
 }
 
-#if IS_BUILTIN(CONFIG_BCM47XX)
+#if IS_BUILTIN(CONFIG_BCM47XX) || IS_BUILTIN(CONFIG_ARCH_BCM_5301X)
 static int bcma_gpio_to_irq(struct gpio_chip *chip, unsigned gpio)
 {
 	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
@@ -204,6 +206,7 @@ static void bcma_gpio_irq_domain_exit(struct bcma_drv_cc *cc)
 
 int bcma_gpio_init(struct bcma_drv_cc *cc)
 {
+	struct bcma_bus *bus = cc->core->bus;
 	struct gpio_chip *chip = &cc->gpio;
 	int err;
 
@@ -215,14 +218,14 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 	chip->set		= bcma_gpio_set_value;
 	chip->direction_input	= bcma_gpio_direction_input;
 	chip->direction_output	= bcma_gpio_direction_output;
-#if IS_BUILTIN(CONFIG_BCM47XX)
+#if IS_BUILTIN(CONFIG_BCM47XX) || IS_BUILTIN(CONFIG_ARCH_BCM_5301X)
 	chip->to_irq		= bcma_gpio_to_irq;
 #endif
 #if IS_BUILTIN(CONFIG_OF)
 	if (cc->core->bus->hosttype == BCMA_HOSTTYPE_SOC)
 		chip->of_node	= cc->core->dev.of_node;
 #endif
-	switch (cc->core->bus->chipinfo.id) {
+	switch (bus->chipinfo.id) {
 	case BCMA_CHIP_ID_BCM5357:
 	case BCMA_CHIP_ID_BCM53572:
 		chip->ngpio	= 32;
@@ -231,13 +234,17 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 		chip->ngpio	= 16;
 	}
 
-	/* There is just one SoC in one device and its GPIO addresses should be
-	 * deterministic to address them more easily. The other buses could get
-	 * a random base number. */
-	if (cc->core->bus->hosttype == BCMA_HOSTTYPE_SOC)
-		chip->base		= 0;
-	else
-		chip->base		= -1;
+	/*
+	 * On MIPS we register GPIO devices (LEDs, buttons) using absolute GPIO
+	 * pin numbers. We don't have Device Tree there and we can't really use
+	 * relative (per chip) numbers.
+	 * So let's use predictable base for BCM47XX and "random" for all other.
+	 */
+#if IS_BUILTIN(CONFIG_BCM47XX)
+	chip->base		= bus->num * BCMA_GPIO_MAX_PINS;
+#else
+	chip->base		= -1;
+#endif
 
 	err = bcma_gpio_irq_domain_init(cc);
 	if (err)
diff --git a/drivers/bcma/driver_pci.c b/drivers/bcma/driver_pci.c
index 786666488a2d..f499a469e66d 100644
--- a/drivers/bcma/driver_pci.c
+++ b/drivers/bcma/driver_pci.c
@@ -282,39 +282,6 @@ void bcma_core_pci_power_save(struct bcma_bus *bus, bool up)
 }
 EXPORT_SYMBOL_GPL(bcma_core_pci_power_save);
 
-int bcma_core_pci_irq_ctl(struct bcma_drv_pci *pc, struct bcma_device *core,
-			  bool enable)
-{
-	struct pci_dev *pdev;
-	u32 coremask, tmp;
-	int err = 0;
-
-	if (!pc || core->bus->hosttype != BCMA_HOSTTYPE_PCI) {
-		/* This bcma device is not on a PCI host-bus. So the IRQs are
-		 * not routed through the PCI core.
-		 * So we must not enable routing through the PCI core. */
-		goto out;
-	}
-
-	pdev = pc->core->bus->host_pci;
-
-	err = pci_read_config_dword(pdev, BCMA_PCI_IRQMASK, &tmp);
-	if (err)
-		goto out;
-
-	coremask = BIT(core->core_index) << 8;
-	if (enable)
-		tmp |= coremask;
-	else
-		tmp &= ~coremask;
-
-	err = pci_write_config_dword(pdev, BCMA_PCI_IRQMASK, tmp);
-
-out:
-	return err;
-}
-EXPORT_SYMBOL_GPL(bcma_core_pci_irq_ctl);
-
 static void bcma_core_pci_extend_L1timer(struct bcma_drv_pci *pc, bool extend)
 {
 	u32 w;
@@ -328,28 +295,12 @@ static void bcma_core_pci_extend_L1timer(struct bcma_drv_pci *pc, bool extend)
 	bcma_pcie_read(pc, BCMA_CORE_PCI_DLLP_PMTHRESHREG);
 }
 
-void bcma_core_pci_up(struct bcma_bus *bus)
+void bcma_core_pci_up(struct bcma_drv_pci *pc)
 {
-	struct bcma_drv_pci *pc;
-
-	if (bus->hosttype != BCMA_HOSTTYPE_PCI)
-		return;
-
-	pc = &bus->drv_pci[0];
-
 	bcma_core_pci_extend_L1timer(pc, true);
 }
-EXPORT_SYMBOL_GPL(bcma_core_pci_up);
 
-void bcma_core_pci_down(struct bcma_bus *bus)
+void bcma_core_pci_down(struct bcma_drv_pci *pc)
 {
-	struct bcma_drv_pci *pc;
-
-	if (bus->hosttype != BCMA_HOSTTYPE_PCI)
-		return;
-
-	pc = &bus->drv_pci[0];
-
 	bcma_core_pci_extend_L1timer(pc, false);
 }
-EXPORT_SYMBOL_GPL(bcma_core_pci_down);
diff --git a/drivers/bcma/driver_pci_host.c b/drivers/bcma/driver_pci_host.c
index c8a6b741967b..c42cec7c7ecc 100644
--- a/drivers/bcma/driver_pci_host.c
+++ b/drivers/bcma/driver_pci_host.c
@@ -11,6 +11,7 @@
 
 #include "bcma_private.h"
 #include <linux/pci.h>
+#include <linux/slab.h>
 #include <linux/export.h>
 #include <linux/bcma/bcma.h>
 #include <asm/paccess.h>
diff --git a/drivers/bcma/driver_pcie2.c b/drivers/bcma/driver_pcie2.c
index e4be537b0c66..b1a6e327cb23 100644
--- a/drivers/bcma/driver_pcie2.c
+++ b/drivers/bcma/driver_pcie2.c
@@ -10,6 +10,7 @@
 
 #include "bcma_private.h"
 #include <linux/bcma/bcma.h>
+#include <linux/pci.h>
 
 /**************************************************
  * R/W ops.
@@ -156,14 +157,23 @@ static void pciedev_reg_pm_clk_period(struct bcma_drv_pcie2 *pcie2)
 
 void bcma_core_pcie2_init(struct bcma_drv_pcie2 *pcie2)
 {
-	struct bcma_chipinfo *ci = &pcie2->core->bus->chipinfo;
+	struct bcma_bus *bus = pcie2->core->bus;
+	struct bcma_chipinfo *ci = &bus->chipinfo;
 	u32 tmp;
 
 	tmp = pcie2_read32(pcie2, BCMA_CORE_PCIE2_SPROM(54));
 	if ((tmp & 0xe) >> 1 == 2)
 		bcma_core_pcie2_cfg_write(pcie2, 0x4e0, 0x17);
 
-	/* TODO: Do we need pcie_reqsize? */
+	switch (bus->chipinfo.id) {
+	case BCMA_CHIP_ID_BCM4360:
+	case BCMA_CHIP_ID_BCM4352:
+		pcie2->reqsize = 1024;
+		break;
+	default:
+		pcie2->reqsize = 128;
+		break;
+	}
 
 	if (ci->id == BCMA_CHIP_ID_BCM4360 && ci->rev > 3)
 		bcma_core_pcie2_war_delay_perst_enab(pcie2, true);
@@ -173,3 +183,18 @@ void bcma_core_pcie2_init(struct bcma_drv_pcie2 *pcie2)
 	pciedev_crwlpciegen2_180(pcie2);
 	pciedev_crwlpciegen2_182(pcie2);
 }
+
+/**************************************************
+ * Runtime ops.
+ **************************************************/
+
+void bcma_core_pcie2_up(struct bcma_drv_pcie2 *pcie2)
+{
+	struct bcma_bus *bus = pcie2->core->bus;
+	struct pci_dev *dev = bus->host_pci;
+	int err;
+
+	err = pcie_set_readrq(dev, pcie2->reqsize);
+	if (err)
+		bcma_err(bus, "Error setting PCI_EXP_DEVCTL_READRQ: %d\n", err);
+}
diff --git a/drivers/bcma/host_pci.c b/drivers/bcma/host_pci.c
index 53c6a8a58859..0856189c065f 100644
--- a/drivers/bcma/host_pci.c
+++ b/drivers/bcma/host_pci.c
@@ -213,16 +213,26 @@ static int bcma_host_pci_probe(struct pci_dev *dev,
 	/* Initialize struct, detect chip */
 	bcma_init_bus(bus);
 
+	/* Scan bus to find out generation of PCIe core */
+	err = bcma_bus_scan(bus);
+	if (err)
+		goto err_pci_unmap_mmio;
+
+	if (bcma_find_core(bus, BCMA_CORE_PCIE2))
+		bus->host_is_pcie2 = true;
+
 	/* Register */
 	err = bcma_bus_register(bus);
 	if (err)
-		goto err_pci_unmap_mmio;
+		goto err_unregister_cores;
 
 	pci_set_drvdata(dev, bus);
 
 out:
 	return err;
 
+err_unregister_cores:
+	bcma_unregister_cores(bus);
 err_pci_unmap_mmio:
 	pci_iounmap(dev, bus->mmio);
 err_pci_release_regions:
@@ -283,9 +293,12 @@ static const struct pci_device_id bcma_pci_bridge_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4357) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4358) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4359) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4360) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4365) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x43a0) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x43a9) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x43aa) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x43b1) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4727) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 43227) },	/* 0xa8db, BCM43217 (sic!) */
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 43228) },	/* 0xa8dc */
@@ -310,3 +323,65 @@ void __exit bcma_host_pci_exit(void)
 {
 	pci_unregister_driver(&bcma_pci_bridge_driver);
 }
+
+/**************************************************
+ * Runtime ops for drivers.
+ **************************************************/
+
+/* See also pcicore_up */
+void bcma_host_pci_up(struct bcma_bus *bus)
+{
+	if (bus->hosttype != BCMA_HOSTTYPE_PCI)
+		return;
+
+	if (bus->host_is_pcie2)
+		bcma_core_pcie2_up(&bus->drv_pcie2);
+	else
+		bcma_core_pci_up(&bus->drv_pci[0]);
+}
+EXPORT_SYMBOL_GPL(bcma_host_pci_up);
+
+/* See also pcicore_down */
+void bcma_host_pci_down(struct bcma_bus *bus)
+{
+	if (bus->hosttype != BCMA_HOSTTYPE_PCI)
+		return;
+
+	if (!bus->host_is_pcie2)
+		bcma_core_pci_down(&bus->drv_pci[0]);
+}
+EXPORT_SYMBOL_GPL(bcma_host_pci_down);
+
+/* See also si_pci_setup */
+int bcma_host_pci_irq_ctl(struct bcma_bus *bus, struct bcma_device *core,
+			  bool enable)
+{
+	struct pci_dev *pdev;
+	u32 coremask, tmp;
+	int err = 0;
+
+	if (bus->hosttype != BCMA_HOSTTYPE_PCI) {
+		/* This bcma device is not on a PCI host-bus. So the IRQs are
+		 * not routed through the PCI core.
+		 * So we must not enable routing through the PCI core. */
+		goto out;
+	}
+
+	pdev = bus->host_pci;
+
+	err = pci_read_config_dword(pdev, BCMA_PCI_IRQMASK, &tmp);
+	if (err)
+		goto out;
+
+	coremask = BIT(core->core_index) << 8;
+	if (enable)
+		tmp |= coremask;
+	else
+		tmp &= ~coremask;
+
+	err = pci_write_config_dword(pdev, BCMA_PCI_IRQMASK, tmp);
+
+out:
+	return err;
+}
+EXPORT_SYMBOL_GPL(bcma_host_pci_irq_ctl);
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 38bde6eab8a4..9635f1033ce5 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -363,7 +363,7 @@ static int bcma_register_devices(struct bcma_bus *bus)
 	return 0;
 }
 
-static void bcma_unregister_cores(struct bcma_bus *bus)
+void bcma_unregister_cores(struct bcma_bus *bus)
 {
 	struct bcma_device *core, *tmp;
 
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/loop.c b/drivers/block/loop.c
index d1f168b73634..c4fd1e45ce1e 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -75,6 +75,7 @@
 #include <linux/sysfs.h>
 #include <linux/miscdevice.h>
 #include <linux/falloc.h>
+#include <linux/uio.h>
 #include "loop.h"
 
 #include <asm/uaccess.h>
@@ -229,13 +230,14 @@ lo_do_transfer(struct loop_device *lo, int cmd,
 static int __do_lo_send_write(struct file *file,
 		u8 *buf, const int len, loff_t pos)
 {
+	struct kvec kvec = {.iov_base = buf, .iov_len = len};
+	struct iov_iter from;
 	ssize_t bw;
-	mm_segment_t old_fs = get_fs();
+
+	iov_iter_kvec(&from, ITER_KVEC | WRITE, &kvec, 1, len);
 
 	file_start_write(file);
-	set_fs(get_ds());
-	bw = file->f_op->write(file, buf, len, &pos);
-	set_fs(old_fs);
+	bw = vfs_iter_write(file, &from, &pos);
 	file_end_write(file);
 	if (likely(bw == len))
 		return 0;
@@ -767,7 +769,7 @@ static int loop_set_fd(struct loop_device *lo, fmode_t mode,
 		goto out_putf;
 
 	if (!(file->f_mode & FMODE_WRITE) || !(mode & FMODE_WRITE) ||
-	    !file->f_op->write)
+	    !file->f_op->write_iter)
 		lo_flags |= LO_FLAGS_READ_ONLY;
 
 	lo_blocksize = S_ISBLK(inode->i_mode) ?
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 ceb32dd52a6c..e23be20a3417 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -3003,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/paride/pg.c b/drivers/block/paride/pg.c
index 2ce3dfd7e6b9..876d0c3eaf58 100644
--- a/drivers/block/paride/pg.c
+++ b/drivers/block/paride/pg.c
@@ -137,7 +137,7 @@
 
 */
 
-static bool verbose = 0;
+static int verbose;
 static int major = PG_MAJOR;
 static char *name = PG_NAME;
 static int disable = 0;
@@ -168,7 +168,7 @@ enum {D_PRT, D_PRO, D_UNI, D_MOD, D_SLV, D_DLY};
 
 #include <asm/uaccess.h>
 
-module_param(verbose, bool, 0644);
+module_param(verbose, int, 0644);
 module_param(major, int, 0);
 module_param(name, charp, 0);
 module_param_array(drive0, int, NULL, 0);
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index 871bd3550cb0..c94386aa563d 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -43,11 +43,22 @@ static const char *default_compressor = "lzo";
 /* Module params (documentation at end) */
 static unsigned int num_devices = 1;
 
+static inline void deprecated_attr_warn(const char *name)
+{
+	pr_warn_once("%d (%s) Attribute %s (and others) will be removed. %s\n",
+			task_pid_nr(current),
+			current->comm,
+			name,
+			"See zram documentation.");
+}
+
 #define ZRAM_ATTR_RO(name)						\
 static ssize_t name##_show(struct device *d,		\
 				struct device_attribute *attr, char *b)	\
 {									\
 	struct zram *zram = dev_to_zram(d);				\
+									\
+	deprecated_attr_warn(__stringify(name));			\
 	return scnprintf(b, PAGE_SIZE, "%llu\n",			\
 		(u64)atomic64_read(&zram->stats.name));			\
 }									\
@@ -89,6 +100,7 @@ static ssize_t orig_data_size_show(struct device *dev,
 {
 	struct zram *zram = dev_to_zram(dev);
 
+	deprecated_attr_warn("orig_data_size");
 	return scnprintf(buf, PAGE_SIZE, "%llu\n",
 		(u64)(atomic64_read(&zram->stats.pages_stored)) << PAGE_SHIFT);
 }
@@ -99,6 +111,7 @@ static ssize_t mem_used_total_show(struct device *dev,
 	u64 val = 0;
 	struct zram *zram = dev_to_zram(dev);
 
+	deprecated_attr_warn("mem_used_total");
 	down_read(&zram->init_lock);
 	if (init_done(zram)) {
 		struct zram_meta *meta = zram->meta;
@@ -128,6 +141,7 @@ static ssize_t mem_limit_show(struct device *dev,
 	u64 val;
 	struct zram *zram = dev_to_zram(dev);
 
+	deprecated_attr_warn("mem_limit");
 	down_read(&zram->init_lock);
 	val = zram->limit_pages;
 	up_read(&zram->init_lock);
@@ -159,6 +173,7 @@ static ssize_t mem_used_max_show(struct device *dev,
 	u64 val = 0;
 	struct zram *zram = dev_to_zram(dev);
 
+	deprecated_attr_warn("mem_used_max");
 	down_read(&zram->init_lock);
 	if (init_done(zram))
 		val = atomic_long_read(&zram->stats.max_used_pages);
@@ -670,8 +685,12 @@ out:
 static int zram_bvec_rw(struct zram *zram, struct bio_vec *bvec, u32 index,
 			int offset, int rw)
 {
+	unsigned long start_time = jiffies;
 	int ret;
 
+	generic_start_io_acct(rw, bvec->bv_len >> SECTOR_SHIFT,
+			&zram->disk->part0);
+
 	if (rw == READ) {
 		atomic64_inc(&zram->stats.num_reads);
 		ret = zram_bvec_read(zram, bvec, index, offset);
@@ -680,6 +699,8 @@ static int zram_bvec_rw(struct zram *zram, struct bio_vec *bvec, u32 index,
 		ret = zram_bvec_write(zram, bvec, index, offset);
 	}
 
+	generic_end_io_acct(rw, &zram->disk->part0, start_time);
+
 	if (unlikely(ret)) {
 		if (rw == READ)
 			atomic64_inc(&zram->stats.failed_reads);
@@ -1027,6 +1048,55 @@ static DEVICE_ATTR_RW(mem_used_max);
 static DEVICE_ATTR_RW(max_comp_streams);
 static DEVICE_ATTR_RW(comp_algorithm);
 
+static ssize_t io_stat_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct zram *zram = dev_to_zram(dev);
+	ssize_t ret;
+
+	down_read(&zram->init_lock);
+	ret = scnprintf(buf, PAGE_SIZE,
+			"%8llu %8llu %8llu %8llu\n",
+			(u64)atomic64_read(&zram->stats.failed_reads),
+			(u64)atomic64_read(&zram->stats.failed_writes),
+			(u64)atomic64_read(&zram->stats.invalid_io),
+			(u64)atomic64_read(&zram->stats.notify_free));
+	up_read(&zram->init_lock);
+
+	return ret;
+}
+
+static ssize_t mm_stat_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct zram *zram = dev_to_zram(dev);
+	u64 orig_size, mem_used = 0;
+	long max_used;
+	ssize_t ret;
+
+	down_read(&zram->init_lock);
+	if (init_done(zram))
+		mem_used = zs_get_total_pages(zram->meta->mem_pool);
+
+	orig_size = atomic64_read(&zram->stats.pages_stored);
+	max_used = atomic_long_read(&zram->stats.max_used_pages);
+
+	ret = scnprintf(buf, PAGE_SIZE,
+			"%8llu %8llu %8llu %8lu %8ld %8llu %8llu\n",
+			orig_size << PAGE_SHIFT,
+			(u64)atomic64_read(&zram->stats.compr_data_size),
+			mem_used << PAGE_SHIFT,
+			zram->limit_pages << PAGE_SHIFT,
+			max_used << PAGE_SHIFT,
+			(u64)atomic64_read(&zram->stats.zero_pages),
+			(u64)atomic64_read(&zram->stats.num_migrated));
+	up_read(&zram->init_lock);
+
+	return ret;
+}
+
+static DEVICE_ATTR_RO(io_stat);
+static DEVICE_ATTR_RO(mm_stat);
 ZRAM_ATTR_RO(num_reads);
 ZRAM_ATTR_RO(num_writes);
 ZRAM_ATTR_RO(failed_reads);
@@ -1054,6 +1124,8 @@ static struct attribute *zram_disk_attrs[] = {
 	&dev_attr_mem_used_max.attr,
 	&dev_attr_max_comp_streams.attr,
 	&dev_attr_comp_algorithm.attr,
+	&dev_attr_io_stat.attr,
+	&dev_attr_mm_stat.attr,
 	NULL,
 };
 
@@ -1082,6 +1154,7 @@ static int create_device(struct zram *zram, int device_id)
 	if (!zram->disk) {
 		pr_warn("Error allocating disk structure for device %d\n",
 			device_id);
+		ret = -ENOMEM;
 		goto out_free_queue;
 	}
 
diff --git a/drivers/block/zram/zram_drv.h b/drivers/block/zram/zram_drv.h
index 17056e589146..570c598f4ce9 100644
--- a/drivers/block/zram/zram_drv.h
+++ b/drivers/block/zram/zram_drv.h
@@ -84,6 +84,7 @@ struct zram_stats {
 	atomic64_t compr_data_size;	/* compressed size of pages stored */
 	atomic64_t num_reads;	/* failed + successful */
 	atomic64_t num_writes;	/* --do-- */
+	atomic64_t num_migrated;	/* no. of migrated object */
 	atomic64_t failed_reads;	/* can happen when memory is too low */
 	atomic64_t failed_writes;	/* can happen when memory is too low */
 	atomic64_t invalid_io;	/* non-page-aligned I/O requests */
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index 364f080768d0..ed5c2738bea2 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -2,9 +2,17 @@
 menu "Bluetooth device drivers"
 	depends on BT
 
+config BT_INTEL
+	tristate
+
+config BT_BCM
+	tristate
+	select FW_LOADER
+
 config BT_HCIBTUSB
 	tristate "HCI USB driver"
 	depends on USB
+	select BT_INTEL
 	help
 	  Bluetooth HCI USB driver.
 	  This driver is required if you want to use Bluetooth devices with
@@ -13,6 +21,17 @@ config BT_HCIBTUSB
 	  Say Y here to compile support for Bluetooth USB devices into the
 	  kernel or say M to compile it as module (btusb).
 
+config BT_HCIBTUSB_BCM
+	bool "Broadcom protocol support"
+	depends on BT_HCIBTUSB
+	select BT_BCM
+	default y
+	help
+	  The Broadcom protocol support enables firmware and patchram
+	  download support for Broadcom Bluetooth controllers.
+
+	  Say Y here to compile support for Broadcom protocol.
+
 config BT_HCIBTSDIO
 	tristate "HCI SDIO driver"
 	depends on MMC
@@ -62,6 +81,7 @@ config BT_HCIUART_BCSP
 config BT_HCIUART_ATH3K
 	bool "Atheros AR300x serial support"
 	depends on BT_HCIUART
+	select BT_HCIUART_H4
 	help
 	  HCIATH3K (HCI Atheros AR300x) is a serial protocol for
 	  communication between host and Atheros AR300x Bluetooth devices.
@@ -94,6 +114,27 @@ config BT_HCIUART_3WIRE
 
 	  Say Y here to compile support for Three-wire UART protocol.
 
+config BT_HCIUART_INTEL
+	bool "Intel protocol support"
+	depends on BT_HCIUART
+	select BT_INTEL
+	help
+	  The Intel protocol support enables Bluetooth HCI over serial
+	  port interface for Intel Bluetooth controllers.
+
+	  Say Y here to compile support for Intel protocol.
+
+config BT_HCIUART_BCM
+	bool "Broadcom protocol support"
+	depends on BT_HCIUART
+	select BT_HCIUART_H4
+	select BT_BCM
+	help
+	  The Broadcom protocol support enables Bluetooth HCI over serial
+	  port interface for Broadcom Bluetooth controllers.
+
+	  Say Y here to compile support for Broadcom protocol.
+
 config BT_HCIBCM203X
 	tristate "HCI BCM203x USB driver"
 	depends on USB
diff --git a/drivers/bluetooth/Makefile b/drivers/bluetooth/Makefile
index 9fe8a875a827..dd0d9c40b999 100644
--- a/drivers/bluetooth/Makefile
+++ b/drivers/bluetooth/Makefile
@@ -15,10 +15,12 @@ obj-$(CONFIG_BT_HCIBTUART)	+= btuart_cs.o
 obj-$(CONFIG_BT_HCIBTUSB)	+= btusb.o
 obj-$(CONFIG_BT_HCIBTSDIO)	+= btsdio.o
 
+obj-$(CONFIG_BT_INTEL)		+= btintel.o
 obj-$(CONFIG_BT_ATH3K)		+= ath3k.o
 obj-$(CONFIG_BT_MRVL)		+= btmrvl.o
 obj-$(CONFIG_BT_MRVL_SDIO)	+= btmrvl_sdio.o
 obj-$(CONFIG_BT_WILINK)		+= btwilink.o
+obj-$(CONFIG_BT_BCM)		+= btbcm.o
 
 btmrvl-y			:= btmrvl_main.o
 btmrvl-$(CONFIG_DEBUG_FS)	+= btmrvl_debugfs.o
@@ -29,6 +31,8 @@ hci_uart-$(CONFIG_BT_HCIUART_BCSP)	+= hci_bcsp.o
 hci_uart-$(CONFIG_BT_HCIUART_LL)	+= hci_ll.o
 hci_uart-$(CONFIG_BT_HCIUART_ATH3K)	+= hci_ath.o
 hci_uart-$(CONFIG_BT_HCIUART_3WIRE)	+= hci_h5.o
+hci_uart-$(CONFIG_BT_HCIUART_INTEL)	+= hci_intel.o
+hci_uart-$(CONFIG_BT_HCIUART_BCM)	+= hci_bcm.o
 hci_uart-objs				:= $(hci_uart-y)
 
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index de4c8499cbac..288547a3c566 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -65,6 +65,7 @@ static const struct usb_device_id ath3k_table[] = {
 	/* Atheros AR3011 with sflash firmware*/
 	{ USB_DEVICE(0x0489, 0xE027) },
 	{ USB_DEVICE(0x0489, 0xE03D) },
+	{ USB_DEVICE(0x04F2, 0xAFF1) },
 	{ USB_DEVICE(0x0930, 0x0215) },
 	{ USB_DEVICE(0x0CF3, 0x3002) },
 	{ USB_DEVICE(0x0CF3, 0xE019) },
diff --git a/drivers/bluetooth/btbcm.c b/drivers/bluetooth/btbcm.c
new file mode 100644
index 000000000000..d0741f3ed7ec
--- /dev/null
+++ b/drivers/bluetooth/btbcm.c
@@ -0,0 +1,387 @@
+/*
+ *
+ *  Bluetooth support for Broadcom devices
+ *
+ *  Copyright (C) 2015  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/firmware.h>
+#include <asm/unaligned.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btbcm.h"
+
+#define VERSION "0.1"
+
+#define BDADDR_BCM20702A0 (&(bdaddr_t) {{0x00, 0xa0, 0x02, 0x70, 0x20, 0x00}})
+
+int btbcm_check_bdaddr(struct hci_dev *hdev)
+{
+	struct hci_rp_read_bd_addr *bda;
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, HCI_OP_READ_BD_ADDR, 0, NULL,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		int err = PTR_ERR(skb);
+		BT_ERR("%s: BCM: Reading device address failed (%d)",
+		       hdev->name, err);
+		return err;
+	}
+
+	if (skb->len != sizeof(*bda)) {
+		BT_ERR("%s: BCM: Device address length mismatch", hdev->name);
+		kfree_skb(skb);
+		return -EIO;
+	}
+
+	bda = (struct hci_rp_read_bd_addr *)skb->data;
+	if (bda->status) {
+		BT_ERR("%s: BCM: Device address result failed (%02x)",
+		       hdev->name, bda->status);
+		kfree_skb(skb);
+		return -bt_to_errno(bda->status);
+	}
+
+	/* The address 00:20:70:02:A0:00 indicates a BCM20702A0 controller
+	 * with no configured address.
+	 */
+	if (!bacmp(&bda->bdaddr, BDADDR_BCM20702A0)) {
+		BT_INFO("%s: BCM: Using default device address (%pMR)",
+			hdev->name, &bda->bdaddr);
+		set_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks);
+	}
+
+	kfree_skb(skb);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btbcm_check_bdaddr);
+
+int btbcm_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
+{
+	struct sk_buff *skb;
+	int err;
+
+	skb = __hci_cmd_sync(hdev, 0xfc01, 6, bdaddr, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		err = PTR_ERR(skb);
+		BT_ERR("%s: BCM: Change address command failed (%d)",
+		       hdev->name, err);
+		return err;
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btbcm_set_bdaddr);
+
+static int btbcm_reset(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, HCI_OP_RESET, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		int err = PTR_ERR(skb);
+		BT_ERR("%s: BCM: Reset failed (%d)", hdev->name, err);
+		return err;
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+
+static struct sk_buff *btbcm_read_local_version(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: BCM: Reading local version info failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return skb;
+	}
+
+	if (skb->len != sizeof(struct hci_rp_read_local_version)) {
+		BT_ERR("%s: BCM: Local version length mismatch", hdev->name);
+		kfree_skb(skb);
+		return ERR_PTR(-EIO);
+	}
+
+	return skb;
+}
+
+static struct sk_buff *btbcm_read_verbose_config(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, 0xfc79, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: BCM: Read verbose config info failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return skb;
+	}
+
+	if (skb->len != 7) {
+		BT_ERR("%s: BCM: Verbose config length mismatch", hdev->name);
+		kfree_skb(skb);
+		return ERR_PTR(-EIO);
+	}
+
+	return skb;
+}
+
+static struct sk_buff *btbcm_read_usb_product(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, 0xfc5a, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: BCM: Read USB product info failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return skb;
+	}
+
+	if (skb->len != 5) {
+		BT_ERR("%s: BCM: USB product length mismatch", hdev->name);
+		kfree_skb(skb);
+		return ERR_PTR(-EIO);
+	}
+
+	return skb;
+}
+
+static const struct {
+	u16 subver;
+	const char *name;
+} bcm_uart_subver_table[] = {
+	{ 0x410e, "BCM43341B0"	},	/* 002.001.014 */
+	{ }
+};
+
+static const struct {
+	u16 subver;
+	const char *name;
+} bcm_usb_subver_table[] = {
+	{ 0x210b, "BCM43142A0"	},	/* 001.001.011 */
+	{ 0x2112, "BCM4314A0"	},	/* 001.001.018 */
+	{ 0x2118, "BCM20702A0"	},	/* 001.001.024 */
+	{ 0x2126, "BCM4335A0"	},	/* 001.001.038 */
+	{ 0x220e, "BCM20702A1"	},	/* 001.002.014 */
+	{ 0x230f, "BCM4354A2"	},	/* 001.003.015 */
+	{ 0x4106, "BCM4335B0"	},	/* 002.001.006 */
+	{ 0x410e, "BCM20702B0"	},	/* 002.001.014 */
+	{ 0x6109, "BCM4335C0"	},	/* 003.001.009 */
+	{ 0x610c, "BCM4354"	},	/* 003.001.012 */
+	{ }
+};
+
+int btbcm_setup_patchram(struct hci_dev *hdev)
+{
+	const struct hci_command_hdr *cmd;
+	const struct firmware *fw;
+	const u8 *fw_ptr;
+	size_t fw_size;
+	char fw_name[64];
+	u16 opcode, subver, rev, pid, vid;
+	const char *hw_name = NULL;
+	struct sk_buff *skb;
+	struct hci_rp_read_local_version *ver;
+	int i, err;
+
+	/* Reset */
+	err = btbcm_reset(hdev);
+	if (err)
+		return err;
+
+	/* Read Local Version Info */
+	skb = btbcm_read_local_version(hdev);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	ver = (struct hci_rp_read_local_version *)skb->data;
+	rev = le16_to_cpu(ver->hci_rev);
+	subver = le16_to_cpu(ver->lmp_subver);
+	kfree_skb(skb);
+
+	/* Read Verbose Config Version Info */
+	skb = btbcm_read_verbose_config(hdev);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	BT_INFO("%s: BCM: chip id %u", hdev->name, skb->data[1]);
+	kfree_skb(skb);
+
+	switch ((rev & 0xf000) >> 12) {
+	case 0:
+		for (i = 0; bcm_uart_subver_table[i].name; i++) {
+			if (subver == bcm_uart_subver_table[i].subver) {
+				hw_name = bcm_uart_subver_table[i].name;
+				break;
+			}
+		}
+
+		snprintf(fw_name, sizeof(fw_name), "brcm/%s.hcd",
+			 hw_name ? : "BCM");
+		break;
+	case 1:
+	case 2:
+		/* Read USB Product Info */
+		skb = btbcm_read_usb_product(hdev);
+		if (IS_ERR(skb))
+			return PTR_ERR(skb);
+
+		vid = get_unaligned_le16(skb->data + 1);
+		pid = get_unaligned_le16(skb->data + 3);
+		kfree_skb(skb);
+
+		for (i = 0; bcm_usb_subver_table[i].name; i++) {
+			if (subver == bcm_usb_subver_table[i].subver) {
+				hw_name = bcm_usb_subver_table[i].name;
+				break;
+			}
+		}
+
+		snprintf(fw_name, sizeof(fw_name), "brcm/%s-%4.4x-%4.4x.hcd",
+			 hw_name ? : "BCM", vid, pid);
+		break;
+	default:
+		return 0;
+	}
+
+	BT_INFO("%s: %s (%3.3u.%3.3u.%3.3u) build %4.4u", hdev->name,
+		hw_name ? : "BCM", (subver & 0x7000) >> 13,
+		(subver & 0x1f00) >> 8, (subver & 0x00ff), rev & 0x0fff);
+
+	err = request_firmware(&fw, fw_name, &hdev->dev);
+	if (err < 0) {
+		BT_INFO("%s: BCM: patch %s not found", hdev->name, fw_name);
+		return 0;
+	}
+
+	/* Start Download */
+	skb = __hci_cmd_sync(hdev, 0xfc2e, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		err = PTR_ERR(skb);
+		BT_ERR("%s: BCM: Download Minidrv command failed (%d)",
+		       hdev->name, err);
+		goto reset;
+	}
+	kfree_skb(skb);
+
+	/* 50 msec delay after Download Minidrv completes */
+	msleep(50);
+
+	fw_ptr = fw->data;
+	fw_size = fw->size;
+
+	while (fw_size >= sizeof(*cmd)) {
+		const u8 *cmd_param;
+
+		cmd = (struct hci_command_hdr *)fw_ptr;
+		fw_ptr += sizeof(*cmd);
+		fw_size -= sizeof(*cmd);
+
+		if (fw_size < cmd->plen) {
+			BT_ERR("%s: BCM: patch %s is corrupted", hdev->name,
+			       fw_name);
+			err = -EINVAL;
+			goto reset;
+		}
+
+		cmd_param = fw_ptr;
+		fw_ptr += cmd->plen;
+		fw_size -= cmd->plen;
+
+		opcode = le16_to_cpu(cmd->opcode);
+
+		skb = __hci_cmd_sync(hdev, opcode, cmd->plen, cmd_param,
+				     HCI_INIT_TIMEOUT);
+		if (IS_ERR(skb)) {
+			err = PTR_ERR(skb);
+			BT_ERR("%s: BCM: patch command %04x failed (%d)",
+			       hdev->name, opcode, err);
+			goto reset;
+		}
+		kfree_skb(skb);
+	}
+
+	/* 250 msec delay after Launch Ram completes */
+	msleep(250);
+
+reset:
+	/* Reset */
+	err = btbcm_reset(hdev);
+	if (err)
+		goto done;
+
+	/* Read Local Version Info */
+	skb = btbcm_read_local_version(hdev);
+	if (IS_ERR(skb)) {
+		err = PTR_ERR(skb);
+		goto done;
+	}
+
+	ver = (struct hci_rp_read_local_version *)skb->data;
+	rev = le16_to_cpu(ver->hci_rev);
+	subver = le16_to_cpu(ver->lmp_subver);
+	kfree_skb(skb);
+
+	BT_INFO("%s: %s (%3.3u.%3.3u.%3.3u) build %4.4u", hdev->name,
+		hw_name ? : "BCM", (subver & 0x7000) >> 13,
+		(subver & 0x1f00) >> 8, (subver & 0x00ff), rev & 0x0fff);
+
+	btbcm_check_bdaddr(hdev);
+
+	set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
+
+done:
+	release_firmware(fw);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(btbcm_setup_patchram);
+
+int btbcm_setup_apple(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	/* Read Verbose Config Version Info */
+	skb = btbcm_read_verbose_config(hdev);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	BT_INFO("%s: BCM: chip id %u build %4.4u", hdev->name, skb->data[1],
+		get_unaligned_le16(skb->data + 5));
+	kfree_skb(skb);
+
+	set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btbcm_setup_apple);
+
+MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
+MODULE_DESCRIPTION("Bluetooth support for Broadcom devices ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/bluetooth/btbcm.h b/drivers/bluetooth/btbcm.h
new file mode 100644
index 000000000000..34268ae3eb46
--- /dev/null
+++ b/drivers/bluetooth/btbcm.h
@@ -0,0 +1,54 @@
+/*
+ *
+ *  Bluetooth support for Broadcom devices
+ *
+ *  Copyright (C) 2015  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#if IS_ENABLED(CONFIG_BT_BCM)
+
+int btbcm_check_bdaddr(struct hci_dev *hdev);
+int btbcm_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr);
+
+int btbcm_setup_patchram(struct hci_dev *hdev);
+int btbcm_setup_apple(struct hci_dev *hdev);
+
+#else
+
+static inline int btbcm_check_bdaddr(struct hci_dev *hdev)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int btbcm_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int btbcm_setup_patchram(struct hci_dev *hdev)
+{
+	return 0;
+}
+
+static inline int btbcm_setup_apple(struct hci_dev *hdev)
+{
+	return 0;
+}
+
+#endif
diff --git a/drivers/bluetooth/btintel.c b/drivers/bluetooth/btintel.c
new file mode 100644
index 000000000000..2d43d4279b00
--- /dev/null
+++ b/drivers/bluetooth/btintel.c
@@ -0,0 +1,101 @@
+/*
+ *
+ *  Bluetooth support for Intel devices
+ *
+ *  Copyright (C) 2015  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/module.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btintel.h"
+
+#define VERSION "0.1"
+
+#define BDADDR_INTEL (&(bdaddr_t) {{0x00, 0x8b, 0x9e, 0x19, 0x03, 0x00}})
+
+int btintel_check_bdaddr(struct hci_dev *hdev)
+{
+	struct hci_rp_read_bd_addr *bda;
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, HCI_OP_READ_BD_ADDR, 0, NULL,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		int err = PTR_ERR(skb);
+		BT_ERR("%s: Reading Intel device address failed (%d)",
+		       hdev->name, err);
+		return err;
+	}
+
+	if (skb->len != sizeof(*bda)) {
+		BT_ERR("%s: Intel device address length mismatch", hdev->name);
+		kfree_skb(skb);
+		return -EIO;
+	}
+
+	bda = (struct hci_rp_read_bd_addr *)skb->data;
+	if (bda->status) {
+		BT_ERR("%s: Intel device address result failed (%02x)",
+		       hdev->name, bda->status);
+		kfree_skb(skb);
+		return -bt_to_errno(bda->status);
+	}
+
+	/* For some Intel based controllers, the default Bluetooth device
+	 * address 00:03:19:9E:8B:00 can be found. These controllers are
+	 * fully operational, but have the danger of duplicate addresses
+	 * and that in turn can cause problems with Bluetooth operation.
+	 */
+	if (!bacmp(&bda->bdaddr, BDADDR_INTEL)) {
+		BT_ERR("%s: Found Intel default device address (%pMR)",
+		       hdev->name, &bda->bdaddr);
+		set_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks);
+	}
+
+	kfree_skb(skb);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btintel_check_bdaddr);
+
+int btintel_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
+{
+	struct sk_buff *skb;
+	int err;
+
+	skb = __hci_cmd_sync(hdev, 0xfc31, 6, bdaddr, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		err = PTR_ERR(skb);
+		BT_ERR("%s: Changing Intel device address failed (%d)",
+		       hdev->name, err);
+		return err;
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btintel_set_bdaddr);
+
+MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
+MODULE_DESCRIPTION("Bluetooth support for Intel devices ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/bluetooth/btintel.h b/drivers/bluetooth/btintel.h
new file mode 100644
index 000000000000..4bda6ab34f60
--- /dev/null
+++ b/drivers/bluetooth/btintel.h
@@ -0,0 +1,89 @@
+/*
+ *
+ *  Bluetooth support for Intel devices
+ *
+ *  Copyright (C) 2015  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+struct intel_version {
+	u8 status;
+	u8 hw_platform;
+	u8 hw_variant;
+	u8 hw_revision;
+	u8 fw_variant;
+	u8 fw_revision;
+	u8 fw_build_num;
+	u8 fw_build_ww;
+	u8 fw_build_yy;
+	u8 fw_patch_num;
+} __packed;
+
+struct intel_boot_params {
+	__u8     status;
+	__u8     otp_format;
+	__u8     otp_content;
+	__u8     otp_patch;
+	__le16   dev_revid;
+	__u8     secure_boot;
+	__u8     key_from_hdr;
+	__u8     key_type;
+	__u8     otp_lock;
+	__u8     api_lock;
+	__u8     debug_lock;
+	bdaddr_t otp_bdaddr;
+	__u8     min_fw_build_nn;
+	__u8     min_fw_build_cw;
+	__u8     min_fw_build_yy;
+	__u8     limited_cce;
+	__u8     unlocked_state;
+} __packed;
+
+struct intel_bootup {
+	__u8     zero;
+	__u8     num_cmds;
+	__u8     source;
+	__u8     reset_type;
+	__u8     reset_reason;
+	__u8     ddc_status;
+} __packed;
+
+struct intel_secure_send_result {
+	__u8     result;
+	__le16   opcode;
+	__u8     status;
+} __packed;
+
+#if IS_ENABLED(CONFIG_BT_INTEL)
+
+int btintel_check_bdaddr(struct hci_dev *hdev);
+int btintel_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr);
+
+#else
+
+static inline int btintel_check_bdaddr(struct hci_dev *hdev)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int btintel_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
+{
+	return -EOPNOTSUPP;
+}
+
+#endif
diff --git a/drivers/bluetooth/btmrvl_drv.h b/drivers/bluetooth/btmrvl_drv.h
index e75f8ee2512c..086f0ec89580 100644
--- a/drivers/bluetooth/btmrvl_drv.h
+++ b/drivers/bluetooth/btmrvl_drv.h
@@ -111,6 +111,7 @@ struct btmrvl_private {
 
 /* Vendor specific Bluetooth commands */
 #define BT_CMD_PSCAN_WIN_REPORT_ENABLE	0xFC03
+#define BT_CMD_ROUTE_SCO_TO_HOST	0xFC1D
 #define BT_CMD_SET_BDADDR		0xFC22
 #define BT_CMD_AUTO_SLEEP_MODE		0xFC23
 #define BT_CMD_HOST_SLEEP_CONFIG	0xFC59
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index 413597789c61..de05deb444ce 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -230,6 +230,18 @@ int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, u8 subcmd)
 }
 EXPORT_SYMBOL_GPL(btmrvl_send_module_cfg_cmd);
 
+static int btmrvl_enable_sco_routing_to_host(struct btmrvl_private *priv)
+{
+	int ret;
+	u8 subcmd = 0;
+
+	ret = btmrvl_send_sync_cmd(priv, BT_CMD_ROUTE_SCO_TO_HOST, &subcmd, 1);
+	if (ret)
+		BT_ERR("BT_CMD_ROUTE_SCO_TO_HOST command failed: %#x", ret);
+
+	return ret;
+}
+
 int btmrvl_pscan_window_reporting(struct btmrvl_private *priv, u8 subcmd)
 {
 	struct btmrvl_sdio_card *card = priv->btmrvl_dev.card;
@@ -558,6 +570,8 @@ static int btmrvl_setup(struct hci_dev *hdev)
 
 	btmrvl_check_device_tree(priv);
 
+	btmrvl_enable_sco_routing_to_host(priv);
+
 	btmrvl_pscan_window_reporting(priv, 0x01);
 
 	priv->btmrvl_dev.psmode = 1;
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 8bfc4c2bba87..de7b236eeae7 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -28,7 +28,10 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 
-#define VERSION "0.7"
+#include "btintel.h"
+#include "btbcm.h"
+
+#define VERSION "0.8"
 
 static bool disable_scofix;
 static bool force_scofix;
@@ -52,6 +55,8 @@ static struct usb_driver btusb_driver;
 #define BTUSB_SWAVE		0x1000
 #define BTUSB_INTEL_NEW		0x2000
 #define BTUSB_AMP		0x4000
+#define BTUSB_QCA_ROME		0x8000
+#define BTUSB_BCM_APPLE		0x10000
 
 static const struct usb_device_id btusb_table[] = {
 	/* Generic Bluetooth USB device */
@@ -61,7 +66,8 @@ static const struct usb_device_id btusb_table[] = {
 	{ USB_DEVICE_INFO(0xe0, 0x01, 0x04), .driver_info = BTUSB_AMP },
 
 	/* Apple-specific (Broadcom) devices */
-	{ USB_VENDOR_AND_INTERFACE_INFO(0x05ac, 0xff, 0x01, 0x01) },
+	{ USB_VENDOR_AND_INTERFACE_INFO(0x05ac, 0xff, 0x01, 0x01),
+	  .driver_info = BTUSB_BCM_APPLE },
 
 	/* MediaTek MT76x0E */
 	{ USB_DEVICE(0x0e8d, 0x763f) },
@@ -107,13 +113,7 @@ static const struct usb_device_id btusb_table[] = {
 	{ USB_DEVICE(0x0c10, 0x0000) },
 
 	/* Broadcom BCM20702A0 */
-	{ USB_DEVICE(0x0489, 0xe042) },
-	{ USB_DEVICE(0x04ca, 0x2003) },
-	{ USB_DEVICE(0x0b05, 0x17b5) },
-	{ USB_DEVICE(0x0b05, 0x17cb) },
 	{ USB_DEVICE(0x413c, 0x8197) },
-	{ USB_DEVICE(0x13d3, 0x3404),
-	  .driver_info = BTUSB_BCM_PATCHRAM },
 
 	/* Broadcom BCM20702B0 (Dynex/Insignia) */
 	{ USB_DEVICE(0x19ff, 0x0239), .driver_info = BTUSB_BCM_PATCHRAM },
@@ -135,10 +135,12 @@ static const struct usb_device_id btusb_table[] = {
 	  .driver_info = BTUSB_BCM_PATCHRAM },
 
 	/* Belkin F8065bf - Broadcom based */
-	{ USB_VENDOR_AND_INTERFACE_INFO(0x050d, 0xff, 0x01, 0x01) },
+	{ USB_VENDOR_AND_INTERFACE_INFO(0x050d, 0xff, 0x01, 0x01),
+	  .driver_info = BTUSB_BCM_PATCHRAM },
 
 	/* IMC Networks - Broadcom based */
-	{ USB_VENDOR_AND_INTERFACE_INFO(0x13d3, 0xff, 0x01, 0x01) },
+	{ USB_VENDOR_AND_INTERFACE_INFO(0x13d3, 0xff, 0x01, 0x01),
+	  .driver_info = BTUSB_BCM_PATCHRAM },
 
 	/* Intel Bluetooth USB Bootloader (RAM module) */
 	{ USB_DEVICE(0x8087, 0x0a5a),
@@ -159,6 +161,7 @@ static const struct usb_device_id blacklist_table[] = {
 	/* Atheros 3011 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xe027), .driver_info = BTUSB_IGNORE },
 	{ USB_DEVICE(0x0489, 0xe03d), .driver_info = BTUSB_IGNORE },
+	{ USB_DEVICE(0x04f2, 0xaff1), .driver_info = BTUSB_IGNORE },
 	{ USB_DEVICE(0x0930, 0x0215), .driver_info = BTUSB_IGNORE },
 	{ USB_DEVICE(0x0cf3, 0x3002), .driver_info = BTUSB_IGNORE },
 	{ USB_DEVICE(0x0cf3, 0xe019), .driver_info = BTUSB_IGNORE },
@@ -212,6 +215,10 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x0489, 0xe036), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe03c), .driver_info = BTUSB_ATH3012 },
 
+	/* QCA ROME chipset */
+	{ USB_DEVICE(0x0cf3, 0xe300), .driver_info = BTUSB_QCA_ROME },
+	{ USB_DEVICE(0x0cf3, 0xe360), .driver_info = BTUSB_QCA_ROME },
+
 	/* Broadcom BCM2035 */
 	{ USB_DEVICE(0x0a5c, 0x2009), .driver_info = BTUSB_BCM92035 },
 	{ USB_DEVICE(0x0a5c, 0x200a), .driver_info = BTUSB_WRONG_SCO_MTU },
@@ -337,17 +344,9 @@ struct btusb_data {
 
 	int (*recv_event)(struct hci_dev *hdev, struct sk_buff *skb);
 	int (*recv_bulk)(struct btusb_data *data, void *buffer, int count);
-};
 
-static int btusb_wait_on_bit_timeout(void *word, int bit, unsigned long timeout,
-				     unsigned mode)
-{
-	might_sleep();
-	if (!test_bit(bit, word))
-		return 0;
-	return out_of_line_wait_on_bit_timeout(word, bit, bit_wait_timeout,
-					       mode, timeout);
-}
+	int (*setup_on_usb)(struct hci_dev *hdev);
+};
 
 static inline void btusb_free_frags(struct btusb_data *data)
 {
@@ -888,6 +887,15 @@ static int btusb_open(struct hci_dev *hdev)
 
 	BT_DBG("%s", hdev->name);
 
+	/* Patching USB firmware files prior to starting any URBs of HCI path
+	 * It is more safe to use USB bulk channel for downloading USB patch
+	 */
+	if (data->setup_on_usb) {
+		err = data->setup_on_usb(hdev);
+		if (err <0)
+			return err;
+	}
+
 	err = usb_autopm_get_interface(data->intf);
 	if (err < 0)
 		return err;
@@ -1263,6 +1271,28 @@ static void btusb_waker(struct work_struct *work)
 	usb_autopm_put_interface(data->intf);
 }
 
+static struct sk_buff *btusb_read_local_version(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return skb;
+	}
+
+	if (skb->len != sizeof(struct hci_rp_read_local_version)) {
+		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION event length mismatch",
+		       hdev->name);
+		kfree_skb(skb);
+		return ERR_PTR(-EIO);
+	}
+
+	return skb;
+}
+
 static int btusb_setup_bcm92035(struct hci_dev *hdev)
 {
 	struct sk_buff *skb;
@@ -1287,12 +1317,9 @@ static int btusb_setup_csr(struct hci_dev *hdev)
 
 	BT_DBG("%s", hdev->name);
 
-	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
-			     HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("Reading local version failed (%ld)", -PTR_ERR(skb));
+	skb = btusb_read_local_version(hdev);
+	if (IS_ERR(skb))
 		return -PTR_ERR(skb);
-	}
 
 	rp = (struct hci_rp_read_local_version *)skb->data;
 
@@ -1318,39 +1345,6 @@ static int btusb_setup_csr(struct hci_dev *hdev)
 	return ret;
 }
 
-struct intel_version {
-	u8 status;
-	u8 hw_platform;
-	u8 hw_variant;
-	u8 hw_revision;
-	u8 fw_variant;
-	u8 fw_revision;
-	u8 fw_build_num;
-	u8 fw_build_ww;
-	u8 fw_build_yy;
-	u8 fw_patch_num;
-} __packed;
-
-struct intel_boot_params {
-	__u8     status;
-	__u8     otp_format;
-	__u8     otp_content;
-	__u8     otp_patch;
-	__le16   dev_revid;
-	__u8     secure_boot;
-	__u8     key_from_hdr;
-	__u8     key_type;
-	__u8     otp_lock;
-	__u8     api_lock;
-	__u8     debug_lock;
-	bdaddr_t otp_bdaddr;
-	__u8     min_fw_build_nn;
-	__u8     min_fw_build_cw;
-	__u8     min_fw_build_yy;
-	__u8     limited_cce;
-	__u8     unlocked_state;
-} __packed;
-
 static const struct firmware *btusb_setup_intel_get_fw(struct hci_dev *hdev,
 						       struct intel_version *ver)
 {
@@ -1507,51 +1501,6 @@ static int btusb_setup_intel_patching(struct hci_dev *hdev,
 	return 0;
 }
 
-#define BDADDR_INTEL (&(bdaddr_t) {{0x00, 0x8b, 0x9e, 0x19, 0x03, 0x00}})
-
-static int btusb_check_bdaddr_intel(struct hci_dev *hdev)
-{
-	struct sk_buff *skb;
-	struct hci_rp_read_bd_addr *rp;
-
-	skb = __hci_cmd_sync(hdev, HCI_OP_READ_BD_ADDR, 0, NULL,
-			     HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s reading Intel device address failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return PTR_ERR(skb);
-	}
-
-	if (skb->len != sizeof(*rp)) {
-		BT_ERR("%s Intel device address length mismatch", hdev->name);
-		kfree_skb(skb);
-		return -EIO;
-	}
-
-	rp = (struct hci_rp_read_bd_addr *)skb->data;
-	if (rp->status) {
-		BT_ERR("%s Intel device address result failed (%02x)",
-		       hdev->name, rp->status);
-		kfree_skb(skb);
-		return -bt_to_errno(rp->status);
-	}
-
-	/* For some Intel based controllers, the default Bluetooth device
-	 * address 00:03:19:9E:8B:00 can be found. These controllers are
-	 * fully operational, but have the danger of duplicate addresses
-	 * and that in turn can cause problems with Bluetooth operation.
-	 */
-	if (!bacmp(&rp->bdaddr, BDADDR_INTEL)) {
-		BT_ERR("%s found Intel default device address (%pMR)",
-		       hdev->name, &rp->bdaddr);
-		set_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks);
-	}
-
-	kfree_skb(skb);
-
-	return 0;
-}
-
 static int btusb_setup_intel(struct hci_dev *hdev)
 {
 	struct sk_buff *skb;
@@ -1624,7 +1573,7 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 		BT_INFO("%s: Intel device is already patched. patch num: %02x",
 			hdev->name, ver->fw_patch_num);
 		kfree_skb(skb);
-		btusb_check_bdaddr_intel(hdev);
+		btintel_check_bdaddr(hdev);
 		return 0;
 	}
 
@@ -1637,7 +1586,7 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	fw = btusb_setup_intel_get_fw(hdev, ver);
 	if (!fw) {
 		kfree_skb(skb);
-		btusb_check_bdaddr_intel(hdev);
+		btintel_check_bdaddr(hdev);
 		return 0;
 	}
 	fw_ptr = fw->data;
@@ -1718,7 +1667,7 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	BT_INFO("%s: Intel Bluetooth firmware patch completed and activated",
 		hdev->name);
 
-	btusb_check_bdaddr_intel(hdev);
+	btintel_check_bdaddr(hdev);
 	return 0;
 
 exit_mfg_disable:
@@ -1734,7 +1683,7 @@ exit_mfg_disable:
 
 	BT_INFO("%s: Intel Bluetooth firmware patch completed", hdev->name);
 
-	btusb_check_bdaddr_intel(hdev);
+	btintel_check_bdaddr(hdev);
 	return 0;
 
 exit_mfg_deactivate:
@@ -1755,7 +1704,7 @@ exit_mfg_deactivate:
 	BT_INFO("%s: Intel Bluetooth firmware patch completed and deactivated",
 		hdev->name);
 
-	btusb_check_bdaddr_intel(hdev);
+	btintel_check_bdaddr(hdev);
 	return 0;
 }
 
@@ -1797,6 +1746,38 @@ static int btusb_recv_bulk_intel(struct btusb_data *data, void *buffer,
 	return btusb_recv_bulk(data, buffer, count);
 }
 
+static void btusb_intel_bootup(struct btusb_data *data, const void *ptr,
+			       unsigned int len)
+{
+	const struct intel_bootup *evt = ptr;
+
+	if (len != sizeof(*evt))
+		return;
+
+	if (test_and_clear_bit(BTUSB_BOOTING, &data->flags)) {
+		smp_mb__after_atomic();
+		wake_up_bit(&data->flags, BTUSB_BOOTING);
+	}
+}
+
+static void btusb_intel_secure_send_result(struct btusb_data *data,
+					   const void *ptr, unsigned int len)
+{
+	const struct intel_secure_send_result *evt = ptr;
+
+	if (len != sizeof(*evt))
+		return;
+
+	if (evt->result)
+		set_bit(BTUSB_FIRMWARE_FAILED, &data->flags);
+
+	if (test_and_clear_bit(BTUSB_DOWNLOADING, &data->flags) &&
+	    test_bit(BTUSB_FIRMWARE_LOADED, &data->flags)) {
+		smp_mb__after_atomic();
+		wake_up_bit(&data->flags, BTUSB_DOWNLOADING);
+	}
+}
+
 static int btusb_recv_event_intel(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct btusb_data *data = hci_get_drvdata(hdev);
@@ -1804,32 +1785,27 @@ static int btusb_recv_event_intel(struct hci_dev *hdev, struct sk_buff *skb)
 	if (test_bit(BTUSB_BOOTLOADER, &data->flags)) {
 		struct hci_event_hdr *hdr = (void *)skb->data;
 
-		/* When the firmware loading completes the device sends
-		 * out a vendor specific event indicating the result of
-		 * the firmware loading.
-		 */
-		if (skb->len == 7 && hdr->evt == 0xff && hdr->plen == 0x05 &&
-		    skb->data[2] == 0x06) {
-			if (skb->data[3] != 0x00)
-				test_bit(BTUSB_FIRMWARE_FAILED, &data->flags);
-
-			if (test_and_clear_bit(BTUSB_DOWNLOADING,
-					       &data->flags) &&
-			    test_bit(BTUSB_FIRMWARE_LOADED, &data->flags)) {
-				smp_mb__after_atomic();
-				wake_up_bit(&data->flags, BTUSB_DOWNLOADING);
-			}
-		}
-
-		/* When switching to the operational firmware the device
-		 * sends a vendor specific event indicating that the bootup
-		 * completed.
-		 */
-		if (skb->len == 9 && hdr->evt == 0xff && hdr->plen == 0x07 &&
-		    skb->data[2] == 0x02) {
-			if (test_and_clear_bit(BTUSB_BOOTING, &data->flags)) {
-				smp_mb__after_atomic();
-				wake_up_bit(&data->flags, BTUSB_BOOTING);
+		if (skb->len > HCI_EVENT_HDR_SIZE && hdr->evt == 0xff &&
+		    hdr->plen > 0) {
+			const void *ptr = skb->data + HCI_EVENT_HDR_SIZE + 1;
+			unsigned int len = skb->len - HCI_EVENT_HDR_SIZE - 1;
+
+			switch (skb->data[2]) {
+			case 0x02:
+				/* When switching to the operational firmware
+				 * the device sends a vendor specific event
+				 * indicating that the bootup completed.
+				 */
+				btusb_intel_bootup(data, ptr, len);
+				break;
+			case 0x06:
+				/* When the firmware loading completes the
+				 * device sends out a vendor specific event
+				 * indicating the result of the firmware
+				 * loading.
+				 */
+				btusb_intel_secure_send_result(data, ptr, len);
+				break;
 			}
 		}
 	}
@@ -2031,7 +2007,7 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	if (ver->fw_variant == 0x23) {
 		kfree_skb(skb);
 		clear_bit(BTUSB_BOOTLOADER, &data->flags);
-		btusb_check_bdaddr_intel(hdev);
+		btintel_check_bdaddr(hdev);
 		return 0;
 	}
 
@@ -2197,9 +2173,9 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	 * and thus just timeout if that happens and fail the setup
 	 * of this device.
 	 */
-	err = btusb_wait_on_bit_timeout(&data->flags, BTUSB_DOWNLOADING,
-					msecs_to_jiffies(5000),
-					TASK_INTERRUPTIBLE);
+	err = wait_on_bit_timeout(&data->flags, BTUSB_DOWNLOADING,
+				  TASK_INTERRUPTIBLE,
+				  msecs_to_jiffies(5000));
 	if (err == 1) {
 		BT_ERR("%s: Firmware loading interrupted", hdev->name);
 		err = -EINTR;
@@ -2250,9 +2226,9 @@ done:
 	 */
 	BT_INFO("%s: Waiting for device to boot", hdev->name);
 
-	err = btusb_wait_on_bit_timeout(&data->flags, BTUSB_BOOTING,
-					msecs_to_jiffies(1000),
-					TASK_INTERRUPTIBLE);
+	err = wait_on_bit_timeout(&data->flags, BTUSB_BOOTING,
+				  TASK_INTERRUPTIBLE,
+				  msecs_to_jiffies(1000));
 
 	if (err == 1) {
 		BT_ERR("%s: Device boot interrupted", hdev->name);
@@ -2315,15 +2291,19 @@ static void btusb_hw_error_intel(struct hci_dev *hdev, u8 code)
 	kfree_skb(skb);
 }
 
-static int btusb_set_bdaddr_intel(struct hci_dev *hdev, const bdaddr_t *bdaddr)
+static int btusb_shutdown_intel(struct hci_dev *hdev)
 {
 	struct sk_buff *skb;
 	long ret;
 
-	skb = __hci_cmd_sync(hdev, 0xfc31, 6, bdaddr, HCI_INIT_TIMEOUT);
+	/* Some platforms have an issue with BT LED when the interface is
+	 * down or BT radio is turned off, which takes 5 seconds to BT LED
+	 * goes off. This command turns off the BT LED immediately.
+	 */
+	skb = __hci_cmd_sync(hdev, 0xfc3f, 0, NULL, HCI_INIT_TIMEOUT);
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
-		BT_ERR("%s: changing Intel device address failed (%ld)",
+		BT_ERR("%s: turning off Intel device LED failed (%ld)",
 		       hdev->name, ret);
 		return ret;
 	}
@@ -2355,232 +2335,279 @@ static int btusb_set_bdaddr_marvell(struct hci_dev *hdev,
 	return 0;
 }
 
-#define BDADDR_BCM20702A0 (&(bdaddr_t) {{0x00, 0xa0, 0x02, 0x70, 0x20, 0x00}})
-
-static int btusb_setup_bcm_patchram(struct hci_dev *hdev)
+static int btusb_set_bdaddr_ath3012(struct hci_dev *hdev,
+				    const bdaddr_t *bdaddr)
 {
-	struct btusb_data *data = hci_get_drvdata(hdev);
-	struct usb_device *udev = data->udev;
-	char fw_name[64];
-	const struct firmware *fw;
-	const u8 *fw_ptr;
-	size_t fw_size;
-	const struct hci_command_hdr *cmd;
-	const u8 *cmd_param;
-	u16 opcode;
 	struct sk_buff *skb;
-	struct hci_rp_read_local_version *ver;
-	struct hci_rp_read_bd_addr *bda;
+	u8 buf[10];
 	long ret;
 
-	snprintf(fw_name, sizeof(fw_name), "brcm/%s-%04x-%04x.hcd",
-		 udev->product ? udev->product : "BCM",
-		 le16_to_cpu(udev->descriptor.idVendor),
-		 le16_to_cpu(udev->descriptor.idProduct));
-
-	ret = request_firmware(&fw, fw_name, &hdev->dev);
-	if (ret < 0) {
-		BT_INFO("%s: BCM: patch %s not found", hdev->name, fw_name);
-		return 0;
-	}
+	buf[0] = 0x01;
+	buf[1] = 0x01;
+	buf[2] = 0x00;
+	buf[3] = sizeof(bdaddr_t);
+	memcpy(buf + 4, bdaddr, sizeof(bdaddr_t));
 
-	/* Reset */
-	skb = __hci_cmd_sync(hdev, HCI_OP_RESET, 0, NULL, HCI_INIT_TIMEOUT);
+	skb = __hci_cmd_sync(hdev, 0xfc0b, sizeof(buf), buf, HCI_INIT_TIMEOUT);
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
-		BT_ERR("%s: HCI_OP_RESET failed (%ld)", hdev->name, ret);
-		goto done;
+		BT_ERR("%s: Change address command failed (%ld)",
+		       hdev->name, ret);
+		return ret;
 	}
 	kfree_skb(skb);
 
-	/* Read Local Version Info */
-	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
-			     HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		ret = PTR_ERR(skb);
-		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION failed (%ld)",
-		       hdev->name, ret);
-		goto done;
-	}
+	return 0;
+}
 
-	if (skb->len != sizeof(*ver)) {
-		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION event length mismatch",
-		       hdev->name);
-		kfree_skb(skb);
-		ret = -EIO;
-		goto done;
-	}
+#define QCA_DFU_PACKET_LEN	4096
 
-	ver = (struct hci_rp_read_local_version *)skb->data;
-	BT_INFO("%s: BCM: patching hci_ver=%02x hci_rev=%04x lmp_ver=%02x "
-		"lmp_subver=%04x", hdev->name, ver->hci_ver, ver->hci_rev,
-		ver->lmp_ver, ver->lmp_subver);
-	kfree_skb(skb);
+#define QCA_GET_TARGET_VERSION	0x09
+#define QCA_CHECK_STATUS	0x05
+#define QCA_DFU_DOWNLOAD	0x01
 
-	/* Start Download */
-	skb = __hci_cmd_sync(hdev, 0xfc2e, 0, NULL, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		ret = PTR_ERR(skb);
-		BT_ERR("%s: BCM: Download Minidrv command failed (%ld)",
-		       hdev->name, ret);
-		goto reset_fw;
-	}
-	kfree_skb(skb);
+#define QCA_SYSCFG_UPDATED	0x40
+#define QCA_PATCH_UPDATED	0x80
+#define QCA_DFU_TIMEOUT		3000
 
-	/* 50 msec delay after Download Minidrv completes */
-	msleep(50);
+struct qca_version {
+	__le32	rom_version;
+	__le32	patch_version;
+	__le32	ram_version;
+	__le32	ref_clock;
+	__u8	reserved[4];
+} __packed;
 
-	fw_ptr = fw->data;
-	fw_size = fw->size;
-
-	while (fw_size >= sizeof(*cmd)) {
-		cmd = (struct hci_command_hdr *)fw_ptr;
-		fw_ptr += sizeof(*cmd);
-		fw_size -= sizeof(*cmd);
-
-		if (fw_size < cmd->plen) {
-			BT_ERR("%s: BCM: patch %s is corrupted",
-			       hdev->name, fw_name);
-			ret = -EINVAL;
-			goto reset_fw;
-		}
+struct qca_rampatch_version {
+	__le16	rom_version;
+	__le16	patch_version;
+} __packed;
 
-		cmd_param = fw_ptr;
-		fw_ptr += cmd->plen;
-		fw_size -= cmd->plen;
+struct qca_device_info {
+	u32	rom_version;
+	u8	rampatch_hdr;	/* length of header in rampatch */
+	u8	nvm_hdr;	/* length of header in NVM */
+	u8	ver_offset;	/* offset of version structure in rampatch */
+};
 
-		opcode = le16_to_cpu(cmd->opcode);
+static const struct qca_device_info qca_devices_table[] = {
+	{ 0x00000100, 20, 4, 10 }, /* Rome 1.0 */
+	{ 0x00000101, 20, 4, 10 }, /* Rome 1.1 */
+	{ 0x00000201, 28, 4, 18 }, /* Rome 2.1 */
+	{ 0x00000300, 28, 4, 18 }, /* Rome 3.0 */
+	{ 0x00000302, 28, 4, 18 }, /* Rome 3.2 */
+};
 
-		skb = __hci_cmd_sync(hdev, opcode, cmd->plen, cmd_param,
-				     HCI_INIT_TIMEOUT);
-		if (IS_ERR(skb)) {
-			ret = PTR_ERR(skb);
-			BT_ERR("%s: BCM: patch command %04x failed (%ld)",
-			       hdev->name, opcode, ret);
-			goto reset_fw;
-		}
-		kfree_skb(skb);
-	}
+static int btusb_qca_send_vendor_req(struct hci_dev *hdev, u8 request,
+				     void *data, u16 size)
+{
+	struct btusb_data *btdata = hci_get_drvdata(hdev);
+	struct usb_device *udev = btdata->udev;
+	int pipe, err;
+	u8 *buf;
 
-	/* 250 msec delay after Launch Ram completes */
-	msleep(250);
+	buf = kmalloc(size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 
-reset_fw:
-	/* Reset */
-	skb = __hci_cmd_sync(hdev, HCI_OP_RESET, 0, NULL, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		ret = PTR_ERR(skb);
-		BT_ERR("%s: HCI_OP_RESET failed (%ld)", hdev->name, ret);
+	/* Found some of USB hosts have IOT issues with ours so that we should
+	 * not wait until HCI layer is ready.
+	 */
+	pipe = usb_rcvctrlpipe(udev, 0);
+	err = usb_control_msg(udev, pipe, request, USB_TYPE_VENDOR | USB_DIR_IN,
+			      0, 0, buf, size, USB_CTRL_SET_TIMEOUT);
+	if (err < 0) {
+		BT_ERR("%s: Failed to access otp area (%d)", hdev->name, err);
 		goto done;
 	}
-	kfree_skb(skb);
 
-	/* Read Local Version Info */
-	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
-			     HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		ret = PTR_ERR(skb);
-		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION failed (%ld)",
-		       hdev->name, ret);
-		goto done;
-	}
+	memcpy(data, buf, size);
 
-	if (skb->len != sizeof(*ver)) {
-		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION event length mismatch",
-		       hdev->name);
-		kfree_skb(skb);
-		ret = -EIO;
-		goto done;
-	}
+done:
+	kfree(buf);
 
-	ver = (struct hci_rp_read_local_version *)skb->data;
-	BT_INFO("%s: BCM: firmware hci_ver=%02x hci_rev=%04x lmp_ver=%02x "
-		"lmp_subver=%04x", hdev->name, ver->hci_ver, ver->hci_rev,
-		ver->lmp_ver, ver->lmp_subver);
-	kfree_skb(skb);
+	return err;
+}
 
-	/* Read BD Address */
-	skb = __hci_cmd_sync(hdev, HCI_OP_READ_BD_ADDR, 0, NULL,
-			     HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		ret = PTR_ERR(skb);
-		BT_ERR("%s: HCI_OP_READ_BD_ADDR failed (%ld)",
-		       hdev->name, ret);
+static int btusb_setup_qca_download_fw(struct hci_dev *hdev,
+				       const struct firmware *firmware,
+				       size_t hdr_size)
+{
+	struct btusb_data *btdata = hci_get_drvdata(hdev);
+	struct usb_device *udev = btdata->udev;
+	size_t count, size, sent = 0;
+	int pipe, len, err;
+	u8 *buf;
+
+	buf = kmalloc(QCA_DFU_PACKET_LEN, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	count = firmware->size;
+
+	size = min_t(size_t, count, hdr_size);
+	memcpy(buf, firmware->data, size);
+
+	/* USB patches should go down to controller through USB path
+	 * because binary format fits to go down through USB channel.
+	 * USB control path is for patching headers and USB bulk is for
+	 * patch body.
+	 */
+	pipe = usb_sndctrlpipe(udev, 0);
+	err = usb_control_msg(udev, pipe, QCA_DFU_DOWNLOAD, USB_TYPE_VENDOR,
+			      0, 0, buf, size, USB_CTRL_SET_TIMEOUT);
+	if (err < 0) {
+		BT_ERR("%s: Failed to send headers (%d)", hdev->name, err);
 		goto done;
 	}
 
-	if (skb->len != sizeof(*bda)) {
-		BT_ERR("%s: HCI_OP_READ_BD_ADDR event length mismatch",
-		       hdev->name);
-		kfree_skb(skb);
-		ret = -EIO;
-		goto done;
+	sent += size;
+	count -= size;
+
+	while (count) {
+		size = min_t(size_t, count, QCA_DFU_PACKET_LEN);
+
+		memcpy(buf, firmware->data + sent, size);
+
+		pipe = usb_sndbulkpipe(udev, 0x02);
+		err = usb_bulk_msg(udev, pipe, buf, size, &len,
+				   QCA_DFU_TIMEOUT);
+		if (err < 0) {
+			BT_ERR("%s: Failed to send body at %zd of %zd (%d)",
+			       hdev->name, sent, firmware->size, err);
+			break;
+		}
+
+		if (size != len) {
+			BT_ERR("%s: Failed to get bulk buffer", hdev->name);
+			err = -EILSEQ;
+			break;
+		}
+
+		sent  += size;
+		count -= size;
 	}
 
-	bda = (struct hci_rp_read_bd_addr *)skb->data;
-	if (bda->status) {
-		BT_ERR("%s: HCI_OP_READ_BD_ADDR error status (%02x)",
-		       hdev->name, bda->status);
-		kfree_skb(skb);
-		ret = -bt_to_errno(bda->status);
-		goto done;
+done:
+	kfree(buf);
+	return err;
+}
+
+static int btusb_setup_qca_load_rampatch(struct hci_dev *hdev,
+					 struct qca_version *ver,
+					 const struct qca_device_info *info)
+{
+	struct qca_rampatch_version *rver;
+	const struct firmware *fw;
+	u32 ver_rom, ver_patch;
+	u16 rver_rom, rver_patch;
+	char fwname[64];
+	int err;
+
+	ver_rom = le32_to_cpu(ver->rom_version);
+	ver_patch = le32_to_cpu(ver->patch_version);
+
+	snprintf(fwname, sizeof(fwname), "qca/rampatch_usb_%08x.bin", ver_rom);
+
+	err = request_firmware(&fw, fwname, &hdev->dev);
+	if (err) {
+		BT_ERR("%s: failed to request rampatch file: %s (%d)",
+		       hdev->name, fwname, err);
+		return err;
 	}
 
-	/* The address 00:20:70:02:A0:00 indicates a BCM20702A0 controller
-	 * with no configured address.
-	 */
-	if (!bacmp(&bda->bdaddr, BDADDR_BCM20702A0)) {
-		BT_INFO("%s: BCM: using default device address (%pMR)",
-			hdev->name, &bda->bdaddr);
-		set_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks);
+	BT_INFO("%s: using rampatch file: %s", hdev->name, fwname);
+
+	rver = (struct qca_rampatch_version *)(fw->data + info->ver_offset);
+	rver_rom = le16_to_cpu(rver->rom_version);
+	rver_patch = le16_to_cpu(rver->patch_version);
+
+	BT_INFO("%s: QCA: patch rome 0x%x build 0x%x, firmware rome 0x%x "
+		"build 0x%x", hdev->name, rver_rom, rver_patch, ver_rom,
+		ver_patch);
+
+	if (rver_rom != ver_rom || rver_patch <= ver_patch) {
+		BT_ERR("%s: rampatch file version did not match with firmware",
+		       hdev->name);
+		err = -EINVAL;
+		goto done;
 	}
 
-	kfree_skb(skb);
+	err = btusb_setup_qca_download_fw(hdev, fw, info->rampatch_hdr);
 
 done:
 	release_firmware(fw);
 
-	return ret;
+	return err;
 }
 
-static int btusb_set_bdaddr_bcm(struct hci_dev *hdev, const bdaddr_t *bdaddr)
+static int btusb_setup_qca_load_nvm(struct hci_dev *hdev,
+				    struct qca_version *ver,
+				    const struct qca_device_info *info)
 {
-	struct sk_buff *skb;
-	long ret;
+	const struct firmware *fw;
+	char fwname[64];
+	int err;
 
-	skb = __hci_cmd_sync(hdev, 0xfc01, 6, bdaddr, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		ret = PTR_ERR(skb);
-		BT_ERR("%s: BCM: Change address command failed (%ld)",
-		       hdev->name, ret);
-		return ret;
+	snprintf(fwname, sizeof(fwname), "qca/nvm_usb_%08x.bin",
+		 le32_to_cpu(ver->rom_version));
+
+	err = request_firmware(&fw, fwname, &hdev->dev);
+	if (err) {
+		BT_ERR("%s: failed to request NVM file: %s (%d)",
+		       hdev->name, fwname, err);
+		return err;
 	}
-	kfree_skb(skb);
 
-	return 0;
+	BT_INFO("%s: using NVM file: %s", hdev->name, fwname);
+
+	err = btusb_setup_qca_download_fw(hdev, fw, info->nvm_hdr);
+
+	release_firmware(fw);
+
+	return err;
 }
 
-static int btusb_set_bdaddr_ath3012(struct hci_dev *hdev,
-				    const bdaddr_t *bdaddr)
+static int btusb_setup_qca(struct hci_dev *hdev)
 {
-	struct sk_buff *skb;
-	u8 buf[10];
-	long ret;
+	const struct qca_device_info *info = NULL;
+	struct qca_version ver;
+	u32 ver_rom;
+	u8 status;
+	int i, err;
 
-	buf[0] = 0x01;
-	buf[1] = 0x01;
-	buf[2] = 0x00;
-	buf[3] = sizeof(bdaddr_t);
-	memcpy(buf + 4, bdaddr, sizeof(bdaddr_t));
+	err = btusb_qca_send_vendor_req(hdev, QCA_GET_TARGET_VERSION, &ver,
+				        sizeof(ver));
+	if (err < 0)
+		return err;
 
-	skb = __hci_cmd_sync(hdev, 0xfc0b, sizeof(buf), buf, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		ret = PTR_ERR(skb);
-		BT_ERR("%s: Change address command failed (%ld)",
-		       hdev->name, ret);
-		return ret;
+	ver_rom = le32_to_cpu(ver.rom_version);
+	for (i = 0; i < ARRAY_SIZE(qca_devices_table); i++) {
+		if (ver_rom == qca_devices_table[i].rom_version)
+			info = &qca_devices_table[i];
+	}
+	if (!info) {
+		BT_ERR("%s: don't support firmware rome 0x%x", hdev->name,
+		       ver_rom);
+		return -ENODEV;
+	}
+
+	err = btusb_qca_send_vendor_req(hdev, QCA_CHECK_STATUS, &status,
+					sizeof(status));
+	if (err < 0)
+		return err;
+
+	if (!(status & QCA_PATCH_UPDATED)) {
+		err = btusb_setup_qca_load_rampatch(hdev, &ver, info);
+		if (err < 0)
+			return err;
+	}
+
+	if (!(status & QCA_SYSCFG_UPDATED)) {
+		err = btusb_setup_qca_load_nvm(hdev, &ver, info);
+		if (err < 0)
+			return err;
 	}
-	kfree_skb(skb);
 
 	return 0;
 }
@@ -2701,23 +2728,29 @@ static int btusb_probe(struct usb_interface *intf,
 	if (id->driver_info & BTUSB_BCM92035)
 		hdev->setup = btusb_setup_bcm92035;
 
+#ifdef CONFIG_BT_HCIBTUSB_BCM
 	if (id->driver_info & BTUSB_BCM_PATCHRAM) {
-		hdev->setup = btusb_setup_bcm_patchram;
-		hdev->set_bdaddr = btusb_set_bdaddr_bcm;
-		set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
+		hdev->setup = btbcm_setup_patchram;
+		hdev->set_bdaddr = btbcm_set_bdaddr;
 	}
 
+	if (id->driver_info & BTUSB_BCM_APPLE)
+		hdev->setup = btbcm_setup_apple;
+#endif
+
 	if (id->driver_info & BTUSB_INTEL) {
 		hdev->setup = btusb_setup_intel;
-		hdev->set_bdaddr = btusb_set_bdaddr_intel;
+		hdev->shutdown = btusb_shutdown_intel;
+		hdev->set_bdaddr = btintel_set_bdaddr;
 		set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
+		set_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks);
 	}
 
 	if (id->driver_info & BTUSB_INTEL_NEW) {
 		hdev->send = btusb_send_frame_intel;
 		hdev->setup = btusb_setup_intel_new;
 		hdev->hw_error = btusb_hw_error_intel;
-		hdev->set_bdaddr = btusb_set_bdaddr_intel;
+		hdev->set_bdaddr = btintel_set_bdaddr;
 		set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
 	}
 
@@ -2734,9 +2767,15 @@ static int btusb_probe(struct usb_interface *intf,
 
 	if (id->driver_info & BTUSB_ATH3012) {
 		hdev->set_bdaddr = btusb_set_bdaddr_ath3012;
+		set_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks);
 		set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
 	}
 
+	if (id->driver_info & BTUSB_QCA_ROME) {
+		data->setup_on_usb = btusb_setup_qca;
+		hdev->set_bdaddr = btusb_set_bdaddr_ath3012;
+	}
+
 	if (id->driver_info & BTUSB_AMP) {
 		/* AMP controllers do not support SCO packets */
 		data->isoc = NULL;
@@ -2772,6 +2811,8 @@ static int btusb_probe(struct usb_interface *intf,
 		/* Fake CSR devices with broken commands */
 		if (bcdDevice <= 0x100)
 			hdev->setup = btusb_setup_csr;
+
+		set_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks);
 	}
 
 	if (id->driver_info & BTUSB_SNIFFER) {
diff --git a/drivers/bluetooth/hci_ath.c b/drivers/bluetooth/hci_ath.c
index 9c4dcf4c62ea..1b3f8647ea2f 100644
--- a/drivers/bluetooth/hci_ath.c
+++ b/drivers/bluetooth/hci_ath.c
@@ -45,6 +45,7 @@ struct ath_struct {
 	struct hci_uart *hu;
 	unsigned int cur_sleep;
 
+	struct sk_buff *rx_skb;
 	struct sk_buff_head txq;
 	struct work_struct ctxtsw;
 };
@@ -136,6 +137,8 @@ static int ath_close(struct hci_uart *hu)
 
 	skb_queue_purge(&ath->txq);
 
+	kfree_skb(ath->rx_skb);
+
 	cancel_work_sync(&ath->ctxtsw);
 
 	hu->priv = NULL;
@@ -187,40 +190,42 @@ static struct sk_buff *ath_dequeue(struct hci_uart *hu)
 	return skb_dequeue(&ath->txq);
 }
 
+static const struct h4_recv_pkt ath_recv_pkts[] = {
+	{ H4_RECV_ACL,   .recv = hci_recv_frame },
+	{ H4_RECV_SCO,   .recv = hci_recv_frame },
+	{ H4_RECV_EVENT, .recv = hci_recv_frame },
+};
+
 /* Recv data */
-static int ath_recv(struct hci_uart *hu, void *data, int count)
+static int ath_recv(struct hci_uart *hu, const void *data, int count)
 {
-	int ret;
+	struct ath_struct *ath = hu->priv;
 
-	ret = hci_recv_stream_fragment(hu->hdev, data, count);
-	if (ret < 0) {
-		BT_ERR("Frame Reassembly Failed");
-		return ret;
+	ath->rx_skb = h4_recv_buf(hu->hdev, ath->rx_skb, data, count,
+				  ath_recv_pkts, ARRAY_SIZE(ath_recv_pkts));
+	if (IS_ERR(ath->rx_skb)) {
+		int err = PTR_ERR(ath->rx_skb);
+		BT_ERR("%s: Frame reassembly failed (%d)", hu->hdev->name, err);
+		return err;
 	}
 
 	return count;
 }
 
-static struct hci_uart_proto athp = {
-	.id = HCI_UART_ATH3K,
-	.open = ath_open,
-	.close = ath_close,
-	.recv = ath_recv,
-	.enqueue = ath_enqueue,
-	.dequeue = ath_dequeue,
-	.flush = ath_flush,
+static const struct hci_uart_proto athp = {
+	.id		= HCI_UART_ATH3K,
+	.name		= "ATH3K",
+	.open		= ath_open,
+	.close		= ath_close,
+	.recv		= ath_recv,
+	.enqueue	= ath_enqueue,
+	.dequeue	= ath_dequeue,
+	.flush		= ath_flush,
 };
 
 int __init ath_init(void)
 {
-	int err = hci_uart_register_proto(&athp);
-
-	if (!err)
-		BT_INFO("HCIATH3K protocol initialized");
-	else
-		BT_ERR("HCIATH3K protocol registration failed");
-
-	return err;
+	return hci_uart_register_proto(&athp);
 }
 
 int __exit ath_deinit(void)
diff --git a/drivers/bluetooth/hci_bcm.c b/drivers/bluetooth/hci_bcm.c
new file mode 100644
index 000000000000..1ec0b4a5ffa6
--- /dev/null
+++ b/drivers/bluetooth/hci_bcm.c
@@ -0,0 +1,153 @@
+/*
+ *
+ *  Bluetooth HCI UART driver for Broadcom devices
+ *
+ *  Copyright (C) 2015  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/skbuff.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btbcm.h"
+#include "hci_uart.h"
+
+struct bcm_data {
+	struct sk_buff *rx_skb;
+	struct sk_buff_head txq;
+};
+
+static int bcm_open(struct hci_uart *hu)
+{
+	struct bcm_data *bcm;
+
+	BT_DBG("hu %p", hu);
+
+	bcm = kzalloc(sizeof(*bcm), GFP_KERNEL);
+	if (!bcm)
+		return -ENOMEM;
+
+	skb_queue_head_init(&bcm->txq);
+
+	hu->priv = bcm;
+	return 0;
+}
+
+static int bcm_close(struct hci_uart *hu)
+{
+	struct bcm_data *bcm = hu->priv;
+
+	BT_DBG("hu %p", hu);
+
+	skb_queue_purge(&bcm->txq);
+	kfree_skb(bcm->rx_skb);
+	kfree(bcm);
+
+	hu->priv = NULL;
+	return 0;
+}
+
+static int bcm_flush(struct hci_uart *hu)
+{
+	struct bcm_data *bcm = hu->priv;
+
+	BT_DBG("hu %p", hu);
+
+	skb_queue_purge(&bcm->txq);
+
+	return 0;
+}
+
+static int bcm_setup(struct hci_uart *hu)
+{
+	BT_DBG("hu %p", hu);
+
+	hu->hdev->set_bdaddr = btbcm_set_bdaddr;
+
+	return btbcm_setup_patchram(hu->hdev);
+}
+
+static const struct h4_recv_pkt bcm_recv_pkts[] = {
+	{ H4_RECV_ACL,   .recv = hci_recv_frame },
+	{ H4_RECV_SCO,   .recv = hci_recv_frame },
+	{ H4_RECV_EVENT, .recv = hci_recv_frame },
+};
+
+static int bcm_recv(struct hci_uart *hu, const void *data, int count)
+{
+	struct bcm_data *bcm = hu->priv;
+
+	if (!test_bit(HCI_UART_REGISTERED, &hu->flags))
+		return -EUNATCH;
+
+	bcm->rx_skb = h4_recv_buf(hu->hdev, bcm->rx_skb, data, count,
+				  bcm_recv_pkts, ARRAY_SIZE(bcm_recv_pkts));
+	if (IS_ERR(bcm->rx_skb)) {
+		int err = PTR_ERR(bcm->rx_skb);
+		BT_ERR("%s: Frame reassembly failed (%d)", hu->hdev->name, err);
+		return err;
+	}
+
+	return count;
+}
+
+static int bcm_enqueue(struct hci_uart *hu, struct sk_buff *skb)
+{
+	struct bcm_data *bcm = hu->priv;
+
+	BT_DBG("hu %p skb %p", hu, skb);
+
+	/* Prepend skb with frame type */
+	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	skb_queue_tail(&bcm->txq, skb);
+
+	return 0;
+}
+
+static struct sk_buff *bcm_dequeue(struct hci_uart *hu)
+{
+	struct bcm_data *bcm = hu->priv;
+
+	return skb_dequeue(&bcm->txq);
+}
+
+static const struct hci_uart_proto bcm_proto = {
+	.id		= HCI_UART_BCM,
+	.name		= "BCM",
+	.open		= bcm_open,
+	.close		= bcm_close,
+	.flush		= bcm_flush,
+	.setup		= bcm_setup,
+	.recv		= bcm_recv,
+	.enqueue	= bcm_enqueue,
+	.dequeue	= bcm_dequeue,
+};
+
+int __init bcm_init(void)
+{
+	return hci_uart_register_proto(&bcm_proto);
+}
+
+int __exit bcm_deinit(void)
+{
+	return hci_uart_unregister_proto(&bcm_proto);
+}
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index 21cc45b34f13..dc8e3d4356a0 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -47,8 +47,6 @@
 
 #include "hci_uart.h"
 
-#define VERSION "0.3"
-
 static bool txcrc = 1;
 static bool hciextn = 1;
 
@@ -554,10 +552,10 @@ static u16 bscp_get_crc(struct bcsp_struct *bcsp)
 }
 
 /* Recv data */
-static int bcsp_recv(struct hci_uart *hu, void *data, int count)
+static int bcsp_recv(struct hci_uart *hu, const void *data, int count)
 {
 	struct bcsp_struct *bcsp = hu->priv;
-	unsigned char *ptr;
+	const unsigned char *ptr;
 
 	BT_DBG("hu %p count %d rx_state %d rx_count %ld", 
 		hu, count, bcsp->rx_state, bcsp->rx_count);
@@ -735,8 +733,9 @@ static int bcsp_close(struct hci_uart *hu)
 	return 0;
 }
 
-static struct hci_uart_proto bcsp = {
+static const struct hci_uart_proto bcsp = {
 	.id		= HCI_UART_BCSP,
+	.name		= "BCSP",
 	.open		= bcsp_open,
 	.close		= bcsp_close,
 	.enqueue	= bcsp_enqueue,
@@ -747,14 +746,7 @@ static struct hci_uart_proto bcsp = {
 
 int __init bcsp_init(void)
 {
-	int err = hci_uart_register_proto(&bcsp);
-
-	if (!err)
-		BT_INFO("HCI BCSP protocol initialized");
-	else
-		BT_ERR("HCI BCSP protocol registration failed");
-
-	return err;
+	return hci_uart_register_proto(&bcsp);
 }
 
 int __exit bcsp_deinit(void)
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index 66db9a803373..f7190f01e135 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -40,17 +40,14 @@
 #include <linux/signal.h>
 #include <linux/ioctl.h>
 #include <linux/skbuff.h>
+#include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 
 #include "hci_uart.h"
 
-#define VERSION "1.2"
-
 struct h4_struct {
-	unsigned long rx_state;
-	unsigned long rx_count;
 	struct sk_buff *rx_skb;
 	struct sk_buff_head txq;
 };
@@ -117,18 +114,26 @@ static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	return 0;
 }
 
+static const struct h4_recv_pkt h4_recv_pkts[] = {
+	{ H4_RECV_ACL,   .recv = hci_recv_frame },
+	{ H4_RECV_SCO,   .recv = hci_recv_frame },
+	{ H4_RECV_EVENT, .recv = hci_recv_frame },
+};
+
 /* Recv data */
-static int h4_recv(struct hci_uart *hu, void *data, int count)
+static int h4_recv(struct hci_uart *hu, const void *data, int count)
 {
-	int ret;
+	struct h4_struct *h4 = hu->priv;
 
 	if (!test_bit(HCI_UART_REGISTERED, &hu->flags))
 		return -EUNATCH;
 
-	ret = hci_recv_stream_fragment(hu->hdev, data, count);
-	if (ret < 0) {
-		BT_ERR("Frame Reassembly Failed");
-		return ret;
+	h4->rx_skb = h4_recv_buf(hu->hdev, h4->rx_skb, data, count,
+				 h4_recv_pkts, ARRAY_SIZE(h4_recv_pkts));
+	if (IS_ERR(h4->rx_skb)) {
+		int err = PTR_ERR(h4->rx_skb);
+		BT_ERR("%s: Frame reassembly failed (%d)", hu->hdev->name, err);
+		return err;
 	}
 
 	return count;
@@ -140,8 +145,9 @@ static struct sk_buff *h4_dequeue(struct hci_uart *hu)
 	return skb_dequeue(&h4->txq);
 }
 
-static struct hci_uart_proto h4p = {
+static const struct hci_uart_proto h4p = {
 	.id		= HCI_UART_H4,
+	.name		= "H4",
 	.open		= h4_open,
 	.close		= h4_close,
 	.recv		= h4_recv,
@@ -152,17 +158,105 @@ static struct hci_uart_proto h4p = {
 
 int __init h4_init(void)
 {
-	int err = hci_uart_register_proto(&h4p);
-
-	if (!err)
-		BT_INFO("HCI H4 protocol initialized");
-	else
-		BT_ERR("HCI H4 protocol registration failed");
-
-	return err;
+	return hci_uart_register_proto(&h4p);
 }
 
 int __exit h4_deinit(void)
 {
 	return hci_uart_unregister_proto(&h4p);
 }
+
+struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
+			    const unsigned char *buffer, int count,
+			    const struct h4_recv_pkt *pkts, int pkts_count)
+{
+	while (count) {
+		int i, len;
+
+		if (!skb) {
+			for (i = 0; i < pkts_count; i++) {
+				if (buffer[0] != (&pkts[i])->type)
+					continue;
+
+				skb = bt_skb_alloc((&pkts[i])->maxlen,
+						   GFP_ATOMIC);
+				if (!skb)
+					return ERR_PTR(-ENOMEM);
+
+				bt_cb(skb)->pkt_type = (&pkts[i])->type;
+				bt_cb(skb)->expect = (&pkts[i])->hlen;
+				break;
+			}
+
+			/* Check for invalid packet type */
+			if (!skb)
+				return ERR_PTR(-EILSEQ);
+
+			count -= 1;
+			buffer += 1;
+		}
+
+		len = min_t(uint, bt_cb(skb)->expect - skb->len, count);
+		memcpy(skb_put(skb, len), buffer, len);
+
+		count -= len;
+		buffer += len;
+
+		/* Check for partial packet */
+		if (skb->len < bt_cb(skb)->expect)
+			continue;
+
+		for (i = 0; i < pkts_count; i++) {
+			if (bt_cb(skb)->pkt_type == (&pkts[i])->type)
+				break;
+		}
+
+		if (i >= pkts_count) {
+			kfree_skb(skb);
+			return ERR_PTR(-EILSEQ);
+		}
+
+		if (skb->len == (&pkts[i])->hlen) {
+			u16 dlen;
+
+			switch ((&pkts[i])->lsize) {
+			case 0:
+				/* No variable data length */
+				(&pkts[i])->recv(hdev, skb);
+				skb = NULL;
+				break;
+			case 1:
+				/* Single octet variable length */
+				dlen = skb->data[(&pkts[i])->loff];
+				bt_cb(skb)->expect += dlen;
+
+				if (skb_tailroom(skb) < dlen) {
+					kfree_skb(skb);
+					return ERR_PTR(-EMSGSIZE);
+				}
+				break;
+			case 2:
+				/* Double octet variable length */
+				dlen = get_unaligned_le16(skb->data +
+							  (&pkts[i])->loff);
+				bt_cb(skb)->expect += dlen;
+
+				if (skb_tailroom(skb) < dlen) {
+					kfree_skb(skb);
+					return ERR_PTR(-EMSGSIZE);
+				}
+				break;
+			default:
+				/* Unsupported variable length */
+				kfree_skb(skb);
+				return ERR_PTR(-EILSEQ);
+			}
+		} else {
+			/* Complete frame */
+			(&pkts[i])->recv(hdev, skb);
+			skb = NULL;
+		}
+	}
+
+	return skb;
+}
diff --git a/drivers/bluetooth/hci_h5.c b/drivers/bluetooth/hci_h5.c
index ec0fa7732c0d..3455cecc9ecf 100644
--- a/drivers/bluetooth/hci_h5.c
+++ b/drivers/bluetooth/hci_h5.c
@@ -511,10 +511,10 @@ static void h5_reset_rx(struct h5 *h5)
 	clear_bit(H5_RX_ESC, &h5->flags);
 }
 
-static int h5_recv(struct hci_uart *hu, void *data, int count)
+static int h5_recv(struct hci_uart *hu, const void *data, int count)
 {
 	struct h5 *h5 = hu->priv;
-	unsigned char *ptr = data;
+	const unsigned char *ptr = data;
 
 	BT_DBG("%s pending %zu count %d", hu->hdev->name, h5->rx_pending,
 	       count);
@@ -743,8 +743,9 @@ static int h5_flush(struct hci_uart *hu)
 	return 0;
 }
 
-static struct hci_uart_proto h5p = {
+static const struct hci_uart_proto h5p = {
 	.id		= HCI_UART_3WIRE,
+	.name		= "Three-wire (H5)",
 	.open		= h5_open,
 	.close		= h5_close,
 	.recv		= h5_recv,
@@ -755,14 +756,7 @@ static struct hci_uart_proto h5p = {
 
 int __init h5_init(void)
 {
-	int err = hci_uart_register_proto(&h5p);
-
-	if (!err)
-		BT_INFO("HCI Three-wire UART (H5) protocol initialized");
-	else
-		BT_ERR("HCI Three-wire UART (H5) protocol init failed");
-
-	return err;
+	return hci_uart_register_proto(&h5p);
 }
 
 int __exit h5_deinit(void)
diff --git a/drivers/bluetooth/hci_intel.c b/drivers/bluetooth/hci_intel.c
new file mode 100644
index 000000000000..5dd07bf05236
--- /dev/null
+++ b/drivers/bluetooth/hci_intel.c
@@ -0,0 +1,31 @@
+/*
+ *
+ *  Bluetooth HCI UART driver for Intel devices
+ *
+ *  Copyright (C) 2015  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/skbuff.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "hci_uart.h"
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index dc487b5d1156..5c9a73f02664 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -44,13 +44,15 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 
+#include "btintel.h"
+#include "btbcm.h"
 #include "hci_uart.h"
 
-#define VERSION "2.2"
+#define VERSION "2.3"
 
-static struct hci_uart_proto *hup[HCI_UART_MAX_PROTO];
+static const struct hci_uart_proto *hup[HCI_UART_MAX_PROTO];
 
-int hci_uart_register_proto(struct hci_uart_proto *p)
+int hci_uart_register_proto(const struct hci_uart_proto *p)
 {
 	if (p->id >= HCI_UART_MAX_PROTO)
 		return -EINVAL;
@@ -60,10 +62,12 @@ int hci_uart_register_proto(struct hci_uart_proto *p)
 
 	hup[p->id] = p;
 
+	BT_INFO("HCI UART protocol %s registered", p->name);
+
 	return 0;
 }
 
-int hci_uart_unregister_proto(struct hci_uart_proto *p)
+int hci_uart_unregister_proto(const struct hci_uart_proto *p)
 {
 	if (p->id >= HCI_UART_MAX_PROTO)
 		return -EINVAL;
@@ -76,7 +80,7 @@ int hci_uart_unregister_proto(struct hci_uart_proto *p)
 	return 0;
 }
 
-static struct hci_uart_proto *hci_uart_get_proto(unsigned int id)
+static const struct hci_uart_proto *hci_uart_get_proto(unsigned int id)
 {
 	if (id >= HCI_UART_MAX_PROTO)
 		return NULL;
@@ -261,6 +265,54 @@ static int hci_uart_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	return 0;
 }
 
+static int hci_uart_setup(struct hci_dev *hdev)
+{
+	struct hci_uart *hu = hci_get_drvdata(hdev);
+	struct hci_rp_read_local_version *ver;
+	struct sk_buff *skb;
+
+	if (hu->proto->setup)
+		return hu->proto->setup(hu);
+
+	if (!test_bit(HCI_UART_VND_DETECT, &hu->hdev_flags))
+		return 0;
+
+	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: Reading local version information failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return 0;
+	}
+
+	if (skb->len != sizeof(*ver)) {
+		BT_ERR("%s: Event length mismatch for version information",
+		       hdev->name);
+		goto done;
+	}
+
+	ver = (struct hci_rp_read_local_version *)skb->data;
+
+	switch (le16_to_cpu(ver->manufacturer)) {
+#ifdef CONFIG_BT_HCIUART_INTEL
+	case 2:
+		hdev->set_bdaddr = btintel_set_bdaddr;
+		btintel_check_bdaddr(hdev);
+		break;
+#endif
+#ifdef CONFIG_BT_HCIUART_BCM
+	case 15:
+		hdev->set_bdaddr = btbcm_set_bdaddr;
+		btbcm_check_bdaddr(hdev);
+		break;
+#endif
+	}
+
+done:
+	kfree_skb(skb);
+	return 0;
+}
+
 /* ------ LDISC part ------ */
 /* hci_uart_tty_open
  *
@@ -316,7 +368,7 @@ static int hci_uart_tty_open(struct tty_struct *tty)
  */
 static void hci_uart_tty_close(struct tty_struct *tty)
 {
-	struct hci_uart *hu = (void *)tty->disc_data;
+	struct hci_uart *hu = tty->disc_data;
 	struct hci_dev *hdev;
 
 	BT_DBG("tty %p", tty);
@@ -355,7 +407,7 @@ static void hci_uart_tty_close(struct tty_struct *tty)
  */
 static void hci_uart_tty_wakeup(struct tty_struct *tty)
 {
-	struct hci_uart *hu = (void *)tty->disc_data;
+	struct hci_uart *hu = tty->disc_data;
 
 	BT_DBG("");
 
@@ -383,9 +435,10 @@ static void hci_uart_tty_wakeup(struct tty_struct *tty)
  *
  * Return Value:    None
  */
-static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data, char *flags, int count)
+static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data,
+				 char *flags, int count)
 {
-	struct hci_uart *hu = (void *)tty->disc_data;
+	struct hci_uart *hu = tty->disc_data;
 
 	if (!hu || tty != hu->tty)
 		return;
@@ -394,7 +447,7 @@ static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data, char *f
 		return;
 
 	spin_lock(&hu->rx_lock);
-	hu->proto->recv(hu, (void *) data, count);
+	hu->proto->recv(hu, data, count);
 
 	if (hu->hdev)
 		hu->hdev->stat.byte_rx += count;
@@ -426,6 +479,7 @@ static int hci_uart_register_dev(struct hci_uart *hu)
 	hdev->close = hci_uart_close;
 	hdev->flush = hci_uart_flush;
 	hdev->send  = hci_uart_send_frame;
+	hdev->setup = hci_uart_setup;
 	SET_HCIDEV_DEV(hdev, hu->tty->dev);
 
 	if (test_bit(HCI_UART_RAW_DEVICE, &hu->hdev_flags))
@@ -458,7 +512,7 @@ static int hci_uart_register_dev(struct hci_uart *hu)
 
 static int hci_uart_set_proto(struct hci_uart *hu, int id)
 {
-	struct hci_uart_proto *p;
+	const struct hci_uart_proto *p;
 	int err;
 
 	p = hci_uart_get_proto(id);
@@ -486,9 +540,10 @@ static int hci_uart_set_flags(struct hci_uart *hu, unsigned long flags)
 				    BIT(HCI_UART_RESET_ON_INIT) |
 				    BIT(HCI_UART_CREATE_AMP) |
 				    BIT(HCI_UART_INIT_PENDING) |
-				    BIT(HCI_UART_EXT_CONFIG);
+				    BIT(HCI_UART_EXT_CONFIG) |
+				    BIT(HCI_UART_VND_DETECT);
 
-	if ((flags & ~valid_flags))
+	if (flags & ~valid_flags)
 		return -EINVAL;
 
 	hu->hdev_flags = flags;
@@ -509,10 +564,10 @@ static int hci_uart_set_flags(struct hci_uart *hu, unsigned long flags)
  *
  * Return Value:    Command dependent
  */
-static int hci_uart_tty_ioctl(struct tty_struct *tty, struct file * file,
-					unsigned int cmd, unsigned long arg)
+static int hci_uart_tty_ioctl(struct tty_struct *tty, struct file *file,
+			      unsigned int cmd, unsigned long arg)
 {
-	struct hci_uart *hu = (void *)tty->disc_data;
+	struct hci_uart *hu = tty->disc_data;
 	int err = 0;
 
 	BT_DBG("");
@@ -566,19 +621,19 @@ static int hci_uart_tty_ioctl(struct tty_struct *tty, struct file * file,
  * We don't provide read/write/poll interface for user space.
  */
 static ssize_t hci_uart_tty_read(struct tty_struct *tty, struct file *file,
-					unsigned char __user *buf, size_t nr)
+				 unsigned char __user *buf, size_t nr)
 {
 	return 0;
 }
 
 static ssize_t hci_uart_tty_write(struct tty_struct *tty, struct file *file,
-					const unsigned char *data, size_t count)
+				  const unsigned char *data, size_t count)
 {
 	return 0;
 }
 
 static unsigned int hci_uart_tty_poll(struct tty_struct *tty,
-					struct file *filp, poll_table *wait)
+				      struct file *filp, poll_table *wait)
 {
 	return 0;
 }
@@ -626,6 +681,9 @@ static int __init hci_uart_init(void)
 #ifdef CONFIG_BT_HCIUART_3WIRE
 	h5_init();
 #endif
+#ifdef CONFIG_BT_HCIUART_BCM
+	bcm_init();
+#endif
 
 	return 0;
 }
@@ -649,6 +707,9 @@ static void __exit hci_uart_exit(void)
 #ifdef CONFIG_BT_HCIUART_3WIRE
 	h5_deinit();
 #endif
+#ifdef CONFIG_BT_HCIUART_BCM
+	bcm_deinit();
+#endif
 
 	/* Release tty registration of line discipline */
 	err = tty_unregister_ldisc(N_HCI);
diff --git a/drivers/bluetooth/hci_ll.c b/drivers/bluetooth/hci_ll.c
index 69a90b1b5ff5..9ee24b075f79 100644
--- a/drivers/bluetooth/hci_ll.c
+++ b/drivers/bluetooth/hci_ll.c
@@ -370,10 +370,10 @@ static inline int ll_check_data_len(struct hci_dev *hdev, struct ll_struct *ll,
 }
 
 /* Recv data */
-static int ll_recv(struct hci_uart *hu, void *data, int count)
+static int ll_recv(struct hci_uart *hu, const void *data, int count)
 {
 	struct ll_struct *ll = hu->priv;
-	char *ptr;
+	const char *ptr;
 	struct hci_event_hdr *eh;
 	struct hci_acl_hdr   *ah;
 	struct hci_sco_hdr   *sh;
@@ -505,8 +505,9 @@ static struct sk_buff *ll_dequeue(struct hci_uart *hu)
 	return skb_dequeue(&ll->txq);
 }
 
-static struct hci_uart_proto llp = {
+static const struct hci_uart_proto llp = {
 	.id		= HCI_UART_LL,
+	.name		= "LL",
 	.open		= ll_open,
 	.close		= ll_close,
 	.recv		= ll_recv,
@@ -517,14 +518,7 @@ static struct hci_uart_proto llp = {
 
 int __init ll_init(void)
 {
-	int err = hci_uart_register_proto(&llp);
-
-	if (!err)
-		BT_INFO("HCILL protocol initialized");
-	else
-		BT_ERR("HCILL protocol registration failed");
-
-	return err;
+	return hci_uart_register_proto(&llp);
 }
 
 int __exit ll_deinit(void)
diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index 247488edcbf9..72120a5ba13c 100644
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -35,7 +35,7 @@
 #define HCIUARTGETFLAGS		_IOR('U', 204, int)
 
 /* UART protocols */
-#define HCI_UART_MAX_PROTO	6
+#define HCI_UART_MAX_PROTO	8
 
 #define HCI_UART_H4	0
 #define HCI_UART_BCSP	1
@@ -43,21 +43,26 @@
 #define HCI_UART_H4DS	3
 #define HCI_UART_LL	4
 #define HCI_UART_ATH3K	5
+#define HCI_UART_INTEL	6
+#define HCI_UART_BCM	7
 
 #define HCI_UART_RAW_DEVICE	0
 #define HCI_UART_RESET_ON_INIT	1
 #define HCI_UART_CREATE_AMP	2
 #define HCI_UART_INIT_PENDING	3
 #define HCI_UART_EXT_CONFIG	4
+#define HCI_UART_VND_DETECT	5
 
 struct hci_uart;
 
 struct hci_uart_proto {
 	unsigned int id;
+	const char *name;
 	int (*open)(struct hci_uart *hu);
 	int (*close)(struct hci_uart *hu);
 	int (*flush)(struct hci_uart *hu);
-	int (*recv)(struct hci_uart *hu, void *data, int len);
+	int (*setup)(struct hci_uart *hu);
+	int (*recv)(struct hci_uart *hu, const void *data, int len);
 	int (*enqueue)(struct hci_uart *hu, struct sk_buff *skb);
 	struct sk_buff *(*dequeue)(struct hci_uart *hu);
 };
@@ -71,7 +76,7 @@ struct hci_uart {
 	struct work_struct	init_ready;
 	struct work_struct	write_work;
 
-	struct hci_uart_proto	*proto;
+	const struct hci_uart_proto *proto;
 	void			*priv;
 
 	struct sk_buff		*tx_skb;
@@ -87,14 +92,48 @@ struct hci_uart {
 #define HCI_UART_SENDING	1
 #define HCI_UART_TX_WAKEUP	2
 
-int hci_uart_register_proto(struct hci_uart_proto *p);
-int hci_uart_unregister_proto(struct hci_uart_proto *p);
+int hci_uart_register_proto(const struct hci_uart_proto *p);
+int hci_uart_unregister_proto(const struct hci_uart_proto *p);
 int hci_uart_tx_wakeup(struct hci_uart *hu);
 int hci_uart_init_ready(struct hci_uart *hu);
 
 #ifdef CONFIG_BT_HCIUART_H4
 int h4_init(void);
 int h4_deinit(void);
+
+struct h4_recv_pkt {
+	u8  type;	/* Packet type */
+	u8  hlen;	/* Header length */
+	u8  loff;	/* Data length offset in header */
+	u8  lsize;	/* Data length field size */
+	u16 maxlen;	/* Max overall packet length */
+	int (*recv)(struct hci_dev *hdev, struct sk_buff *skb);
+};
+
+#define H4_RECV_ACL \
+	.type = HCI_ACLDATA_PKT, \
+	.hlen = HCI_ACL_HDR_SIZE, \
+	.loff = 2, \
+	.lsize = 2, \
+	.maxlen = HCI_MAX_FRAME_SIZE \
+
+#define H4_RECV_SCO \
+	.type = HCI_SCODATA_PKT, \
+	.hlen = HCI_SCO_HDR_SIZE, \
+	.loff = 2, \
+	.lsize = 1, \
+	.maxlen = HCI_MAX_SCO_SIZE
+
+#define H4_RECV_EVENT \
+	.type = HCI_EVENT_PKT, \
+	.hlen = HCI_EVENT_HDR_SIZE, \
+	.loff = 1, \
+	.lsize = 1, \
+	.maxlen = HCI_MAX_EVENT_SIZE
+
+struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
+			    const unsigned char *buffer, int count,
+			    const struct h4_recv_pkt *pkts, int pkts_count);
 #endif
 
 #ifdef CONFIG_BT_HCIUART_BCSP
@@ -116,3 +155,8 @@ int ath_deinit(void);
 int h5_init(void);
 int h5_deinit(void);
 #endif
+
+#ifdef CONFIG_BT_HCIUART_BCM
+int bcm_init(void);
+int bcm_deinit(void);
+#endif
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/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index de57b38809c7..f48cf11c655e 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -101,6 +101,19 @@ config HW_RANDOM_BCM2835
 
 	  If unsure, say Y.
 
+config HW_RANDOM_IPROC_RNG200
+	tristate "Broadcom iProc RNG200 support"
+	depends on ARCH_BCM_IPROC
+	default HW_RANDOM
+	---help---
+	  This driver provides kernel-side support for the RNG200
+	  hardware found on the Broadcom iProc SoCs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called iproc-rng200
+
+	  If unsure, say Y.
+
 config HW_RANDOM_GEODE
 	tristate "AMD Geode HW Random Number Generator support"
 	depends on X86_32 && PCI
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index 0b4cd57f4e24..055bb01510ad 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -28,5 +28,6 @@ obj-$(CONFIG_HW_RANDOM_POWERNV) += powernv-rng.o
 obj-$(CONFIG_HW_RANDOM_EXYNOS)	+= exynos-rng.o
 obj-$(CONFIG_HW_RANDOM_TPM) += tpm-rng.o
 obj-$(CONFIG_HW_RANDOM_BCM2835) += bcm2835-rng.o
+obj-$(CONFIG_HW_RANDOM_IPROC_RNG200) += iproc-rng200.o
 obj-$(CONFIG_HW_RANDOM_MSM) += msm-rng.o
 obj-$(CONFIG_HW_RANDOM_XGENE) += xgene-rng.o
diff --git a/drivers/char/hw_random/bcm63xx-rng.c b/drivers/char/hw_random/bcm63xx-rng.c
index ba6a65ac023b..d1494ecd9e11 100644
--- a/drivers/char/hw_random/bcm63xx-rng.c
+++ b/drivers/char/hw_random/bcm63xx-rng.c
@@ -13,24 +13,37 @@
 #include <linux/platform_device.h>
 #include <linux/hw_random.h>
 
-#include <bcm63xx_io.h>
-#include <bcm63xx_regs.h>
+#define RNG_CTRL			0x00
+#define RNG_EN				(1 << 0)
+
+#define RNG_STAT			0x04
+#define RNG_AVAIL_MASK			(0xff000000)
+
+#define RNG_DATA			0x08
+#define RNG_THRES			0x0c
+#define RNG_MASK			0x10
 
 struct bcm63xx_rng_priv {
+	struct hwrng rng;
 	struct clk *clk;
 	void __iomem *regs;
 };
 
-#define to_rng_priv(rng)	((struct bcm63xx_rng_priv *)rng->priv)
+#define to_rng_priv(rng)	container_of(rng, struct bcm63xx_rng_priv, rng)
 
 static int bcm63xx_rng_init(struct hwrng *rng)
 {
 	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
 	u32 val;
+	int error;
+
+	error = clk_prepare_enable(priv->clk);
+	if (error)
+		return error;
 
-	val = bcm_readl(priv->regs + RNG_CTRL);
+	val = __raw_readl(priv->regs + RNG_CTRL);
 	val |= RNG_EN;
-	bcm_writel(val, priv->regs + RNG_CTRL);
+	__raw_writel(val, priv->regs + RNG_CTRL);
 
 	return 0;
 }
@@ -40,23 +53,25 @@ static void bcm63xx_rng_cleanup(struct hwrng *rng)
 	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
 	u32 val;
 
-	val = bcm_readl(priv->regs + RNG_CTRL);
+	val = __raw_readl(priv->regs + RNG_CTRL);
 	val &= ~RNG_EN;
-	bcm_writel(val, priv->regs + RNG_CTRL);
+	__raw_writel(val, priv->regs + RNG_CTRL);
+
+	clk_didsable_unprepare(prov->clk);
 }
 
 static int bcm63xx_rng_data_present(struct hwrng *rng, int wait)
 {
 	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
 
-	return bcm_readl(priv->regs + RNG_STAT) & RNG_AVAIL_MASK;
+	return __raw_readl(priv->regs + RNG_STAT) & RNG_AVAIL_MASK;
 }
 
 static int bcm63xx_rng_data_read(struct hwrng *rng, u32 *data)
 {
 	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
 
-	*data = bcm_readl(priv->regs + RNG_DATA);
+	*data = __raw_readl(priv->regs + RNG_DATA);
 
 	return 4;
 }
@@ -72,94 +87,53 @@ static int bcm63xx_rng_probe(struct platform_device *pdev)
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!r) {
 		dev_err(&pdev->dev, "no iomem resource\n");
-		ret = -ENXIO;
-		goto out;
+		return -ENXIO;
 	}
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&pdev->dev, "no memory for private structure\n");
-		ret = -ENOMEM;
-		goto out;
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->rng.name = pdev->name;
+	priv->rng.init = bcm63xx_rng_init;
+	priv->rng.cleanup = bcm63xx_rng_cleanup;
+	prov->rng.data_present = bcm63xx_rng_data_present;
+	priv->rng.data_read = bcm63xx_rng_data_read;
+
+	priv->clk = devm_clk_get(&pdev->dev, "ipsec");
+	if (IS_ERR(priv->clk)) {
+		error = PTR_ERR(priv->clk);
+		dev_err(&pdev->dev, "no clock for device: %d\n", error);
+		return error;
 	}
 
-	rng = kzalloc(sizeof(*rng), GFP_KERNEL);
-	if (!rng) {
-		dev_err(&pdev->dev, "no memory for rng structure\n");
-		ret = -ENOMEM;
-		goto out_free_priv;
-	}
-
-	platform_set_drvdata(pdev, rng);
-	rng->priv = (unsigned long)priv;
-	rng->name = pdev->name;
-	rng->init = bcm63xx_rng_init;
-	rng->cleanup = bcm63xx_rng_cleanup;
-	rng->data_present = bcm63xx_rng_data_present;
-	rng->data_read = bcm63xx_rng_data_read;
-
-	clk = clk_get(&pdev->dev, "ipsec");
-	if (IS_ERR(clk)) {
-		dev_err(&pdev->dev, "no clock for device\n");
-		ret = PTR_ERR(clk);
-		goto out_free_rng;
-	}
-
-	priv->clk = clk;
-
 	if (!devm_request_mem_region(&pdev->dev, r->start,
 					resource_size(r), pdev->name)) {
 		dev_err(&pdev->dev, "request mem failed");
-		ret = -ENOMEM;
-		goto out_free_rng;
+		return -EBUSY;
 	}
 
 	priv->regs = devm_ioremap_nocache(&pdev->dev, r->start,
 					resource_size(r));
 	if (!priv->regs) {
 		dev_err(&pdev->dev, "ioremap failed");
-		ret = -ENOMEM;
-		goto out_free_rng;
+		return -ENOMEM;
 	}
 
-	clk_enable(clk);
-
-	ret = hwrng_register(rng);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to register rng device\n");
-		goto out_clk_disable;
+	error = devm_hwrng_register(&pdev->dev, &priv->rng);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register rng device: %d\n",
+			error);
+		return error;
 	}
 
 	dev_info(&pdev->dev, "registered RNG driver\n");
 
 	return 0;
-
-out_clk_disable:
-	clk_disable(clk);
-out_free_rng:
-	kfree(rng);
-out_free_priv:
-	kfree(priv);
-out:
-	return ret;
-}
-
-static int bcm63xx_rng_remove(struct platform_device *pdev)
-{
-	struct hwrng *rng = platform_get_drvdata(pdev);
-	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
-
-	hwrng_unregister(rng);
-	clk_disable(priv->clk);
-	kfree(priv);
-	kfree(rng);
-
-	return 0;
 }
 
 static struct platform_driver bcm63xx_rng_driver = {
 	.probe		= bcm63xx_rng_probe,
-	.remove		= bcm63xx_rng_remove,
 	.driver		= {
 		.name	= "bcm63xx-rng",
 	},
diff --git a/drivers/char/hw_random/core.c b/drivers/char/hw_random/core.c
index 32a8a867f7f8..571ef61f8ea9 100644
--- a/drivers/char/hw_random/core.c
+++ b/drivers/char/hw_random/core.c
@@ -179,7 +179,8 @@ skip_init:
 	add_early_randomness(rng);
 
 	current_quality = rng->quality ? : default_quality;
-	current_quality &= 1023;
+	if (current_quality > 1024)
+		current_quality = 1024;
 
 	if (current_quality == 0 && hwrng_fill)
 		kthread_stop(hwrng_fill);
@@ -536,6 +537,48 @@ void hwrng_unregister(struct hwrng *rng)
 }
 EXPORT_SYMBOL_GPL(hwrng_unregister);
 
+static void devm_hwrng_release(struct device *dev, void *res)
+{
+	hwrng_unregister(*(struct hwrng **)res);
+}
+
+static int devm_hwrng_match(struct device *dev, void *res, void *data)
+{
+	struct hwrng **r = res;
+
+	if (WARN_ON(!r || !*r))
+		return 0;
+
+	return *r == data;
+}
+
+int devm_hwrng_register(struct device *dev, struct hwrng *rng)
+{
+	struct hwrng **ptr;
+	int error;
+
+	ptr = devres_alloc(devm_hwrng_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return -ENOMEM;
+
+	error = hwrng_register(rng);
+	if (error) {
+		devres_free(ptr);
+		return error;
+	}
+
+	*ptr = rng;
+	devres_add(dev, ptr);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devm_hwrng_register);
+
+void devm_hwrng_unregister(struct device *dev, struct hwrng *rng)
+{
+	devres_release(dev, devm_hwrng_release, devm_hwrng_match, rng);
+}
+EXPORT_SYMBOL_GPL(devm_hwrng_unregister);
+
 static int __init hwrng_modinit(void)
 {
 	return register_miscdev();
diff --git a/drivers/char/hw_random/exynos-rng.c b/drivers/char/hw_random/exynos-rng.c
index fed0830bf724..dc4701fd814f 100644
--- a/drivers/char/hw_random/exynos-rng.c
+++ b/drivers/char/hw_random/exynos-rng.c
@@ -131,16 +131,7 @@ static int exynos_rng_probe(struct platform_device *pdev)
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
-	return hwrng_register(&exynos_rng->rng);
-}
-
-static int exynos_rng_remove(struct platform_device *pdev)
-{
-	struct exynos_rng *exynos_rng = platform_get_drvdata(pdev);
-
-	hwrng_unregister(&exynos_rng->rng);
-
-	return 0;
+	return devm_hwrng_register(&pdev->dev, &exynos_rng->rng);
 }
 
 #ifdef CONFIG_PM
@@ -172,7 +163,6 @@ static struct platform_driver exynos_rng_driver = {
 		.pm	= &exynos_rng_pm_ops,
 	},
 	.probe		= exynos_rng_probe,
-	.remove		= exynos_rng_remove,
 };
 
 module_platform_driver(exynos_rng_driver);
diff --git a/drivers/char/hw_random/iproc-rng200.c b/drivers/char/hw_random/iproc-rng200.c
new file mode 100644
index 000000000000..3eaf7cb96d36
--- /dev/null
+++ b/drivers/char/hw_random/iproc-rng200.c
@@ -0,0 +1,239 @@
+/*
+* 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.
+*/
+/*
+ * DESCRIPTION: The Broadcom iProc RNG200 Driver
+ */
+
+#include <linux/hw_random.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+
+/* Registers */
+#define RNG_CTRL_OFFSET					0x00
+#define RNG_CTRL_RNG_RBGEN_MASK				0x00001FFF
+#define RNG_CTRL_RNG_RBGEN_ENABLE			0x00000001
+#define RNG_CTRL_RNG_RBGEN_DISABLE			0x00000000
+
+#define RNG_SOFT_RESET_OFFSET				0x04
+#define RNG_SOFT_RESET					0x00000001
+
+#define RBG_SOFT_RESET_OFFSET				0x08
+#define RBG_SOFT_RESET					0x00000001
+
+#define RNG_INT_STATUS_OFFSET				0x18
+#define RNG_INT_STATUS_MASTER_FAIL_LOCKOUT_IRQ_MASK	0x80000000
+#define RNG_INT_STATUS_STARTUP_TRANSITIONS_MET_IRQ_MASK	0x00020000
+#define RNG_INT_STATUS_NIST_FAIL_IRQ_MASK		0x00000020
+#define RNG_INT_STATUS_TOTAL_BITS_COUNT_IRQ_MASK	0x00000001
+
+#define RNG_FIFO_DATA_OFFSET				0x20
+
+#define RNG_FIFO_COUNT_OFFSET				0x24
+#define RNG_FIFO_COUNT_RNG_FIFO_COUNT_MASK		0x000000FF
+
+struct iproc_rng200_dev {
+	struct hwrng rng;
+	void __iomem *base;
+};
+
+#define to_rng_priv(rng)	container_of(rng, struct iproc_rng200_dev, rng)
+
+static void iproc_rng200_restart(void __iomem *rng_base)
+{
+	uint32_t val;
+
+	/* Disable RBG */
+	val = ioread32(rng_base + RNG_CTRL_OFFSET);
+	val &= ~RNG_CTRL_RNG_RBGEN_MASK;
+	val |= RNG_CTRL_RNG_RBGEN_DISABLE;
+	iowrite32(val, rng_base + RNG_CTRL_OFFSET);
+
+	/* Clear all interrupt status */
+	iowrite32(0xFFFFFFFFUL, rng_base + RNG_INT_STATUS_OFFSET);
+
+	/* Reset RNG and RBG */
+	val = ioread32(rng_base + RBG_SOFT_RESET_OFFSET);
+	val |= RBG_SOFT_RESET;
+	iowrite32(val, rng_base + RBG_SOFT_RESET_OFFSET);
+
+	val = ioread32(rng_base + RNG_SOFT_RESET_OFFSET);
+	val |= RNG_SOFT_RESET;
+	iowrite32(val, rng_base + RNG_SOFT_RESET_OFFSET);
+
+	val = ioread32(rng_base + RNG_SOFT_RESET_OFFSET);
+	val &= ~RNG_SOFT_RESET;
+	iowrite32(val, rng_base + RNG_SOFT_RESET_OFFSET);
+
+	val = ioread32(rng_base + RBG_SOFT_RESET_OFFSET);
+	val &= ~RBG_SOFT_RESET;
+	iowrite32(val, rng_base + RBG_SOFT_RESET_OFFSET);
+
+	/* Enable RBG */
+	val = ioread32(rng_base + RNG_CTRL_OFFSET);
+	val &= ~RNG_CTRL_RNG_RBGEN_MASK;
+	val |= RNG_CTRL_RNG_RBGEN_ENABLE;
+	iowrite32(val, rng_base + RNG_CTRL_OFFSET);
+}
+
+static int iproc_rng200_read(struct hwrng *rng, void *buf, size_t max,
+			     bool wait)
+{
+	struct iproc_rng200_dev *priv = to_rng_priv(rng);
+	uint32_t num_remaining = max;
+	uint32_t status;
+
+	#define MAX_RESETS_PER_READ	1
+	uint32_t num_resets = 0;
+
+	#define MAX_IDLE_TIME	(1 * HZ)
+	unsigned long idle_endtime = jiffies + MAX_IDLE_TIME;
+
+	while ((num_remaining > 0) && time_before(jiffies, idle_endtime)) {
+
+		/* Is RNG sane? If not, reset it. */
+		status = ioread32(priv->base + RNG_INT_STATUS_OFFSET);
+		if ((status & (RNG_INT_STATUS_MASTER_FAIL_LOCKOUT_IRQ_MASK |
+			RNG_INT_STATUS_NIST_FAIL_IRQ_MASK)) != 0) {
+
+			if (num_resets >= MAX_RESETS_PER_READ)
+				return max - num_remaining;
+
+			iproc_rng200_restart(priv->base);
+			num_resets++;
+		}
+
+		/* Are there any random numbers available? */
+		if ((ioread32(priv->base + RNG_FIFO_COUNT_OFFSET) &
+				RNG_FIFO_COUNT_RNG_FIFO_COUNT_MASK) > 0) {
+
+			if (num_remaining >= sizeof(uint32_t)) {
+				/* Buffer has room to store entire word */
+				*(uint32_t *)buf = ioread32(priv->base +
+							RNG_FIFO_DATA_OFFSET);
+				buf += sizeof(uint32_t);
+				num_remaining -= sizeof(uint32_t);
+			} else {
+				/* Buffer can only store partial word */
+				uint32_t rnd_number = ioread32(priv->base +
+							RNG_FIFO_DATA_OFFSET);
+				memcpy(buf, &rnd_number, num_remaining);
+				buf += num_remaining;
+				num_remaining = 0;
+			}
+
+			/* Reset the IDLE timeout */
+			idle_endtime = jiffies + MAX_IDLE_TIME;
+		} else {
+			if (!wait)
+				/* Cannot wait, return immediately */
+				return max - num_remaining;
+
+			/* Can wait, give others chance to run */
+			usleep_range(min(num_remaining * 10, 500U), 500);
+		}
+	}
+
+	return max - num_remaining;
+}
+
+static int iproc_rng200_init(struct hwrng *rng)
+{
+	struct iproc_rng200_dev *priv = to_rng_priv(rng);
+	uint32_t val;
+
+	/* Setup RNG. */
+	val = ioread32(priv->base + RNG_CTRL_OFFSET);
+	val &= ~RNG_CTRL_RNG_RBGEN_MASK;
+	val |= RNG_CTRL_RNG_RBGEN_ENABLE;
+	iowrite32(val, priv->base + RNG_CTRL_OFFSET);
+
+	return 0;
+}
+
+static void iproc_rng200_cleanup(struct hwrng *rng)
+{
+	struct iproc_rng200_dev *priv = to_rng_priv(rng);
+	uint32_t val;
+
+	/* Disable RNG hardware */
+	val = ioread32(priv->base + RNG_CTRL_OFFSET);
+	val &= ~RNG_CTRL_RNG_RBGEN_MASK;
+	val |= RNG_CTRL_RNG_RBGEN_DISABLE;
+	iowrite32(val, priv->base + RNG_CTRL_OFFSET);
+}
+
+static int iproc_rng200_probe(struct platform_device *pdev)
+{
+	struct iproc_rng200_dev *priv;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	/* Map peripheral */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "failed to get rng resources\n");
+		return -EINVAL;
+	}
+
+	priv->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->base)) {
+		dev_err(dev, "failed to remap rng regs\n");
+		return PTR_ERR(priv->base);
+	}
+
+	priv->rng.name = "iproc-rng200",
+	priv->rng.read = iproc_rng200_read,
+	priv->rng.init = iproc_rng200_init,
+	priv->rng.cleanup = iproc_rng200_cleanup,
+
+	/* Register driver */
+	ret = devm_hwrng_register(dev, &priv->rng);
+	if (ret) {
+		dev_err(dev, "hwrng registration failed\n");
+		return ret;
+	}
+
+	dev_info(dev, "hwrng registered\n");
+
+	return 0;
+}
+
+static const struct of_device_id iproc_rng200_of_match[] = {
+	{ .compatible = "brcm,iproc-rng200", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, iproc_rng200_of_match);
+
+static struct platform_driver iproc_rng200_driver = {
+	.driver = {
+		.name		= "iproc-rng200",
+		.of_match_table = iproc_rng200_of_match,
+	},
+	.probe		= iproc_rng200_probe,
+};
+module_platform_driver(iproc_rng200_driver);
+
+MODULE_AUTHOR("Broadcom");
+MODULE_DESCRIPTION("iProc RNG200 Random Number Generator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/char/hw_random/msm-rng.c b/drivers/char/hw_random/msm-rng.c
index cea1c703d62f..96fb986402eb 100644
--- a/drivers/char/hw_random/msm-rng.c
+++ b/drivers/char/hw_random/msm-rng.c
@@ -157,7 +157,7 @@ static int msm_rng_probe(struct platform_device *pdev)
 	rng->hwrng.cleanup = msm_rng_cleanup,
 	rng->hwrng.read = msm_rng_read,
 
-	ret = hwrng_register(&rng->hwrng);
+	ret = devm_hwrng_register(&pdev->dev, &rng->hwrng);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register hwrng\n");
 		return ret;
@@ -166,14 +166,6 @@ static int msm_rng_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int msm_rng_remove(struct platform_device *pdev)
-{
-	struct msm_rng *rng = platform_get_drvdata(pdev);
-
-	hwrng_unregister(&rng->hwrng);
-	return 0;
-}
-
 static const struct of_device_id msm_rng_of_match[] = {
 	{ .compatible = "qcom,prng", },
 	{}
@@ -182,7 +174,6 @@ MODULE_DEVICE_TABLE(of, msm_rng_of_match);
 
 static struct platform_driver msm_rng_driver = {
 	.probe = msm_rng_probe,
-	.remove = msm_rng_remove,
 	.driver = {
 		.name = KBUILD_MODNAME,
 		.of_match_table = of_match_ptr(msm_rng_of_match),
diff --git a/drivers/char/hw_random/octeon-rng.c b/drivers/char/hw_random/octeon-rng.c
index be1c3f607398..6234a4a19b56 100644
--- a/drivers/char/hw_random/octeon-rng.c
+++ b/drivers/char/hw_random/octeon-rng.c
@@ -105,7 +105,7 @@ static int octeon_rng_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __exit octeon_rng_remove(struct platform_device *pdev)
+static int octeon_rng_remove(struct platform_device *pdev)
 {
 	struct hwrng *rng = platform_get_drvdata(pdev);
 
@@ -119,7 +119,7 @@ static struct platform_driver octeon_rng_driver = {
 		.name		= "octeon_rng",
 	},
 	.probe		= octeon_rng_probe,
-	.remove		= __exit_p(octeon_rng_remove),
+	.remove		= octeon_rng_remove,
 };
 
 module_platform_driver(octeon_rng_driver);
diff --git a/drivers/char/hw_random/omap-rng.c b/drivers/char/hw_random/omap-rng.c
index d14dcf788f17..8a1432e8bb80 100644
--- a/drivers/char/hw_random/omap-rng.c
+++ b/drivers/char/hw_random/omap-rng.c
@@ -236,7 +236,7 @@ static int omap4_rng_init(struct omap_rng_dev *priv)
 	u32 val;
 
 	/* Return if RNG is already running. */
-	if (omap_rng_read(priv, RNG_CONFIG_REG) & RNG_CONTROL_ENABLE_TRNG_MASK)
+	if (omap_rng_read(priv, RNG_CONTROL_REG) & RNG_CONTROL_ENABLE_TRNG_MASK)
 		return 0;
 
 	val = RNG_CONFIG_MIN_REFIL_CYCLES << RNG_CONFIG_MIN_REFIL_CYCLES_SHIFT;
@@ -262,7 +262,7 @@ static void omap4_rng_cleanup(struct omap_rng_dev *priv)
 
 	val = omap_rng_read(priv, RNG_CONTROL_REG);
 	val &= ~RNG_CONTROL_ENABLE_TRNG_MASK;
-	omap_rng_write(priv, RNG_CONFIG_REG, val);
+	omap_rng_write(priv, RNG_CONTROL_REG, val);
 }
 
 static irqreturn_t omap4_rng_irq(int irq, void *dev_id)
@@ -408,7 +408,7 @@ err_ioremap:
 	return ret;
 }
 
-static int __exit omap_rng_remove(struct platform_device *pdev)
+static int omap_rng_remove(struct platform_device *pdev)
 {
 	struct omap_rng_dev *priv = platform_get_drvdata(pdev);
 
@@ -422,9 +422,7 @@ static int __exit omap_rng_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-
-static int omap_rng_suspend(struct device *dev)
+static int __maybe_unused omap_rng_suspend(struct device *dev)
 {
 	struct omap_rng_dev *priv = dev_get_drvdata(dev);
 
@@ -434,7 +432,7 @@ static int omap_rng_suspend(struct device *dev)
 	return 0;
 }
 
-static int omap_rng_resume(struct device *dev)
+static int __maybe_unused omap_rng_resume(struct device *dev)
 {
 	struct omap_rng_dev *priv = dev_get_drvdata(dev);
 
@@ -445,22 +443,15 @@ static int omap_rng_resume(struct device *dev)
 }
 
 static SIMPLE_DEV_PM_OPS(omap_rng_pm, omap_rng_suspend, omap_rng_resume);
-#define	OMAP_RNG_PM	(&omap_rng_pm)
-
-#else
-
-#define	OMAP_RNG_PM	NULL
-
-#endif
 
 static struct platform_driver omap_rng_driver = {
 	.driver = {
 		.name		= "omap_rng",
-		.pm		= OMAP_RNG_PM,
+		.pm		= &omap_rng_pm,
 		.of_match_table = of_match_ptr(omap_rng_of_match),
 	},
 	.probe		= omap_rng_probe,
-	.remove		= __exit_p(omap_rng_remove),
+	.remove		= omap_rng_remove,
 };
 
 module_platform_driver(omap_rng_driver);
diff --git a/drivers/char/hw_random/pseries-rng.c b/drivers/char/hw_random/pseries-rng.c
index bcf86f91800a..63ce51d09af1 100644
--- a/drivers/char/hw_random/pseries-rng.c
+++ b/drivers/char/hw_random/pseries-rng.c
@@ -61,13 +61,13 @@ static struct hwrng pseries_rng = {
 	.read		= pseries_rng_read,
 };
 
-static int __init pseries_rng_probe(struct vio_dev *dev,
+static int pseries_rng_probe(struct vio_dev *dev,
 		const struct vio_device_id *id)
 {
 	return hwrng_register(&pseries_rng);
 }
 
-static int __exit pseries_rng_remove(struct vio_dev *dev)
+static int pseries_rng_remove(struct vio_dev *dev)
 {
 	hwrng_unregister(&pseries_rng);
 	return 0;
diff --git a/drivers/char/hw_random/xgene-rng.c b/drivers/char/hw_random/xgene-rng.c
index 23caa05380a8..c37cf754a985 100644
--- a/drivers/char/hw_random/xgene-rng.c
+++ b/drivers/char/hw_random/xgene-rng.c
@@ -21,6 +21,7 @@
  *
  */
 
+#include <linux/acpi.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/hw_random.h>
@@ -310,6 +311,14 @@ static int xgene_rng_init(struct hwrng *rng)
 	return 0;
 }
 
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id xgene_rng_acpi_match[] = {
+	{ "APMC0D18", },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, xgene_rng_acpi_match);
+#endif
+
 static struct hwrng xgene_rng_func = {
 	.name		= "xgene-rng",
 	.init		= xgene_rng_init,
@@ -415,6 +424,7 @@ static struct platform_driver xgene_rng_driver = {
 	.driver = {
 		.name		= "xgene-rng",
 		.of_match_table = xgene_rng_of_match,
+		.acpi_match_table = ACPI_PTR(xgene_rng_acpi_match),
 	},
 };
 
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/mem.c b/drivers/char/mem.c
index 297110c12635..6b1721f978c2 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -26,7 +26,7 @@
 #include <linux/pfn.h>
 #include <linux/export.h>
 #include <linux/io.h>
-#include <linux/aio.h>
+#include <linux/uio.h>
 
 #include <linux/uaccess.h>
 
@@ -607,16 +607,16 @@ static ssize_t write_null(struct file *file, const char __user *buf,
 	return count;
 }
 
-static ssize_t aio_read_null(struct kiocb *iocb, const struct iovec *iov,
-			     unsigned long nr_segs, loff_t pos)
+static ssize_t read_iter_null(struct kiocb *iocb, struct iov_iter *to)
 {
 	return 0;
 }
 
-static ssize_t aio_write_null(struct kiocb *iocb, const struct iovec *iov,
-			      unsigned long nr_segs, loff_t pos)
+static ssize_t write_iter_null(struct kiocb *iocb, struct iov_iter *from)
 {
-	return iov_length(iov, nr_segs);
+	size_t count = iov_iter_count(from);
+	iov_iter_advance(from, count);
+	return count;
 }
 
 static int pipe_to_null(struct pipe_inode_info *info, struct pipe_buffer *buf,
@@ -718,7 +718,7 @@ static int open_port(struct inode *inode, struct file *filp)
 #define zero_lseek	null_lseek
 #define full_lseek      null_lseek
 #define write_zero	write_null
-#define aio_write_zero	aio_write_null
+#define write_iter_zero	write_iter_null
 #define open_mem	open_port
 #define open_kmem	open_mem
 
@@ -750,8 +750,8 @@ static const struct file_operations null_fops = {
 	.llseek		= null_lseek,
 	.read		= read_null,
 	.write		= write_null,
-	.aio_read	= aio_read_null,
-	.aio_write	= aio_write_null,
+	.read_iter	= read_iter_null,
+	.write_iter	= write_iter_null,
 	.splice_write	= splice_write_null,
 };
 
@@ -764,10 +764,9 @@ static const struct file_operations __maybe_unused port_fops = {
 
 static const struct file_operations zero_fops = {
 	.llseek		= zero_lseek,
-	.read		= new_sync_read,
 	.write		= write_zero,
 	.read_iter	= read_iter_zero,
-	.aio_write	= aio_write_zero,
+	.write_iter	= write_iter_zero,
 	.mmap		= mmap_zero,
 #ifndef CONFIG_MMU
 	.mmap_capabilities = zero_mmap_capabilities,
@@ -776,7 +775,6 @@ static const struct file_operations zero_fops = {
 
 static const struct file_operations full_fops = {
 	.llseek		= full_lseek,
-	.read		= new_sync_read,
 	.read_iter	= read_iter_zero,
 	.write		= write_full,
 };
diff --git a/drivers/char/raw.c b/drivers/char/raw.c
index 6e29bf2db536..5fc291c6157e 100644
--- a/drivers/char/raw.c
+++ b/drivers/char/raw.c
@@ -282,9 +282,7 @@ static long raw_ctl_compat_ioctl(struct file *file, unsigned int cmd,
 #endif
 
 static const struct file_operations raw_fops = {
-	.read		= new_sync_read,
 	.read_iter	= blkdev_read_iter,
-	.write		= new_sync_write,
 	.write_iter	= blkdev_write_iter,
 	.fsync		= blkdev_fsync,
 	.open		= raw_open,
diff --git a/drivers/char/tile-srom.c b/drivers/char/tile-srom.c
index 02e76ac6d282..69f6b4acc377 100644
--- a/drivers/char/tile-srom.c
+++ b/drivers/char/tile-srom.c
@@ -27,7 +27,6 @@
 #include <linux/types.h>	/* size_t */
 #include <linux/proc_fs.h>
 #include <linux/fcntl.h>	/* O_ACCMODE */
-#include <linux/aio.h>
 #include <linux/pagemap.h>
 #include <linux/hugetlb.h>
 #include <linux/uaccess.h>
diff --git a/drivers/char/tpm/Kconfig b/drivers/char/tpm/Kconfig
index 9d4e37549eb2..3b84a8b1bfbe 100644
--- a/drivers/char/tpm/Kconfig
+++ b/drivers/char/tpm/Kconfig
@@ -25,13 +25,14 @@ menuconfig TCG_TPM
 if TCG_TPM
 
 config TCG_TIS
-	tristate "TPM Interface Specification 1.2 Interface"
+	tristate "TPM Interface Specification 1.2 Interface / TPM 2.0 FIFO Interface"
 	depends on X86
 	---help---
 	  If you have a TPM security chip that is compliant with the
-	  TCG TIS 1.2 TPM specification say Yes and it will be accessible
-	  from within Linux.  To compile this driver as a module, choose
-	  M here; the module will be called tpm_tis.
+	  TCG TIS 1.2 TPM specification (TPM1.2) or the TCG PTP FIFO
+	  specification (TPM2.0) say Yes and it will be accessible from
+	  within Linux. To compile this driver as a module, choose  M here;
+	  the module will be called tpm_tis.
 
 config TCG_TIS_I2C_ATMEL
 	tristate "TPM Interface Specification 1.2 Interface (I2C - Atmel)"
@@ -100,16 +101,6 @@ config TCG_IBMVTPM
 	  will be accessible from within Linux.  To compile this driver
 	  as a module, choose M here; the module will be called tpm_ibmvtpm.
 
-config TCG_TIS_I2C_ST33
-	tristate "TPM Interface Specification 1.2 Interface (I2C - STMicroelectronics)"
-	depends on I2C
-	depends on GPIOLIB
-	---help---
-	  If you have a TPM security chip from STMicroelectronics working with
-	  an I2C bus say Yes and it will be accessible from within Linux.
-	  To compile this driver as a module, choose M here; the module will be
-	  called tpm_i2c_stm_st33.
-
 config TCG_XEN
 	tristate "XEN TPM Interface"
 	depends on TCG_TPM && XEN
@@ -131,4 +122,5 @@ config TCG_CRB
 	  from within Linux.  To compile this driver as a module, choose
 	  M here; the module will be called tpm_crb.
 
+source "drivers/char/tpm/st33zp24/Kconfig"
 endif # TCG_TPM
diff --git a/drivers/char/tpm/Makefile b/drivers/char/tpm/Makefile
index 990cf183931d..56e8f1f3dc7e 100644
--- a/drivers/char/tpm/Makefile
+++ b/drivers/char/tpm/Makefile
@@ -20,6 +20,6 @@ obj-$(CONFIG_TCG_NSC) += tpm_nsc.o
 obj-$(CONFIG_TCG_ATMEL) += tpm_atmel.o
 obj-$(CONFIG_TCG_INFINEON) += tpm_infineon.o
 obj-$(CONFIG_TCG_IBMVTPM) += tpm_ibmvtpm.o
-obj-$(CONFIG_TCG_TIS_I2C_ST33) += tpm_i2c_stm_st33.o
+obj-$(CONFIG_TCG_TIS_ST33ZP24) += st33zp24/
 obj-$(CONFIG_TCG_XEN) += xen-tpmfront.o
 obj-$(CONFIG_TCG_CRB) += tpm_crb.o
diff --git a/drivers/char/tpm/st33zp24/Kconfig b/drivers/char/tpm/st33zp24/Kconfig
new file mode 100644
index 000000000000..09cb727864f0
--- /dev/null
+++ b/drivers/char/tpm/st33zp24/Kconfig
@@ -0,0 +1,30 @@
+config TCG_TIS_ST33ZP24
+	tristate "STMicroelectronics TPM Interface Specification 1.2 Interface"
+	depends on GPIOLIB
+	---help---
+	  STMicroelectronics ST33ZP24 core driver. It implements the core
+	  TPM1.2 logic and hooks into the TPM kernel APIs. Physical layers will
+	  register against it.
+
+	  To compile this driver as a module, choose m here. The module will be called
+	  tpm_st33zp24.
+
+config TCG_TIS_ST33ZP24_I2C
+	tristate "TPM 1.2 ST33ZP24 I2C support"
+	depends on TCG_TIS_ST33ZP24
+	depends on I2C
+	---help---
+	  This module adds support for the STMicroelectronics TPM security chip
+	  ST33ZP24 with i2c interface.
+	  To compile this driver as a module, choose M here; the module will be
+	  called tpm_st33zp24_i2c.
+
+config TCG_TIS_ST33ZP24_SPI
+	tristate "TPM 1.2 ST33ZP24 SPI support"
+	depends on TCG_TIS_ST33ZP24
+	depends on SPI
+	---help---
+	  This module adds support for the STMicroelectronics TPM security chip
+	  ST33ZP24 with spi interface.
+	  To compile this driver as a module, choose M here; the module will be
+	  called tpm_st33zp24_spi.
diff --git a/drivers/char/tpm/st33zp24/Makefile b/drivers/char/tpm/st33zp24/Makefile
new file mode 100644
index 000000000000..74a722e5e068
--- /dev/null
+++ b/drivers/char/tpm/st33zp24/Makefile
@@ -0,0 +1,12 @@
+#
+# Makefile for ST33ZP24 TPM 1.2 driver
+#
+
+tpm_st33zp24-objs = st33zp24.o
+obj-$(CONFIG_TCG_TIS_ST33ZP24) += tpm_st33zp24.o
+
+tpm_st33zp24_i2c-objs = i2c.o
+obj-$(CONFIG_TCG_TIS_ST33ZP24_I2C) += tpm_st33zp24_i2c.o
+
+tpm_st33zp24_spi-objs = spi.o
+obj-$(CONFIG_TCG_TIS_ST33ZP24_SPI) += tpm_st33zp24_spi.o
diff --git a/drivers/char/tpm/st33zp24/i2c.c b/drivers/char/tpm/st33zp24/i2c.c
new file mode 100644
index 000000000000..ad1ee180e0c2
--- /dev/null
+++ b/drivers/char/tpm/st33zp24/i2c.c
@@ -0,0 +1,276 @@
+/*
+ * STMicroelectronics TPM I2C Linux driver for TPM ST33ZP24
+ * Copyright (C) 2009 - 2015 STMicroelectronics
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/tpm.h>
+#include <linux/platform_data/st33zp24.h>
+
+#include "st33zp24.h"
+
+#define TPM_DUMMY_BYTE			0xAA
+
+struct st33zp24_i2c_phy {
+	struct i2c_client *client;
+	u8 buf[TPM_BUFSIZE + 1];
+	int io_lpcpd;
+};
+
+/*
+ * write8_reg
+ * Send byte to the TIS register according to the ST33ZP24 I2C protocol.
+ * @param: tpm_register, the tpm tis register where the data should be written
+ * @param: tpm_data, the tpm_data to write inside the tpm_register
+ * @param: tpm_size, The length of the data
+ * @return: Returns negative errno, or else the number of bytes written.
+ */
+static int write8_reg(void *phy_id, u8 tpm_register, u8 *tpm_data, int tpm_size)
+{
+	struct st33zp24_i2c_phy *phy = phy_id;
+
+	phy->buf[0] = tpm_register;
+	memcpy(phy->buf + 1, tpm_data, tpm_size);
+	return i2c_master_send(phy->client, phy->buf, tpm_size + 1);
+} /* write8_reg() */
+
+/*
+ * read8_reg
+ * Recv byte from the TIS register according to the ST33ZP24 I2C protocol.
+ * @param: tpm_register, the tpm tis register where the data should be read
+ * @param: tpm_data, the TPM response
+ * @param: tpm_size, tpm TPM response size to read.
+ * @return: number of byte read successfully: should be one if success.
+ */
+static int read8_reg(void *phy_id, u8 tpm_register, u8 *tpm_data, int tpm_size)
+{
+	struct st33zp24_i2c_phy *phy = phy_id;
+	u8 status = 0;
+	u8 data;
+
+	data = TPM_DUMMY_BYTE;
+	status = write8_reg(phy, tpm_register, &data, 1);
+	if (status == 2)
+		status = i2c_master_recv(phy->client, tpm_data, tpm_size);
+	return status;
+} /* read8_reg() */
+
+/*
+ * st33zp24_i2c_send
+ * Send byte to the TIS register according to the ST33ZP24 I2C protocol.
+ * @param: phy_id, the phy description
+ * @param: tpm_register, the tpm tis register where the data should be written
+ * @param: tpm_data, the tpm_data to write inside the tpm_register
+ * @param: tpm_size, the length of the data
+ * @return: number of byte written successfully: should be one if success.
+ */
+static int st33zp24_i2c_send(void *phy_id, u8 tpm_register, u8 *tpm_data,
+			     int tpm_size)
+{
+	return write8_reg(phy_id, tpm_register | TPM_WRITE_DIRECTION, tpm_data,
+			  tpm_size);
+}
+
+/*
+ * st33zp24_i2c_recv
+ * Recv byte from the TIS register according to the ST33ZP24 I2C protocol.
+ * @param: phy_id, the phy description
+ * @param: tpm_register, the tpm tis register where the data should be read
+ * @param: tpm_data, the TPM response
+ * @param: tpm_size, tpm TPM response size to read.
+ * @return: number of byte read successfully: should be one if success.
+ */
+static int st33zp24_i2c_recv(void *phy_id, u8 tpm_register, u8 *tpm_data,
+			     int tpm_size)
+{
+	return read8_reg(phy_id, tpm_register, tpm_data, tpm_size);
+}
+
+static const struct st33zp24_phy_ops i2c_phy_ops = {
+	.send = st33zp24_i2c_send,
+	.recv = st33zp24_i2c_recv,
+};
+
+#ifdef CONFIG_OF
+static int st33zp24_i2c_of_request_resources(struct st33zp24_i2c_phy *phy)
+{
+	struct device_node *pp;
+	struct i2c_client *client = phy->client;
+	int gpio;
+	int ret;
+
+	pp = client->dev.of_node;
+	if (!pp) {
+		dev_err(&client->dev, "No platform data\n");
+		return -ENODEV;
+	}
+
+	/* Get GPIO from device tree */
+	gpio = of_get_named_gpio(pp, "lpcpd-gpios", 0);
+	if (gpio < 0) {
+		dev_err(&client->dev,
+			"Failed to retrieve lpcpd-gpios from dts.\n");
+		phy->io_lpcpd = -1;
+		/*
+		 * lpcpd pin is not specified. This is not an issue as
+		 * power management can be also managed by TPM specific
+		 * commands. So leave with a success status code.
+		 */
+		return 0;
+	}
+	/* GPIO request and configuration */
+	ret = devm_gpio_request_one(&client->dev, gpio,
+			GPIOF_OUT_INIT_HIGH, "TPM IO LPCPD");
+	if (ret) {
+		dev_err(&client->dev, "Failed to request lpcpd pin\n");
+		return -ENODEV;
+	}
+	phy->io_lpcpd = gpio;
+
+	return 0;
+}
+#else
+static int st33zp24_i2c_of_request_resources(struct st33zp24_i2c_phy *phy)
+{
+	return -ENODEV;
+}
+#endif
+
+static int st33zp24_i2c_request_resources(struct i2c_client *client,
+					  struct st33zp24_i2c_phy *phy)
+{
+	struct st33zp24_platform_data *pdata;
+	int ret;
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->dev, "No platform data\n");
+		return -ENODEV;
+	}
+
+	/* store for late use */
+	phy->io_lpcpd = pdata->io_lpcpd;
+
+	if (gpio_is_valid(pdata->io_lpcpd)) {
+		ret = devm_gpio_request_one(&client->dev,
+				pdata->io_lpcpd, GPIOF_OUT_INIT_HIGH,
+				"TPM IO_LPCPD");
+		if (ret) {
+			dev_err(&client->dev, "Failed to request lpcpd pin\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * st33zp24_i2c_probe initialize the TPM device
+ * @param: client, the i2c_client drescription (TPM I2C description).
+ * @param: id, the i2c_device_id struct.
+ * @return: 0 in case of success.
+ *	 -1 in other case.
+ */
+static int st33zp24_i2c_probe(struct i2c_client *client,
+			      const struct i2c_device_id *id)
+{
+	int ret;
+	struct st33zp24_platform_data *pdata;
+	struct st33zp24_i2c_phy *phy;
+
+	if (!client) {
+		pr_info("%s: i2c client is NULL. Device not accessible.\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_info(&client->dev, "client not i2c capable\n");
+		return -ENODEV;
+	}
+
+	phy = devm_kzalloc(&client->dev, sizeof(struct st33zp24_i2c_phy),
+			   GFP_KERNEL);
+	if (!phy)
+		return -ENOMEM;
+
+	phy->client = client;
+	pdata = client->dev.platform_data;
+	if (!pdata && client->dev.of_node) {
+		ret = st33zp24_i2c_of_request_resources(phy);
+		if (ret)
+			return ret;
+	} else if (pdata) {
+		ret = st33zp24_i2c_request_resources(client, phy);
+		if (ret)
+			return ret;
+	}
+
+	return st33zp24_probe(phy, &i2c_phy_ops, &client->dev, client->irq,
+			      phy->io_lpcpd);
+}
+
+/*
+ * st33zp24_i2c_remove remove the TPM device
+ * @param: client, the i2c_client description (TPM I2C description).
+ * @return: 0 in case of success.
+ */
+static int st33zp24_i2c_remove(struct i2c_client *client)
+{
+	struct tpm_chip *chip = i2c_get_clientdata(client);
+
+	return st33zp24_remove(chip);
+}
+
+static const struct i2c_device_id st33zp24_i2c_id[] = {
+	{TPM_ST33_I2C, 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, st33zp24_i2c_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id of_st33zp24_i2c_match[] = {
+	{ .compatible = "st,st33zp24-i2c", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, of_st33zp24_i2c_match);
+#endif
+
+static SIMPLE_DEV_PM_OPS(st33zp24_i2c_ops, st33zp24_pm_suspend,
+			 st33zp24_pm_resume);
+
+static struct i2c_driver st33zp24_i2c_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = TPM_ST33_I2C,
+		.pm = &st33zp24_i2c_ops,
+		.of_match_table = of_match_ptr(of_st33zp24_i2c_match),
+	},
+	.probe = st33zp24_i2c_probe,
+	.remove = st33zp24_i2c_remove,
+	.id_table = st33zp24_i2c_id
+};
+
+module_i2c_driver(st33zp24_i2c_driver);
+
+MODULE_AUTHOR("TPM support (TPMsupport@list.st.com)");
+MODULE_DESCRIPTION("STM TPM 1.2 I2C ST33 Driver");
+MODULE_VERSION("1.3.0");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/tpm/st33zp24/spi.c b/drivers/char/tpm/st33zp24/spi.c
new file mode 100644
index 000000000000..f0184a1b0c1c
--- /dev/null
+++ b/drivers/char/tpm/st33zp24/spi.c
@@ -0,0 +1,399 @@
+/*
+ * STMicroelectronics TPM SPI Linux driver for TPM ST33ZP24
+ * Copyright (C) 2009 - 2015  STMicroelectronics
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/gpio.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/tpm.h>
+#include <linux/platform_data/st33zp24.h>
+
+#include "st33zp24.h"
+
+#define TPM_DATA_FIFO           0x24
+#define TPM_INTF_CAPABILITY     0x14
+
+#define TPM_DUMMY_BYTE		0x00
+
+#define MAX_SPI_LATENCY		15
+#define LOCALITY0		0
+
+#define ST33ZP24_OK					0x5A
+#define ST33ZP24_UNDEFINED_ERR				0x80
+#define ST33ZP24_BADLOCALITY				0x81
+#define ST33ZP24_TISREGISTER_UKNOWN			0x82
+#define ST33ZP24_LOCALITY_NOT_ACTIVATED			0x83
+#define ST33ZP24_HASH_END_BEFORE_HASH_START		0x84
+#define ST33ZP24_BAD_COMMAND_ORDER			0x85
+#define ST33ZP24_INCORECT_RECEIVED_LENGTH		0x86
+#define ST33ZP24_TPM_FIFO_OVERFLOW			0x89
+#define ST33ZP24_UNEXPECTED_READ_FIFO			0x8A
+#define ST33ZP24_UNEXPECTED_WRITE_FIFO			0x8B
+#define ST33ZP24_CMDRDY_SET_WHEN_PROCESSING_HASH_END	0x90
+#define ST33ZP24_DUMMY_BYTES				0x00
+
+/*
+ * TPM command can be up to 2048 byte, A TPM response can be up to
+ * 1024 byte.
+ * Between command and response, there are latency byte (up to 15
+ * usually on st33zp24 2 are enough).
+ *
+ * Overall when sending a command and expecting an answer we need if
+ * worst case:
+ * 2048 (for the TPM command) + 1024 (for the TPM answer).  We need
+ * some latency byte before the answer is available (max 15).
+ * We have 2048 + 1024 + 15.
+ */
+#define ST33ZP24_SPI_BUFFER_SIZE (TPM_BUFSIZE + (TPM_BUFSIZE / 2) +\
+				  MAX_SPI_LATENCY)
+
+
+struct st33zp24_spi_phy {
+	struct spi_device *spi_device;
+	struct spi_transfer spi_xfer;
+	u8 tx_buf[ST33ZP24_SPI_BUFFER_SIZE];
+	u8 rx_buf[ST33ZP24_SPI_BUFFER_SIZE];
+
+	int io_lpcpd;
+	int latency;
+};
+
+static int st33zp24_status_to_errno(u8 code)
+{
+	switch (code) {
+	case ST33ZP24_OK:
+		return 0;
+	case ST33ZP24_UNDEFINED_ERR:
+	case ST33ZP24_BADLOCALITY:
+	case ST33ZP24_TISREGISTER_UKNOWN:
+	case ST33ZP24_LOCALITY_NOT_ACTIVATED:
+	case ST33ZP24_HASH_END_BEFORE_HASH_START:
+	case ST33ZP24_BAD_COMMAND_ORDER:
+	case ST33ZP24_UNEXPECTED_READ_FIFO:
+	case ST33ZP24_UNEXPECTED_WRITE_FIFO:
+	case ST33ZP24_CMDRDY_SET_WHEN_PROCESSING_HASH_END:
+		return -EPROTO;
+	case ST33ZP24_INCORECT_RECEIVED_LENGTH:
+	case ST33ZP24_TPM_FIFO_OVERFLOW:
+		return -EMSGSIZE;
+	case ST33ZP24_DUMMY_BYTES:
+		return -ENOSYS;
+	}
+	return code;
+}
+
+/*
+ * st33zp24_spi_send
+ * Send byte to the TIS register according to the ST33ZP24 SPI protocol.
+ * @param: phy_id, the phy description
+ * @param: tpm_register, the tpm tis register where the data should be written
+ * @param: tpm_data, the tpm_data to write inside the tpm_register
+ * @param: tpm_size, The length of the data
+ * @return: should be zero if success else a negative error code.
+ */
+static int st33zp24_spi_send(void *phy_id, u8 tpm_register, u8 *tpm_data,
+			     int tpm_size)
+{
+	u8 data = 0;
+	int total_length = 0, nbr_dummy_bytes = 0, ret = 0;
+	struct st33zp24_spi_phy *phy = phy_id;
+	struct spi_device *dev = phy->spi_device;
+	u8 *tx_buf = (u8 *)phy->spi_xfer.tx_buf;
+	u8 *rx_buf = phy->spi_xfer.rx_buf;
+
+	/* Pre-Header */
+	data = TPM_WRITE_DIRECTION | LOCALITY0;
+	memcpy(tx_buf + total_length, &data, sizeof(data));
+	total_length++;
+	data = tpm_register;
+	memcpy(tx_buf + total_length, &data, sizeof(data));
+	total_length++;
+
+	if (tpm_size > 0 && tpm_register == TPM_DATA_FIFO) {
+		tx_buf[total_length++] = tpm_size >> 8;
+		tx_buf[total_length++] = tpm_size;
+	}
+
+	memcpy(&tx_buf[total_length], tpm_data, tpm_size);
+	total_length += tpm_size;
+
+	nbr_dummy_bytes = phy->latency;
+	memset(&tx_buf[total_length], TPM_DUMMY_BYTE, nbr_dummy_bytes);
+
+	phy->spi_xfer.len = total_length + nbr_dummy_bytes;
+
+	ret = spi_sync_transfer(dev, &phy->spi_xfer, 1);
+	if (ret == 0)
+		ret = rx_buf[total_length + nbr_dummy_bytes - 1];
+
+	return st33zp24_status_to_errno(ret);
+} /* st33zp24_spi_send() */
+
+/*
+ * read8_recv
+ * Recv byte from the TIS register according to the ST33ZP24 SPI protocol.
+ * @param: phy_id, the phy description
+ * @param: tpm_register, the tpm tis register where the data should be read
+ * @param: tpm_data, the TPM response
+ * @param: tpm_size, tpm TPM response size to read.
+ * @return: should be zero if success else a negative error code.
+ */
+static int read8_reg(void *phy_id, u8 tpm_register, u8 *tpm_data, int tpm_size)
+{
+	u8 data = 0;
+	int total_length = 0, nbr_dummy_bytes, ret;
+	struct st33zp24_spi_phy *phy = phy_id;
+	struct spi_device *dev = phy->spi_device;
+	u8 *tx_buf = (u8 *)phy->spi_xfer.tx_buf;
+	u8 *rx_buf = phy->spi_xfer.rx_buf;
+
+	/* Pre-Header */
+	data = LOCALITY0;
+	memcpy(tx_buf + total_length, &data, sizeof(data));
+	total_length++;
+	data = tpm_register;
+	memcpy(tx_buf + total_length, &data, sizeof(data));
+	total_length++;
+
+	nbr_dummy_bytes = phy->latency;
+	memset(&tx_buf[total_length], TPM_DUMMY_BYTE,
+	       nbr_dummy_bytes + tpm_size);
+
+	phy->spi_xfer.len = total_length + nbr_dummy_bytes + tpm_size;
+
+	/* header + status byte + size of the data + status byte */
+	ret = spi_sync_transfer(dev, &phy->spi_xfer, 1);
+	if (tpm_size > 0 && ret == 0) {
+		ret = rx_buf[total_length + nbr_dummy_bytes - 1];
+
+		memcpy(tpm_data, rx_buf + total_length + nbr_dummy_bytes,
+		       tpm_size);
+	}
+
+	return ret;
+} /* read8_reg() */
+
+/*
+ * st33zp24_spi_recv
+ * Recv byte from the TIS register according to the ST33ZP24 SPI protocol.
+ * @param: phy_id, the phy description
+ * @param: tpm_register, the tpm tis register where the data should be read
+ * @param: tpm_data, the TPM response
+ * @param: tpm_size, tpm TPM response size to read.
+ * @return: number of byte read successfully: should be one if success.
+ */
+static int st33zp24_spi_recv(void *phy_id, u8 tpm_register, u8 *tpm_data,
+			     int tpm_size)
+{
+	int ret;
+
+	ret = read8_reg(phy_id, tpm_register, tpm_data, tpm_size);
+	if (!st33zp24_status_to_errno(ret))
+		return tpm_size;
+	return ret;
+} /* st33zp24_spi_recv() */
+
+static int evaluate_latency(void *phy_id)
+{
+	struct st33zp24_spi_phy *phy = phy_id;
+	int latency = 1, status = 0;
+	u8 data = 0;
+
+	while (!status && latency < MAX_SPI_LATENCY) {
+		phy->latency = latency;
+		status = read8_reg(phy_id, TPM_INTF_CAPABILITY, &data, 1);
+		latency++;
+	}
+	return latency - 1;
+} /* evaluate_latency() */
+
+static const struct st33zp24_phy_ops spi_phy_ops = {
+	.send = st33zp24_spi_send,
+	.recv = st33zp24_spi_recv,
+};
+
+#ifdef CONFIG_OF
+static int tpm_stm_spi_of_request_resources(struct st33zp24_spi_phy *phy)
+{
+	struct device_node *pp;
+	struct spi_device *dev = phy->spi_device;
+	int gpio;
+	int ret;
+
+	pp = dev->dev.of_node;
+	if (!pp) {
+		dev_err(&dev->dev, "No platform data\n");
+		return -ENODEV;
+	}
+
+	/* Get GPIO from device tree */
+	gpio = of_get_named_gpio(pp, "lpcpd-gpios", 0);
+	if (gpio < 0) {
+		dev_err(&dev->dev,
+			"Failed to retrieve lpcpd-gpios from dts.\n");
+		phy->io_lpcpd = -1;
+		/*
+		 * lpcpd pin is not specified. This is not an issue as
+		 * power management can be also managed by TPM specific
+		 * commands. So leave with a success status code.
+		 */
+		return 0;
+	}
+	/* GPIO request and configuration */
+	ret = devm_gpio_request_one(&dev->dev, gpio,
+			GPIOF_OUT_INIT_HIGH, "TPM IO LPCPD");
+	if (ret) {
+		dev_err(&dev->dev, "Failed to request lpcpd pin\n");
+		return -ENODEV;
+	}
+	phy->io_lpcpd = gpio;
+
+	return 0;
+}
+#else
+static int tpm_stm_spi_of_request_resources(struct st33zp24_spi_phy *phy)
+{
+	return -ENODEV;
+}
+#endif
+
+static int tpm_stm_spi_request_resources(struct spi_device *dev,
+					 struct st33zp24_spi_phy *phy)
+{
+	struct st33zp24_platform_data *pdata;
+	int ret;
+
+	pdata = dev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&dev->dev, "No platform data\n");
+		return -ENODEV;
+	}
+
+	/* store for late use */
+	phy->io_lpcpd = pdata->io_lpcpd;
+
+	if (gpio_is_valid(pdata->io_lpcpd)) {
+		ret = devm_gpio_request_one(&dev->dev,
+				pdata->io_lpcpd, GPIOF_OUT_INIT_HIGH,
+				"TPM IO_LPCPD");
+		if (ret) {
+			dev_err(&dev->dev, "%s : reset gpio_request failed\n",
+				__FILE__);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * tpm_st33_spi_probe initialize the TPM device
+ * @param: dev, the spi_device drescription (TPM SPI description).
+ * @return: 0 in case of success.
+ *	 or a negative value describing the error.
+ */
+static int
+tpm_st33_spi_probe(struct spi_device *dev)
+{
+	int ret;
+	struct st33zp24_platform_data *pdata;
+	struct st33zp24_spi_phy *phy;
+
+	/* Check SPI platform functionnalities */
+	if (!dev) {
+		pr_info("%s: dev is NULL. Device is not accessible.\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	phy = devm_kzalloc(&dev->dev, sizeof(struct st33zp24_spi_phy),
+			   GFP_KERNEL);
+	if (!phy)
+		return -ENOMEM;
+
+	phy->spi_device = dev;
+	pdata = dev->dev.platform_data;
+	if (!pdata && dev->dev.of_node) {
+		ret = tpm_stm_spi_of_request_resources(phy);
+		if (ret)
+			return ret;
+	} else if (pdata) {
+		ret = tpm_stm_spi_request_resources(dev, phy);
+		if (ret)
+			return ret;
+	}
+
+	phy->spi_xfer.tx_buf = phy->tx_buf;
+	phy->spi_xfer.rx_buf = phy->rx_buf;
+
+	phy->latency = evaluate_latency(phy);
+	if (phy->latency <= 0)
+		return -ENODEV;
+
+	return st33zp24_probe(phy, &spi_phy_ops, &dev->dev, dev->irq,
+			      phy->io_lpcpd);
+}
+
+/*
+ * tpm_st33_spi_remove remove the TPM device
+ * @param: client, the spi_device drescription (TPM SPI description).
+ * @return: 0 in case of success.
+ */
+static int tpm_st33_spi_remove(struct spi_device *dev)
+{
+	struct tpm_chip *chip = spi_get_drvdata(dev);
+
+	return st33zp24_remove(chip);
+}
+
+static const struct spi_device_id st33zp24_spi_id[] = {
+	{TPM_ST33_SPI, 0},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, st33zp24_spi_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id of_st33zp24_spi_match[] = {
+	{ .compatible = "st,st33zp24-spi", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, of_st33zp24_spi_match);
+#endif
+
+static SIMPLE_DEV_PM_OPS(st33zp24_spi_ops, st33zp24_pm_suspend,
+			 st33zp24_pm_resume);
+
+static struct spi_driver tpm_st33_spi_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = TPM_ST33_SPI,
+		.pm = &st33zp24_spi_ops,
+		.of_match_table = of_match_ptr(of_st33zp24_spi_match),
+	},
+	.probe = tpm_st33_spi_probe,
+	.remove = tpm_st33_spi_remove,
+	.id_table = st33zp24_spi_id,
+};
+
+module_spi_driver(tpm_st33_spi_driver);
+
+MODULE_AUTHOR("TPM support (TPMsupport@list.st.com)");
+MODULE_DESCRIPTION("STM TPM 1.2 SPI ST33 Driver");
+MODULE_VERSION("1.3.0");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/tpm/st33zp24/st33zp24.c b/drivers/char/tpm/st33zp24/st33zp24.c
new file mode 100644
index 000000000000..8d626784cd8d
--- /dev/null
+++ b/drivers/char/tpm/st33zp24/st33zp24.c
@@ -0,0 +1,698 @@
+/*
+ * STMicroelectronics TPM Linux driver for TPM ST33ZP24
+ * Copyright (C) 2009 - 2015 STMicroelectronics
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/freezer.h>
+#include <linux/string.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/sched.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+
+#include "../tpm.h"
+#include "st33zp24.h"
+
+#define TPM_ACCESS			0x0
+#define TPM_STS				0x18
+#define TPM_DATA_FIFO			0x24
+#define TPM_INTF_CAPABILITY		0x14
+#define TPM_INT_STATUS			0x10
+#define TPM_INT_ENABLE			0x08
+
+#define LOCALITY0			0
+
+enum st33zp24_access {
+	TPM_ACCESS_VALID = 0x80,
+	TPM_ACCESS_ACTIVE_LOCALITY = 0x20,
+	TPM_ACCESS_REQUEST_PENDING = 0x04,
+	TPM_ACCESS_REQUEST_USE = 0x02,
+};
+
+enum st33zp24_status {
+	TPM_STS_VALID = 0x80,
+	TPM_STS_COMMAND_READY = 0x40,
+	TPM_STS_GO = 0x20,
+	TPM_STS_DATA_AVAIL = 0x10,
+	TPM_STS_DATA_EXPECT = 0x08,
+};
+
+enum st33zp24_int_flags {
+	TPM_GLOBAL_INT_ENABLE = 0x80,
+	TPM_INTF_CMD_READY_INT = 0x080,
+	TPM_INTF_FIFO_AVALAIBLE_INT = 0x040,
+	TPM_INTF_WAKE_UP_READY_INT = 0x020,
+	TPM_INTF_LOCALITY_CHANGE_INT = 0x004,
+	TPM_INTF_STS_VALID_INT = 0x002,
+	TPM_INTF_DATA_AVAIL_INT = 0x001,
+};
+
+enum tis_defaults {
+	TIS_SHORT_TIMEOUT = 750,
+	TIS_LONG_TIMEOUT = 2000,
+};
+
+struct st33zp24_dev {
+	struct tpm_chip *chip;
+	void *phy_id;
+	const struct st33zp24_phy_ops *ops;
+	u32 intrs;
+	int io_lpcpd;
+};
+
+/*
+ * clear_interruption clear the pending interrupt.
+ * @param: tpm_dev, the tpm device device.
+ * @return: the interrupt status value.
+ */
+static u8 clear_interruption(struct st33zp24_dev *tpm_dev)
+{
+	u8 interrupt;
+
+	tpm_dev->ops->recv(tpm_dev->phy_id, TPM_INT_STATUS, &interrupt, 1);
+	tpm_dev->ops->send(tpm_dev->phy_id, TPM_INT_STATUS, &interrupt, 1);
+	return interrupt;
+} /* clear_interruption() */
+
+/*
+ * st33zp24_cancel, cancel the current command execution or
+ * set STS to COMMAND READY.
+ * @param: chip, the tpm_chip description as specified in driver/char/tpm/tpm.h
+ */
+static void st33zp24_cancel(struct tpm_chip *chip)
+{
+	struct st33zp24_dev *tpm_dev;
+	u8 data;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	data = TPM_STS_COMMAND_READY;
+	tpm_dev->ops->send(tpm_dev->phy_id, TPM_STS, &data, 1);
+} /* st33zp24_cancel() */
+
+/*
+ * st33zp24_status return the TPM_STS register
+ * @param: chip, the tpm chip description
+ * @return: the TPM_STS register value.
+ */
+static u8 st33zp24_status(struct tpm_chip *chip)
+{
+	struct st33zp24_dev *tpm_dev;
+	u8 data;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	tpm_dev->ops->recv(tpm_dev->phy_id, TPM_STS, &data, 1);
+	return data;
+} /* st33zp24_status() */
+
+/*
+ * check_locality if the locality is active
+ * @param: chip, the tpm chip description
+ * @return: the active locality or -EACCESS.
+ */
+static int check_locality(struct tpm_chip *chip)
+{
+	struct st33zp24_dev *tpm_dev;
+	u8 data;
+	u8 status;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	status = tpm_dev->ops->recv(tpm_dev->phy_id, TPM_ACCESS, &data, 1);
+	if (status && (data &
+		(TPM_ACCESS_ACTIVE_LOCALITY | TPM_ACCESS_VALID)) ==
+		(TPM_ACCESS_ACTIVE_LOCALITY | TPM_ACCESS_VALID))
+		return chip->vendor.locality;
+
+	return -EACCES;
+} /* check_locality() */
+
+/*
+ * request_locality request the TPM locality
+ * @param: chip, the chip description
+ * @return: the active locality or negative value.
+ */
+static int request_locality(struct tpm_chip *chip)
+{
+	unsigned long stop;
+	long ret;
+	struct st33zp24_dev *tpm_dev;
+	u8 data;
+
+	if (check_locality(chip) == chip->vendor.locality)
+		return chip->vendor.locality;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	data = TPM_ACCESS_REQUEST_USE;
+	ret = tpm_dev->ops->send(tpm_dev->phy_id, TPM_ACCESS, &data, 1);
+	if (ret < 0)
+		return ret;
+
+	stop = jiffies + chip->vendor.timeout_a;
+
+	/* Request locality is usually effective after the request */
+	do {
+		if (check_locality(chip) >= 0)
+			return chip->vendor.locality;
+		msleep(TPM_TIMEOUT);
+	} while (time_before(jiffies, stop));
+
+	/* could not get locality */
+	return -EACCES;
+} /* request_locality() */
+
+/*
+ * release_locality release the active locality
+ * @param: chip, the tpm chip description.
+ */
+static void release_locality(struct tpm_chip *chip)
+{
+	struct st33zp24_dev *tpm_dev;
+	u8 data;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+	data = TPM_ACCESS_ACTIVE_LOCALITY;
+
+	tpm_dev->ops->send(tpm_dev->phy_id, TPM_ACCESS, &data, 1);
+}
+
+/*
+ * get_burstcount return the burstcount value
+ * @param: chip, the chip description
+ * return: the burstcount or negative value.
+ */
+static int get_burstcount(struct tpm_chip *chip)
+{
+	unsigned long stop;
+	int burstcnt, status;
+	u8 tpm_reg, temp;
+	struct st33zp24_dev *tpm_dev;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	stop = jiffies + chip->vendor.timeout_d;
+	do {
+		tpm_reg = TPM_STS + 1;
+		status = tpm_dev->ops->recv(tpm_dev->phy_id, tpm_reg, &temp, 1);
+		if (status < 0)
+			return -EBUSY;
+
+		tpm_reg = TPM_STS + 2;
+		burstcnt = temp;
+		status = tpm_dev->ops->recv(tpm_dev->phy_id, tpm_reg, &temp, 1);
+		if (status < 0)
+			return -EBUSY;
+
+		burstcnt |= temp << 8;
+		if (burstcnt)
+			return burstcnt;
+		msleep(TPM_TIMEOUT);
+	} while (time_before(jiffies, stop));
+	return -EBUSY;
+} /* get_burstcount() */
+
+
+/*
+ * wait_for_tpm_stat_cond
+ * @param: chip, chip description
+ * @param: mask, expected mask value
+ * @param: check_cancel, does the command expected to be canceled ?
+ * @param: canceled, did we received a cancel request ?
+ * @return: true if status == mask or if the command is canceled.
+ * false in other cases.
+ */
+static bool wait_for_tpm_stat_cond(struct tpm_chip *chip, u8 mask,
+				bool check_cancel, bool *canceled)
+{
+	u8 status = chip->ops->status(chip);
+
+	*canceled = false;
+	if ((status & mask) == mask)
+		return true;
+	if (check_cancel && chip->ops->req_canceled(chip, status)) {
+		*canceled = true;
+		return true;
+	}
+	return false;
+}
+
+/*
+ * wait_for_stat wait for a TPM_STS value
+ * @param: chip, the tpm chip description
+ * @param: mask, the value mask to wait
+ * @param: timeout, the timeout
+ * @param: queue, the wait queue.
+ * @param: check_cancel, does the command can be cancelled ?
+ * @return: the tpm status, 0 if success, -ETIME if timeout is reached.
+ */
+static int wait_for_stat(struct tpm_chip *chip, u8 mask, unsigned long timeout,
+			wait_queue_head_t *queue, bool check_cancel)
+{
+	unsigned long stop;
+	int ret = 0;
+	bool canceled = false;
+	bool condition;
+	u32 cur_intrs;
+	u8 status;
+	struct st33zp24_dev *tpm_dev;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	/* check current status */
+	status = st33zp24_status(chip);
+	if ((status & mask) == mask)
+		return 0;
+
+	stop = jiffies + timeout;
+
+	if (chip->vendor.irq) {
+		cur_intrs = tpm_dev->intrs;
+		clear_interruption(tpm_dev);
+		enable_irq(chip->vendor.irq);
+
+		do {
+			if (ret == -ERESTARTSYS && freezing(current))
+				clear_thread_flag(TIF_SIGPENDING);
+
+			timeout = stop - jiffies;
+			if ((long) timeout <= 0)
+				return -1;
+
+			ret = wait_event_interruptible_timeout(*queue,
+						cur_intrs != tpm_dev->intrs,
+						timeout);
+			clear_interruption(tpm_dev);
+			condition = wait_for_tpm_stat_cond(chip, mask,
+						check_cancel, &canceled);
+			if (ret >= 0 && condition) {
+				if (canceled)
+					return -ECANCELED;
+				return 0;
+			}
+		} while (ret == -ERESTARTSYS && freezing(current));
+
+		disable_irq_nosync(chip->vendor.irq);
+
+	} else {
+		do {
+			msleep(TPM_TIMEOUT);
+			status = chip->ops->status(chip);
+			if ((status & mask) == mask)
+				return 0;
+		} while (time_before(jiffies, stop));
+	}
+
+	return -ETIME;
+} /* wait_for_stat() */
+
+/*
+ * recv_data receive data
+ * @param: chip, the tpm chip description
+ * @param: buf, the buffer where the data are received
+ * @param: count, the number of data to receive
+ * @return: the number of bytes read from TPM FIFO.
+ */
+static int recv_data(struct tpm_chip *chip, u8 *buf, size_t count)
+{
+	int size = 0, burstcnt, len, ret;
+	struct st33zp24_dev *tpm_dev;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	while (size < count &&
+	       wait_for_stat(chip,
+			     TPM_STS_DATA_AVAIL | TPM_STS_VALID,
+			     chip->vendor.timeout_c,
+			     &chip->vendor.read_queue, true) == 0) {
+		burstcnt = get_burstcount(chip);
+		if (burstcnt < 0)
+			return burstcnt;
+		len = min_t(int, burstcnt, count - size);
+		ret = tpm_dev->ops->recv(tpm_dev->phy_id, TPM_DATA_FIFO,
+					 buf + size, len);
+		if (ret < 0)
+			return ret;
+
+		size += len;
+	}
+	return size;
+}
+
+/*
+ * tpm_ioserirq_handler the serirq irq handler
+ * @param: irq, the tpm chip description
+ * @param: dev_id, the description of the chip
+ * @return: the status of the handler.
+ */
+static irqreturn_t tpm_ioserirq_handler(int irq, void *dev_id)
+{
+	struct tpm_chip *chip = dev_id;
+	struct st33zp24_dev *tpm_dev;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	tpm_dev->intrs++;
+	wake_up_interruptible(&chip->vendor.read_queue);
+	disable_irq_nosync(chip->vendor.irq);
+
+	return IRQ_HANDLED;
+} /* tpm_ioserirq_handler() */
+
+/*
+ * st33zp24_send send TPM commands through the I2C bus.
+ *
+ * @param: chip, the tpm_chip description as specified in driver/char/tpm/tpm.h
+ * @param: buf,	the buffer to send.
+ * @param: count, the number of bytes to send.
+ * @return: In case of success the number of bytes sent.
+ *			In other case, a < 0 value describing the issue.
+ */
+static int st33zp24_send(struct tpm_chip *chip, unsigned char *buf,
+			 size_t len)
+{
+	u32 status, i, size, ordinal;
+	int burstcnt = 0;
+	int ret;
+	u8 data;
+	struct st33zp24_dev *tpm_dev;
+
+	if (!chip)
+		return -EBUSY;
+	if (len < TPM_HEADER_SIZE)
+		return -EBUSY;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	ret = request_locality(chip);
+	if (ret < 0)
+		return ret;
+
+	status = st33zp24_status(chip);
+	if ((status & TPM_STS_COMMAND_READY) == 0) {
+		st33zp24_cancel(chip);
+		if (wait_for_stat
+		    (chip, TPM_STS_COMMAND_READY, chip->vendor.timeout_b,
+		     &chip->vendor.read_queue, false) < 0) {
+			ret = -ETIME;
+			goto out_err;
+		}
+	}
+
+	for (i = 0; i < len - 1;) {
+		burstcnt = get_burstcount(chip);
+		if (burstcnt < 0)
+			return burstcnt;
+		size = min_t(int, len - i - 1, burstcnt);
+		ret = tpm_dev->ops->send(tpm_dev->phy_id, TPM_DATA_FIFO,
+					 buf + i, size);
+		if (ret < 0)
+			goto out_err;
+
+		i += size;
+	}
+
+	status = st33zp24_status(chip);
+	if ((status & TPM_STS_DATA_EXPECT) == 0) {
+		ret = -EIO;
+		goto out_err;
+	}
+
+	ret = tpm_dev->ops->send(tpm_dev->phy_id, TPM_DATA_FIFO,
+				 buf + len - 1, 1);
+	if (ret < 0)
+		goto out_err;
+
+	status = st33zp24_status(chip);
+	if ((status & TPM_STS_DATA_EXPECT) != 0) {
+		ret = -EIO;
+		goto out_err;
+	}
+
+	data = TPM_STS_GO;
+	ret = tpm_dev->ops->send(tpm_dev->phy_id, TPM_STS, &data, 1);
+	if (ret < 0)
+		goto out_err;
+
+	if (chip->vendor.irq) {
+		ordinal = be32_to_cpu(*((__be32 *) (buf + 6)));
+
+		ret = wait_for_stat(chip, TPM_STS_DATA_AVAIL | TPM_STS_VALID,
+				tpm_calc_ordinal_duration(chip, ordinal),
+				&chip->vendor.read_queue, false);
+		if (ret < 0)
+			goto out_err;
+	}
+
+	return len;
+out_err:
+	st33zp24_cancel(chip);
+	release_locality(chip);
+	return ret;
+}
+
+/*
+ * st33zp24_recv received TPM response through TPM phy.
+ * @param: chip, the tpm_chip description as specified in driver/char/tpm/tpm.h.
+ * @param: buf,	the buffer to store datas.
+ * @param: count, the number of bytes to send.
+ * @return: In case of success the number of bytes received.
+ *	    In other case, a < 0 value describing the issue.
+ */
+static int st33zp24_recv(struct tpm_chip *chip, unsigned char *buf,
+			    size_t count)
+{
+	int size = 0;
+	int expected;
+
+	if (!chip)
+		return -EBUSY;
+
+	if (count < TPM_HEADER_SIZE) {
+		size = -EIO;
+		goto out;
+	}
+
+	size = recv_data(chip, buf, TPM_HEADER_SIZE);
+	if (size < TPM_HEADER_SIZE) {
+		dev_err(&chip->dev, "Unable to read header\n");
+		goto out;
+	}
+
+	expected = be32_to_cpu(*(__be32 *)(buf + 2));
+	if (expected > count) {
+		size = -EIO;
+		goto out;
+	}
+
+	size += recv_data(chip, &buf[TPM_HEADER_SIZE],
+			expected - TPM_HEADER_SIZE);
+	if (size < expected) {
+		dev_err(&chip->dev, "Unable to read remainder of result\n");
+		size = -ETIME;
+	}
+
+out:
+	st33zp24_cancel(chip);
+	release_locality(chip);
+	return size;
+}
+
+/*
+ * st33zp24_req_canceled
+ * @param: chip, the tpm_chip description as specified in driver/char/tpm/tpm.h.
+ * @param: status, the TPM status.
+ * @return: Does TPM ready to compute a new command ? true.
+ */
+static bool st33zp24_req_canceled(struct tpm_chip *chip, u8 status)
+{
+	return (status == TPM_STS_COMMAND_READY);
+}
+
+static const struct tpm_class_ops st33zp24_tpm = {
+	.send = st33zp24_send,
+	.recv = st33zp24_recv,
+	.cancel = st33zp24_cancel,
+	.status = st33zp24_status,
+	.req_complete_mask = TPM_STS_DATA_AVAIL | TPM_STS_VALID,
+	.req_complete_val = TPM_STS_DATA_AVAIL | TPM_STS_VALID,
+	.req_canceled = st33zp24_req_canceled,
+};
+
+/*
+ * st33zp24_probe initialize the TPM device
+ * @param: client, the i2c_client drescription (TPM I2C description).
+ * @param: id, the i2c_device_id struct.
+ * @return: 0 in case of success.
+ *	 -1 in other case.
+ */
+int st33zp24_probe(void *phy_id, const struct st33zp24_phy_ops *ops,
+		   struct device *dev, int irq, int io_lpcpd)
+{
+	int ret;
+	u8 intmask = 0;
+	struct tpm_chip *chip;
+	struct st33zp24_dev *tpm_dev;
+
+	chip = tpmm_chip_alloc(dev, &st33zp24_tpm);
+	if (IS_ERR(chip))
+		return PTR_ERR(chip);
+
+	tpm_dev = devm_kzalloc(dev, sizeof(struct st33zp24_dev),
+			       GFP_KERNEL);
+	if (!tpm_dev)
+		return -ENOMEM;
+
+	TPM_VPRIV(chip) = tpm_dev;
+	tpm_dev->phy_id = phy_id;
+	tpm_dev->ops = ops;
+
+	chip->vendor.timeout_a = msecs_to_jiffies(TIS_SHORT_TIMEOUT);
+	chip->vendor.timeout_b = msecs_to_jiffies(TIS_LONG_TIMEOUT);
+	chip->vendor.timeout_c = msecs_to_jiffies(TIS_SHORT_TIMEOUT);
+	chip->vendor.timeout_d = msecs_to_jiffies(TIS_SHORT_TIMEOUT);
+
+	chip->vendor.locality = LOCALITY0;
+
+	if (irq) {
+		/* INTERRUPT Setup */
+		init_waitqueue_head(&chip->vendor.read_queue);
+		tpm_dev->intrs = 0;
+
+		if (request_locality(chip) != LOCALITY0) {
+			ret = -ENODEV;
+			goto _tpm_clean_answer;
+		}
+
+		clear_interruption(tpm_dev);
+		ret = devm_request_irq(dev, irq, tpm_ioserirq_handler,
+				IRQF_TRIGGER_HIGH, "TPM SERIRQ management",
+				chip);
+		if (ret < 0) {
+			dev_err(&chip->dev, "TPM SERIRQ signals %d not available\n",
+				irq);
+			goto _tpm_clean_answer;
+		}
+
+		intmask |= TPM_INTF_CMD_READY_INT
+			|  TPM_INTF_STS_VALID_INT
+			|  TPM_INTF_DATA_AVAIL_INT;
+
+		ret = tpm_dev->ops->send(tpm_dev->phy_id, TPM_INT_ENABLE,
+					 &intmask, 1);
+		if (ret < 0)
+			goto _tpm_clean_answer;
+
+		intmask = TPM_GLOBAL_INT_ENABLE;
+		ret = tpm_dev->ops->send(tpm_dev->phy_id, (TPM_INT_ENABLE + 3),
+					 &intmask, 1);
+		if (ret < 0)
+			goto _tpm_clean_answer;
+
+		chip->vendor.irq = irq;
+
+		disable_irq_nosync(chip->vendor.irq);
+
+		tpm_gen_interrupt(chip);
+	}
+
+	tpm_get_timeouts(chip);
+	tpm_do_selftest(chip);
+
+	return tpm_chip_register(chip);
+_tpm_clean_answer:
+	dev_info(&chip->dev, "TPM initialization fail\n");
+	return ret;
+}
+EXPORT_SYMBOL(st33zp24_probe);
+
+/*
+ * st33zp24_remove remove the TPM device
+ * @param: tpm_data, the tpm phy.
+ * @return: 0 in case of success.
+ */
+int st33zp24_remove(struct tpm_chip *chip)
+{
+	tpm_chip_unregister(chip);
+	return 0;
+}
+EXPORT_SYMBOL(st33zp24_remove);
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * st33zp24_pm_suspend suspend the TPM device
+ * @param: tpm_data, the tpm phy.
+ * @param: mesg, the power management message.
+ * @return: 0 in case of success.
+ */
+int st33zp24_pm_suspend(struct device *dev)
+{
+	struct tpm_chip *chip = dev_get_drvdata(dev);
+	struct st33zp24_dev *tpm_dev;
+	int ret = 0;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	if (gpio_is_valid(tpm_dev->io_lpcpd))
+		gpio_set_value(tpm_dev->io_lpcpd, 0);
+	else
+		ret = tpm_pm_suspend(dev);
+
+	return ret;
+} /* st33zp24_pm_suspend() */
+EXPORT_SYMBOL(st33zp24_pm_suspend);
+
+/*
+ * st33zp24_pm_resume resume the TPM device
+ * @param: tpm_data, the tpm phy.
+ * @return: 0 in case of success.
+ */
+int st33zp24_pm_resume(struct device *dev)
+{
+	struct tpm_chip *chip = dev_get_drvdata(dev);
+	struct st33zp24_dev *tpm_dev;
+	int ret = 0;
+
+	tpm_dev = (struct st33zp24_dev *)TPM_VPRIV(chip);
+
+	if (gpio_is_valid(tpm_dev->io_lpcpd)) {
+		gpio_set_value(tpm_dev->io_lpcpd, 1);
+		ret = wait_for_stat(chip,
+				TPM_STS_VALID, chip->vendor.timeout_b,
+				&chip->vendor.read_queue, false);
+	} else {
+		ret = tpm_pm_resume(dev);
+		if (!ret)
+			tpm_do_selftest(chip);
+	}
+	return ret;
+} /* st33zp24_pm_resume() */
+EXPORT_SYMBOL(st33zp24_pm_resume);
+#endif
+
+MODULE_AUTHOR("TPM support (TPMsupport@list.st.com)");
+MODULE_DESCRIPTION("ST33ZP24 TPM 1.2 driver");
+MODULE_VERSION("1.3.0");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/tpm/st33zp24/st33zp24.h b/drivers/char/tpm/st33zp24/st33zp24.h
new file mode 100644
index 000000000000..c207cebf67dd
--- /dev/null
+++ b/drivers/char/tpm/st33zp24/st33zp24.h
@@ -0,0 +1,37 @@
+/*
+ * STMicroelectronics TPM Linux driver for TPM ST33ZP24
+ * Copyright (C) 2009 - 2015  STMicroelectronics
+ *
+ * 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 that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __LOCAL_ST33ZP24_H__
+#define __LOCAL_ST33ZP24_H__
+
+#define TPM_WRITE_DIRECTION             0x80
+#define TPM_BUFSIZE                     2048
+
+struct st33zp24_phy_ops {
+	int (*send)(void *phy_id, u8 tpm_register, u8 *tpm_data, int tpm_size);
+	int (*recv)(void *phy_id, u8 tpm_register, u8 *tpm_data, int tpm_size);
+};
+
+#ifdef CONFIG_PM_SLEEP
+int st33zp24_pm_suspend(struct device *dev);
+int st33zp24_pm_resume(struct device *dev);
+#endif
+
+int st33zp24_probe(void *phy_id, const struct st33zp24_phy_ops *ops,
+		   struct device *dev, int irq, int io_lpcpd);
+int st33zp24_remove(struct tpm_chip *chip);
+#endif /* __LOCAL_ST33ZP24_H__ */
diff --git a/drivers/char/tpm/tpm-chip.c b/drivers/char/tpm/tpm-chip.c
index e096e9cddb40..283f00a7f036 100644
--- a/drivers/char/tpm/tpm-chip.c
+++ b/drivers/char/tpm/tpm-chip.c
@@ -170,6 +170,41 @@ static void tpm_dev_del_device(struct tpm_chip *chip)
 	device_unregister(&chip->dev);
 }
 
+static int tpm1_chip_register(struct tpm_chip *chip)
+{
+	int rc;
+
+	if (chip->flags & TPM_CHIP_FLAG_TPM2)
+		return 0;
+
+	rc = tpm_sysfs_add_device(chip);
+	if (rc)
+		return rc;
+
+	rc = tpm_add_ppi(chip);
+	if (rc) {
+		tpm_sysfs_del_device(chip);
+		return rc;
+	}
+
+	chip->bios_dir = tpm_bios_log_setup(chip->devname);
+
+	return 0;
+}
+
+static void tpm1_chip_unregister(struct tpm_chip *chip)
+{
+	if (chip->flags & TPM_CHIP_FLAG_TPM2)
+		return;
+
+	if (chip->bios_dir)
+		tpm_bios_log_teardown(chip->bios_dir);
+
+	tpm_remove_ppi(chip);
+
+	tpm_sysfs_del_device(chip);
+}
+
 /*
  * tpm_chip_register() - create a character device for the TPM chip
  * @chip: TPM chip to use.
@@ -185,22 +220,13 @@ int tpm_chip_register(struct tpm_chip *chip)
 {
 	int rc;
 
-	/* Populate sysfs for TPM1 devices. */
-	if (!(chip->flags & TPM_CHIP_FLAG_TPM2)) {
-		rc = tpm_sysfs_add_device(chip);
-		if (rc)
-			goto del_misc;
-
-		rc = tpm_add_ppi(chip);
-		if (rc)
-			goto del_sysfs;
-
-		chip->bios_dir = tpm_bios_log_setup(chip->devname);
-	}
+	rc = tpm1_chip_register(chip);
+	if (rc)
+		return rc;
 
 	rc = tpm_dev_add_device(chip);
 	if (rc)
-		return rc;
+		goto out_err;
 
 	/* Make the chip available. */
 	spin_lock(&driver_lock);
@@ -210,10 +236,8 @@ int tpm_chip_register(struct tpm_chip *chip)
 	chip->flags |= TPM_CHIP_FLAG_REGISTERED;
 
 	return 0;
-del_sysfs:
-	tpm_sysfs_del_device(chip);
-del_misc:
-	tpm_dev_del_device(chip);
+out_err:
+	tpm1_chip_unregister(chip);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(tpm_chip_register);
@@ -238,13 +262,7 @@ void tpm_chip_unregister(struct tpm_chip *chip)
 	spin_unlock(&driver_lock);
 	synchronize_rcu();
 
-	if (!(chip->flags & TPM_CHIP_FLAG_TPM2)) {
-		if (chip->bios_dir)
-			tpm_bios_log_teardown(chip->bios_dir);
-		tpm_remove_ppi(chip);
-		tpm_sysfs_del_device(chip);
-	}
-
+	tpm1_chip_unregister(chip);
 	tpm_dev_del_device(chip);
 }
 EXPORT_SYMBOL_GPL(tpm_chip_unregister);
diff --git a/drivers/char/tpm/tpm_i2c_stm_st33.c b/drivers/char/tpm/tpm_i2c_stm_st33.c
deleted file mode 100644
index 612845b36c29..000000000000
--- a/drivers/char/tpm/tpm_i2c_stm_st33.c
+++ /dev/null
@@ -1,911 +0,0 @@
-/*
- * STMicroelectronics TPM I2C Linux driver for TPM ST33ZP24
- * Copyright (C) 2009, 2010, 2014  STMicroelectronics
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see <http://www.gnu.org/licenses/>.
- *
- * STMicroelectronics version 1.2.1, Copyright (C) 2014
- * STMicroelectronics comes with ABSOLUTELY NO WARRANTY.
- * This is free software, and you are welcome to redistribute it
- * under certain conditions.
- *
- * @Author: Christophe RICARD tpmsupport@st.com
- *
- * @File: tpm_stm_st33_i2c.c
- *
- * @Synopsis:
- *	09/15/2010:	First shot driver tpm_tis driver for
- *			 lpc is used as model.
- */
-
-#include <linux/pci.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/i2c.h>
-#include <linux/fs.h>
-#include <linux/miscdevice.h>
-#include <linux/kernel.h>
-#include <linux/delay.h>
-#include <linux/wait.h>
-#include <linux/freezer.h>
-#include <linux/string.h>
-#include <linux/interrupt.h>
-#include <linux/sysfs.h>
-#include <linux/gpio.h>
-#include <linux/sched.h>
-#include <linux/uaccess.h>
-#include <linux/io.h>
-#include <linux/slab.h>
-#include <linux/of_irq.h>
-#include <linux/of_gpio.h>
-
-#include <linux/platform_data/tpm_stm_st33.h>
-#include "tpm.h"
-
-#define TPM_ACCESS			0x0
-#define TPM_STS				0x18
-#define TPM_HASH_END			0x20
-#define TPM_DATA_FIFO			0x24
-#define TPM_HASH_DATA			0x24
-#define TPM_HASH_START			0x28
-#define TPM_INTF_CAPABILITY		0x14
-#define TPM_INT_STATUS			0x10
-#define TPM_INT_ENABLE			0x08
-
-#define TPM_DUMMY_BYTE			0xAA
-#define TPM_WRITE_DIRECTION		0x80
-#define TPM_HEADER_SIZE			10
-#define TPM_BUFSIZE			2048
-
-#define LOCALITY0		0
-
-
-enum stm33zp24_access {
-	TPM_ACCESS_VALID = 0x80,
-	TPM_ACCESS_ACTIVE_LOCALITY = 0x20,
-	TPM_ACCESS_REQUEST_PENDING = 0x04,
-	TPM_ACCESS_REQUEST_USE = 0x02,
-};
-
-enum stm33zp24_status {
-	TPM_STS_VALID = 0x80,
-	TPM_STS_COMMAND_READY = 0x40,
-	TPM_STS_GO = 0x20,
-	TPM_STS_DATA_AVAIL = 0x10,
-	TPM_STS_DATA_EXPECT = 0x08,
-};
-
-enum stm33zp24_int_flags {
-	TPM_GLOBAL_INT_ENABLE = 0x80,
-	TPM_INTF_CMD_READY_INT = 0x080,
-	TPM_INTF_FIFO_AVALAIBLE_INT = 0x040,
-	TPM_INTF_WAKE_UP_READY_INT = 0x020,
-	TPM_INTF_LOCALITY_CHANGE_INT = 0x004,
-	TPM_INTF_STS_VALID_INT = 0x002,
-	TPM_INTF_DATA_AVAIL_INT = 0x001,
-};
-
-enum tis_defaults {
-	TIS_SHORT_TIMEOUT = 750,
-	TIS_LONG_TIMEOUT = 2000,
-};
-
-struct tpm_stm_dev {
-	struct i2c_client *client;
-	struct tpm_chip *chip;
-	u8 buf[TPM_BUFSIZE + 1];
-	u32 intrs;
-	int io_lpcpd;
-};
-
-/*
- * write8_reg
- * Send byte to the TIS register according to the ST33ZP24 I2C protocol.
- * @param: tpm_register, the tpm tis register where the data should be written
- * @param: tpm_data, the tpm_data to write inside the tpm_register
- * @param: tpm_size, The length of the data
- * @return: Returns negative errno, or else the number of bytes written.
- */
-static int write8_reg(struct tpm_stm_dev *tpm_dev, u8 tpm_register,
-		      u8 *tpm_data, u16 tpm_size)
-{
-	tpm_dev->buf[0] = tpm_register;
-	memcpy(tpm_dev->buf + 1, tpm_data, tpm_size);
-	return i2c_master_send(tpm_dev->client, tpm_dev->buf, tpm_size + 1);
-} /* write8_reg() */
-
-/*
- * read8_reg
- * Recv byte from the TIS register according to the ST33ZP24 I2C protocol.
- * @param: tpm_register, the tpm tis register where the data should be read
- * @param: tpm_data, the TPM response
- * @param: tpm_size, tpm TPM response size to read.
- * @return: number of byte read successfully: should be one if success.
- */
-static int read8_reg(struct tpm_stm_dev *tpm_dev, u8 tpm_register,
-		    u8 *tpm_data, int tpm_size)
-{
-	u8 status = 0;
-	u8 data;
-
-	data = TPM_DUMMY_BYTE;
-	status = write8_reg(tpm_dev, tpm_register, &data, 1);
-	if (status == 2)
-		status = i2c_master_recv(tpm_dev->client, tpm_data, tpm_size);
-	return status;
-} /* read8_reg() */
-
-/*
- * I2C_WRITE_DATA
- * Send byte to the TIS register according to the ST33ZP24 I2C protocol.
- * @param: tpm_dev, the chip description
- * @param: tpm_register, the tpm tis register where the data should be written
- * @param: tpm_data, the tpm_data to write inside the tpm_register
- * @param: tpm_size, The length of the data
- * @return: number of byte written successfully: should be one if success.
- */
-#define I2C_WRITE_DATA(tpm_dev, tpm_register, tpm_data, tpm_size) \
-	(write8_reg(tpm_dev, tpm_register | \
-	TPM_WRITE_DIRECTION, tpm_data, tpm_size))
-
-/*
- * I2C_READ_DATA
- * Recv byte from the TIS register according to the ST33ZP24 I2C protocol.
- * @param: tpm_dev, the chip description
- * @param: tpm_register, the tpm tis register where the data should be read
- * @param: tpm_data, the TPM response
- * @param: tpm_size, tpm TPM response size to read.
- * @return: number of byte read successfully: should be one if success.
- */
-#define I2C_READ_DATA(tpm_dev, tpm_register, tpm_data, tpm_size) \
-	(read8_reg(tpm_dev, tpm_register, tpm_data, tpm_size))
-
-/*
- * clear_interruption
- * clear the TPM interrupt register.
- * @param: tpm, the chip description
- * @return: the TPM_INT_STATUS value
- */
-static u8 clear_interruption(struct tpm_stm_dev *tpm_dev)
-{
-	u8 interrupt;
-
-	I2C_READ_DATA(tpm_dev, TPM_INT_STATUS, &interrupt, 1);
-	I2C_WRITE_DATA(tpm_dev, TPM_INT_STATUS, &interrupt, 1);
-	return interrupt;
-} /* clear_interruption() */
-
-/*
- * tpm_stm_i2c_cancel, cancel is not implemented.
- * @param: chip, the tpm_chip description as specified in driver/char/tpm/tpm.h
- */
-static void tpm_stm_i2c_cancel(struct tpm_chip *chip)
-{
-	struct tpm_stm_dev *tpm_dev;
-	u8 data;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	data = TPM_STS_COMMAND_READY;
-	I2C_WRITE_DATA(tpm_dev, TPM_STS, &data, 1);
-} /* tpm_stm_i2c_cancel() */
-
-/*
- * tpm_stm_spi_status return the TPM_STS register
- * @param: chip, the tpm chip description
- * @return: the TPM_STS register value.
- */
-static u8 tpm_stm_i2c_status(struct tpm_chip *chip)
-{
-	struct tpm_stm_dev *tpm_dev;
-	u8 data;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	I2C_READ_DATA(tpm_dev, TPM_STS, &data, 1);
-	return data;
-} /* tpm_stm_i2c_status() */
-
-
-/*
- * check_locality if the locality is active
- * @param: chip, the tpm chip description
- * @return: the active locality or -EACCESS.
- */
-static int check_locality(struct tpm_chip *chip)
-{
-	struct tpm_stm_dev *tpm_dev;
-	u8 data;
-	u8 status;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	status = I2C_READ_DATA(tpm_dev, TPM_ACCESS, &data, 1);
-	if (status && (data &
-		(TPM_ACCESS_ACTIVE_LOCALITY | TPM_ACCESS_VALID)) ==
-		(TPM_ACCESS_ACTIVE_LOCALITY | TPM_ACCESS_VALID))
-		return chip->vendor.locality;
-
-	return -EACCES;
-} /* check_locality() */
-
-/*
- * request_locality request the TPM locality
- * @param: chip, the chip description
- * @return: the active locality or EACCESS.
- */
-static int request_locality(struct tpm_chip *chip)
-{
-	unsigned long stop;
-	long ret;
-	struct tpm_stm_dev *tpm_dev;
-	u8 data;
-
-	if (check_locality(chip) == chip->vendor.locality)
-		return chip->vendor.locality;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	data = TPM_ACCESS_REQUEST_USE;
-	ret = I2C_WRITE_DATA(tpm_dev, TPM_ACCESS, &data, 1);
-	if (ret < 0)
-		goto end;
-
-	stop = jiffies + chip->vendor.timeout_a;
-
-	/* Request locality is usually effective after the request */
-	do {
-		if (check_locality(chip) >= 0)
-			return chip->vendor.locality;
-		msleep(TPM_TIMEOUT);
-	} while (time_before(jiffies, stop));
-	ret = -EACCES;
-end:
-	return ret;
-} /* request_locality() */
-
-/*
- * release_locality release the active locality
- * @param: chip, the tpm chip description.
- */
-static void release_locality(struct tpm_chip *chip)
-{
-	struct tpm_stm_dev *tpm_dev;
-	u8 data;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-	data = TPM_ACCESS_ACTIVE_LOCALITY;
-
-	I2C_WRITE_DATA(tpm_dev, TPM_ACCESS, &data, 1);
-}
-
-/*
- * get_burstcount return the burstcount address 0x19 0x1A
- * @param: chip, the chip description
- * return: the burstcount.
- */
-static int get_burstcount(struct tpm_chip *chip)
-{
-	unsigned long stop;
-	int burstcnt, status;
-	u8 tpm_reg, temp;
-	struct tpm_stm_dev *tpm_dev;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	stop = jiffies + chip->vendor.timeout_d;
-	do {
-		tpm_reg = TPM_STS + 1;
-		status = I2C_READ_DATA(tpm_dev, tpm_reg, &temp, 1);
-		if (status < 0)
-			goto end;
-
-		tpm_reg = tpm_reg + 1;
-		burstcnt = temp;
-		status = I2C_READ_DATA(tpm_dev, tpm_reg, &temp, 1);
-		if (status < 0)
-			goto end;
-
-		burstcnt |= temp << 8;
-		if (burstcnt)
-			return burstcnt;
-		msleep(TPM_TIMEOUT);
-	} while (time_before(jiffies, stop));
-
-end:
-	return -EBUSY;
-} /* get_burstcount() */
-
-static bool wait_for_tpm_stat_cond(struct tpm_chip *chip, u8 mask,
-				bool check_cancel, bool *canceled)
-{
-	u8 status = chip->ops->status(chip);
-
-	*canceled = false;
-	if ((status & mask) == mask)
-		return true;
-	if (check_cancel && chip->ops->req_canceled(chip, status)) {
-		*canceled = true;
-		return true;
-	}
-	return false;
-}
-
-/*
- * interrupt_to_status
- * @param: irq_mask, the irq mask value to wait
- * @return: the corresponding tpm_sts value
- */
-static u8 interrupt_to_status(u8 irq_mask)
-{
-	u8 status = 0;
-
-	if ((irq_mask & TPM_INTF_STS_VALID_INT) == TPM_INTF_STS_VALID_INT)
-		status |= TPM_STS_VALID;
-	if ((irq_mask & TPM_INTF_DATA_AVAIL_INT) == TPM_INTF_DATA_AVAIL_INT)
-		status |= TPM_STS_DATA_AVAIL;
-	if ((irq_mask & TPM_INTF_CMD_READY_INT) == TPM_INTF_CMD_READY_INT)
-		status |= TPM_STS_COMMAND_READY;
-
-	return status;
-} /* status_to_interrupt() */
-
-/*
- * wait_for_stat wait for a TPM_STS value
- * @param: chip, the tpm chip description
- * @param: mask, the value mask to wait
- * @param: timeout, the timeout
- * @param: queue, the wait queue.
- * @param: check_cancel, does the command can be cancelled ?
- * @return: the tpm status, 0 if success, -ETIME if timeout is reached.
- */
-static int wait_for_stat(struct tpm_chip *chip, u8 mask, unsigned long timeout,
-			wait_queue_head_t *queue, bool check_cancel)
-{
-	unsigned long stop;
-	int ret;
-	bool canceled = false;
-	bool condition;
-	u32 cur_intrs;
-	u8 interrupt, status;
-	struct tpm_stm_dev *tpm_dev;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	/* check current status */
-	status = tpm_stm_i2c_status(chip);
-	if ((status & mask) == mask)
-		return 0;
-
-	stop = jiffies + timeout;
-
-	if (chip->vendor.irq) {
-		cur_intrs = tpm_dev->intrs;
-		interrupt = clear_interruption(tpm_dev);
-		enable_irq(chip->vendor.irq);
-
-again:
-		timeout = stop - jiffies;
-		if ((long) timeout <= 0)
-			return -1;
-
-		ret = wait_event_interruptible_timeout(*queue,
-					cur_intrs != tpm_dev->intrs, timeout);
-
-		interrupt |= clear_interruption(tpm_dev);
-		status = interrupt_to_status(interrupt);
-		condition = wait_for_tpm_stat_cond(chip, mask,
-						   check_cancel, &canceled);
-
-		if (ret >= 0 && condition) {
-			if (canceled)
-				return -ECANCELED;
-			return 0;
-		}
-		if (ret == -ERESTARTSYS && freezing(current)) {
-			clear_thread_flag(TIF_SIGPENDING);
-			goto again;
-		}
-		disable_irq_nosync(chip->vendor.irq);
-
-	} else {
-		do {
-			msleep(TPM_TIMEOUT);
-			status = chip->ops->status(chip);
-			if ((status & mask) == mask)
-				return 0;
-		} while (time_before(jiffies, stop));
-	}
-
-	return -ETIME;
-} /* wait_for_stat() */
-
-/*
- * recv_data receive data
- * @param: chip, the tpm chip description
- * @param: buf, the buffer where the data are received
- * @param: count, the number of data to receive
- * @return: the number of bytes read from TPM FIFO.
- */
-static int recv_data(struct tpm_chip *chip, u8 *buf, size_t count)
-{
-	int size = 0, burstcnt, len, ret;
-	struct tpm_stm_dev *tpm_dev;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	while (size < count &&
-	       wait_for_stat(chip,
-			     TPM_STS_DATA_AVAIL | TPM_STS_VALID,
-			     chip->vendor.timeout_c,
-			     &chip->vendor.read_queue, true) == 0) {
-		burstcnt = get_burstcount(chip);
-		if (burstcnt < 0)
-			return burstcnt;
-		len = min_t(int, burstcnt, count - size);
-		ret = I2C_READ_DATA(tpm_dev, TPM_DATA_FIFO, buf + size, len);
-		if (ret < 0)
-			return ret;
-
-		size += len;
-	}
-	return size;
-}
-
-/*
- * tpm_ioserirq_handler the serirq irq handler
- * @param: irq, the tpm chip description
- * @param: dev_id, the description of the chip
- * @return: the status of the handler.
- */
-static irqreturn_t tpm_ioserirq_handler(int irq, void *dev_id)
-{
-	struct tpm_chip *chip = dev_id;
-	struct tpm_stm_dev *tpm_dev;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-
-	tpm_dev->intrs++;
-	wake_up_interruptible(&chip->vendor.read_queue);
-	disable_irq_nosync(chip->vendor.irq);
-
-	return IRQ_HANDLED;
-} /* tpm_ioserirq_handler() */
-
-
-/*
- * tpm_stm_i2c_send send TPM commands through the I2C bus.
- *
- * @param: chip, the tpm_chip description as specified in driver/char/tpm/tpm.h
- * @param: buf,	the buffer to send.
- * @param: count, the number of bytes to send.
- * @return: In case of success the number of bytes sent.
- *			In other case, a < 0 value describing the issue.
- */
-static int tpm_stm_i2c_send(struct tpm_chip *chip, unsigned char *buf,
-			    size_t len)
-{
-	u32 status, i, size;
-	int burstcnt = 0;
-	int ret;
-	u8 data;
-	struct i2c_client *client;
-	struct tpm_stm_dev *tpm_dev;
-
-	if (!chip)
-		return -EBUSY;
-	if (len < TPM_HEADER_SIZE)
-		return -EBUSY;
-
-	tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-	client = tpm_dev->client;
-
-	client->flags = 0;
-
-	ret = request_locality(chip);
-	if (ret < 0)
-		return ret;
-
-	status = tpm_stm_i2c_status(chip);
-	if ((status & TPM_STS_COMMAND_READY) == 0) {
-		tpm_stm_i2c_cancel(chip);
-		if (wait_for_stat
-		    (chip, TPM_STS_COMMAND_READY, chip->vendor.timeout_b,
-		     &chip->vendor.read_queue, false) < 0) {
-			ret = -ETIME;
-			goto out_err;
-		}
-	}
-
-	for (i = 0; i < len - 1;) {
-		burstcnt = get_burstcount(chip);
-		if (burstcnt < 0)
-			return burstcnt;
-		size = min_t(int, len - i - 1, burstcnt);
-		ret = I2C_WRITE_DATA(tpm_dev, TPM_DATA_FIFO, buf + i, size);
-		if (ret < 0)
-			goto out_err;
-
-		i += size;
-	}
-
-	status = tpm_stm_i2c_status(chip);
-	if ((status & TPM_STS_DATA_EXPECT) == 0) {
-		ret = -EIO;
-		goto out_err;
-	}
-
-	ret = I2C_WRITE_DATA(tpm_dev, TPM_DATA_FIFO, buf + len - 1, 1);
-	if (ret < 0)
-		goto out_err;
-
-	status = tpm_stm_i2c_status(chip);
-	if ((status & TPM_STS_DATA_EXPECT) != 0) {
-		ret = -EIO;
-		goto out_err;
-	}
-
-	data = TPM_STS_GO;
-	I2C_WRITE_DATA(tpm_dev, TPM_STS, &data, 1);
-
-	return len;
-out_err:
-	tpm_stm_i2c_cancel(chip);
-	release_locality(chip);
-	return ret;
-}
-
-/*
- * tpm_stm_i2c_recv received TPM response through the I2C bus.
- * @param: chip, the tpm_chip description as specified in driver/char/tpm/tpm.h.
- * @param: buf,	the buffer to store datas.
- * @param: count, the number of bytes to send.
- * @return: In case of success the number of bytes received.
- *	    In other case, a < 0 value describing the issue.
- */
-static int tpm_stm_i2c_recv(struct tpm_chip *chip, unsigned char *buf,
-			    size_t count)
-{
-	int size = 0;
-	int expected;
-
-	if (!chip)
-		return -EBUSY;
-
-	if (count < TPM_HEADER_SIZE) {
-		size = -EIO;
-		goto out;
-	}
-
-	size = recv_data(chip, buf, TPM_HEADER_SIZE);
-	if (size < TPM_HEADER_SIZE) {
-		dev_err(chip->pdev, "Unable to read header\n");
-		goto out;
-	}
-
-	expected = be32_to_cpu(*(__be32 *)(buf + 2));
-	if (expected > count) {
-		size = -EIO;
-		goto out;
-	}
-
-	size += recv_data(chip, &buf[TPM_HEADER_SIZE],
-			expected - TPM_HEADER_SIZE);
-	if (size < expected) {
-		dev_err(chip->pdev, "Unable to read remainder of result\n");
-		size = -ETIME;
-		goto out;
-	}
-
-out:
-	chip->ops->cancel(chip);
-	release_locality(chip);
-	return size;
-}
-
-static bool tpm_stm_i2c_req_canceled(struct tpm_chip *chip, u8 status)
-{
-	return (status == TPM_STS_COMMAND_READY);
-}
-
-static const struct tpm_class_ops st_i2c_tpm = {
-	.send = tpm_stm_i2c_send,
-	.recv = tpm_stm_i2c_recv,
-	.cancel = tpm_stm_i2c_cancel,
-	.status = tpm_stm_i2c_status,
-	.req_complete_mask = TPM_STS_DATA_AVAIL | TPM_STS_VALID,
-	.req_complete_val = TPM_STS_DATA_AVAIL | TPM_STS_VALID,
-	.req_canceled = tpm_stm_i2c_req_canceled,
-};
-
-#ifdef CONFIG_OF
-static int tpm_stm_i2c_of_request_resources(struct tpm_chip *chip)
-{
-	struct device_node *pp;
-	struct tpm_stm_dev *tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-	struct i2c_client *client = tpm_dev->client;
-	int gpio;
-	int ret;
-
-	pp = client->dev.of_node;
-	if (!pp) {
-		dev_err(chip->pdev, "No platform data\n");
-		return -ENODEV;
-	}
-
-	/* Get GPIO from device tree */
-	gpio = of_get_named_gpio(pp, "lpcpd-gpios", 0);
-	if (gpio < 0) {
-		dev_err(chip->pdev, "Failed to retrieve lpcpd-gpios from dts.\n");
-		tpm_dev->io_lpcpd = -1;
-		/*
-		 * lpcpd pin is not specified. This is not an issue as
-		 * power management can be also managed by TPM specific
-		 * commands. So leave with a success status code.
-		 */
-		return 0;
-	}
-	/* GPIO request and configuration */
-	ret = devm_gpio_request_one(&client->dev, gpio,
-			GPIOF_OUT_INIT_HIGH, "TPM IO LPCPD");
-	if (ret) {
-		dev_err(chip->pdev, "Failed to request lpcpd pin\n");
-		return -ENODEV;
-	}
-	tpm_dev->io_lpcpd = gpio;
-
-	return 0;
-}
-#else
-static int tpm_stm_i2c_of_request_resources(struct tpm_chip *chip)
-{
-	return -ENODEV;
-}
-#endif
-
-static int tpm_stm_i2c_request_resources(struct i2c_client *client,
-					 struct tpm_chip *chip)
-{
-	struct st33zp24_platform_data *pdata;
-	struct tpm_stm_dev *tpm_dev = (struct tpm_stm_dev *)TPM_VPRIV(chip);
-	int ret;
-
-	pdata = client->dev.platform_data;
-	if (!pdata) {
-		dev_err(chip->pdev, "No platform data\n");
-		return -ENODEV;
-	}
-
-	/* store for late use */
-	tpm_dev->io_lpcpd = pdata->io_lpcpd;
-
-	if (gpio_is_valid(pdata->io_lpcpd)) {
-		ret = devm_gpio_request_one(&client->dev,
-				pdata->io_lpcpd, GPIOF_OUT_INIT_HIGH,
-				"TPM IO_LPCPD");
-		if (ret) {
-			dev_err(chip->pdev, "%s : reset gpio_request failed\n",
-				__FILE__);
-			return ret;
-		}
-	}
-
-	return 0;
-}
-
-/*
- * tpm_stm_i2c_probe initialize the TPM device
- * @param: client, the i2c_client drescription (TPM I2C description).
- * @param: id, the i2c_device_id struct.
- * @return: 0 in case of success.
- *	 -1 in other case.
- */
-static int
-tpm_stm_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
-{
-	int ret;
-	u8 intmask = 0;
-	struct tpm_chip *chip;
-	struct st33zp24_platform_data *platform_data;
-	struct tpm_stm_dev *tpm_dev;
-
-	if (!client) {
-		pr_info("%s: i2c client is NULL. Device not accessible.\n",
-			__func__);
-		return -ENODEV;
-	}
-
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-		dev_info(&client->dev, "client not i2c capable\n");
-		return -ENODEV;
-	}
-
-	tpm_dev = devm_kzalloc(&client->dev, sizeof(struct tpm_stm_dev),
-			       GFP_KERNEL);
-	if (!tpm_dev)
-		return -ENOMEM;
-
-	chip = tpmm_chip_alloc(&client->dev, &st_i2c_tpm);
-	if (IS_ERR(chip))
-		return PTR_ERR(chip);
-
-	TPM_VPRIV(chip) = tpm_dev;
-	tpm_dev->client = client;
-
-	platform_data = client->dev.platform_data;
-	if (!platform_data && client->dev.of_node) {
-		ret = tpm_stm_i2c_of_request_resources(chip);
-		if (ret)
-			goto _tpm_clean_answer;
-	} else if (platform_data) {
-		ret = tpm_stm_i2c_request_resources(client, chip);
-		if (ret)
-			goto _tpm_clean_answer;
-	}
-
-	chip->vendor.timeout_a = msecs_to_jiffies(TIS_SHORT_TIMEOUT);
-	chip->vendor.timeout_b = msecs_to_jiffies(TIS_LONG_TIMEOUT);
-	chip->vendor.timeout_c = msecs_to_jiffies(TIS_SHORT_TIMEOUT);
-	chip->vendor.timeout_d = msecs_to_jiffies(TIS_SHORT_TIMEOUT);
-
-	chip->vendor.locality = LOCALITY0;
-
-	if (client->irq) {
-		/* INTERRUPT Setup */
-		init_waitqueue_head(&chip->vendor.read_queue);
-		tpm_dev->intrs = 0;
-
-		if (request_locality(chip) != LOCALITY0) {
-			ret = -ENODEV;
-			goto _tpm_clean_answer;
-		}
-
-		clear_interruption(tpm_dev);
-		ret = devm_request_irq(&client->dev, client->irq,
-				tpm_ioserirq_handler,
-				IRQF_TRIGGER_HIGH,
-				"TPM SERIRQ management", chip);
-		if (ret < 0) {
-			dev_err(chip->pdev, "TPM SERIRQ signals %d not available\n",
-				client->irq);
-			goto _tpm_clean_answer;
-		}
-
-		intmask |= TPM_INTF_CMD_READY_INT
-			|  TPM_INTF_STS_VALID_INT
-			|  TPM_INTF_DATA_AVAIL_INT;
-
-		ret = I2C_WRITE_DATA(tpm_dev, TPM_INT_ENABLE, &intmask, 1);
-		if (ret < 0)
-			goto _tpm_clean_answer;
-
-		intmask = TPM_GLOBAL_INT_ENABLE;
-		ret = I2C_WRITE_DATA(tpm_dev, (TPM_INT_ENABLE + 3),
-				     &intmask, 1);
-		if (ret < 0)
-			goto _tpm_clean_answer;
-
-		chip->vendor.irq = client->irq;
-
-		disable_irq_nosync(chip->vendor.irq);
-
-		tpm_gen_interrupt(chip);
-	}
-
-	tpm_get_timeouts(chip);
-	tpm_do_selftest(chip);
-
-	return tpm_chip_register(chip);
-_tpm_clean_answer:
-	dev_info(chip->pdev, "TPM I2C initialisation fail\n");
-	return ret;
-}
-
-/*
- * tpm_stm_i2c_remove remove the TPM device
- * @param: client, the i2c_client description (TPM I2C description).
- * @return: 0 in case of success.
- */
-static int tpm_stm_i2c_remove(struct i2c_client *client)
-{
-	struct tpm_chip *chip =
-		(struct tpm_chip *) i2c_get_clientdata(client);
-
-	if (chip)
-		tpm_chip_unregister(chip);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM_SLEEP
-/*
- * tpm_stm_i2c_pm_suspend suspend the TPM device
- * @param: client, the i2c_client drescription (TPM I2C description).
- * @param: mesg, the power management message.
- * @return: 0 in case of success.
- */
-static int tpm_stm_i2c_pm_suspend(struct device *dev)
-{
-	struct st33zp24_platform_data *pin_infos = dev->platform_data;
-	int ret = 0;
-
-	if (gpio_is_valid(pin_infos->io_lpcpd))
-		gpio_set_value(pin_infos->io_lpcpd, 0);
-	else
-		ret = tpm_pm_suspend(dev);
-
-	return ret;
-} /* tpm_stm_i2c_suspend() */
-
-/*
- * tpm_stm_i2c_pm_resume resume the TPM device
- * @param: client, the i2c_client drescription (TPM I2C description).
- * @return: 0 in case of success.
- */
-static int tpm_stm_i2c_pm_resume(struct device *dev)
-{
-	struct tpm_chip *chip = dev_get_drvdata(dev);
-	struct st33zp24_platform_data *pin_infos = dev->platform_data;
-
-	int ret = 0;
-
-	if (gpio_is_valid(pin_infos->io_lpcpd)) {
-		gpio_set_value(pin_infos->io_lpcpd, 1);
-		ret = wait_for_stat(chip,
-				TPM_STS_VALID, chip->vendor.timeout_b,
-				&chip->vendor.read_queue, false);
-	} else {
-		ret = tpm_pm_resume(dev);
-		if (!ret)
-			tpm_do_selftest(chip);
-	}
-	return ret;
-} /* tpm_stm_i2c_pm_resume() */
-#endif
-
-static const struct i2c_device_id tpm_stm_i2c_id[] = {
-	{TPM_ST33_I2C, 0},
-	{}
-};
-MODULE_DEVICE_TABLE(i2c, tpm_stm_i2c_id);
-
-#ifdef CONFIG_OF
-static const struct of_device_id of_st33zp24_i2c_match[] = {
-	{ .compatible = "st,st33zp24-i2c", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, of_st33zp24_i2c_match);
-#endif
-
-static SIMPLE_DEV_PM_OPS(tpm_stm_i2c_ops, tpm_stm_i2c_pm_suspend,
-			 tpm_stm_i2c_pm_resume);
-
-static struct i2c_driver tpm_stm_i2c_driver = {
-	.driver = {
-		.owner = THIS_MODULE,
-		.name = TPM_ST33_I2C,
-		.pm = &tpm_stm_i2c_ops,
-		.of_match_table = of_match_ptr(of_st33zp24_i2c_match),
-	},
-	.probe = tpm_stm_i2c_probe,
-	.remove = tpm_stm_i2c_remove,
-	.id_table = tpm_stm_i2c_id
-};
-
-module_i2c_driver(tpm_stm_i2c_driver);
-
-MODULE_AUTHOR("Christophe Ricard (tpmsupport@st.com)");
-MODULE_DESCRIPTION("STM TPM I2C ST33 Driver");
-MODULE_VERSION("1.2.1");
-MODULE_LICENSE("GPL");
diff --git a/drivers/char/tpm/tpm_infineon.c b/drivers/char/tpm/tpm_infineon.c
index 6d492132ad2b..6c488e635fdd 100644
--- a/drivers/char/tpm/tpm_infineon.c
+++ b/drivers/char/tpm/tpm_infineon.c
@@ -591,27 +591,8 @@ static void tpm_inf_pnp_remove(struct pnp_dev *dev)
 	}
 }
 
-static int tpm_inf_pnp_suspend(struct pnp_dev *dev, pm_message_t pm_state)
-{
-	struct tpm_chip *chip = pnp_get_drvdata(dev);
-	int rc;
-	if (chip) {
-		u8 savestate[] = {
-			0, 193,	/* TPM_TAG_RQU_COMMAND */
-			0, 0, 0, 10,	/* blob length (in bytes) */
-			0, 0, 0, 152	/* TPM_ORD_SaveState */
-		};
-		dev_info(&dev->dev, "saving TPM state\n");
-		rc = tpm_inf_send(chip, savestate, sizeof(savestate));
-		if (rc < 0) {
-			dev_err(&dev->dev, "error while saving TPM state\n");
-			return rc;
-		}
-	}
-	return 0;
-}
-
-static int tpm_inf_pnp_resume(struct pnp_dev *dev)
+#ifdef CONFIG_PM_SLEEP
+static int tpm_inf_resume(struct device *dev)
 {
 	/* Re-configure TPM after suspending */
 	tpm_config_out(ENABLE_REGISTER_PAIR, TPM_INF_ADDR);
@@ -625,30 +606,22 @@ static int tpm_inf_pnp_resume(struct pnp_dev *dev)
 	tpm_config_out(DISABLE_REGISTER_PAIR, TPM_INF_ADDR);
 	/* disable RESET, LP and IRQC */
 	tpm_data_out(RESET_LP_IRQC_DISABLE, CMD);
-	return tpm_pm_resume(&dev->dev);
+	return tpm_pm_resume(dev);
 }
+#endif
+static SIMPLE_DEV_PM_OPS(tpm_inf_pm, tpm_pm_suspend, tpm_inf_resume);
 
 static struct pnp_driver tpm_inf_pnp_driver = {
 	.name = "tpm_inf_pnp",
 	.id_table = tpm_inf_pnp_tbl,
 	.probe = tpm_inf_pnp_probe,
-	.suspend = tpm_inf_pnp_suspend,
-	.resume = tpm_inf_pnp_resume,
-	.remove = tpm_inf_pnp_remove
+	.remove = tpm_inf_pnp_remove,
+	.driver = {
+		.pm = &tpm_inf_pm,
+	}
 };
 
-static int __init init_inf(void)
-{
-	return pnp_register_driver(&tpm_inf_pnp_driver);
-}
-
-static void __exit cleanup_inf(void)
-{
-	pnp_unregister_driver(&tpm_inf_pnp_driver);
-}
-
-module_init(init_inf);
-module_exit(cleanup_inf);
+module_pnp_driver(tpm_inf_pnp_driver);
 
 MODULE_AUTHOR("Marcel Selhorst <tpmdd@sirrix.com>");
 MODULE_DESCRIPTION("Driver for Infineon TPM SLD 9630 TT 1.1 / SLB 9635 TT 1.2");
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/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 68161f7a07d6..a0b036ccb118 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -192,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
@@ -201,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
@@ -210,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
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/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/Kconfig b/drivers/cpufreq/Kconfig
index a171fef2c2b6..659879a56dba 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -293,5 +293,13 @@ config SH_CPU_FREQ
 	  If unsure, say N.
 endif
 
+config QORIQ_CPUFREQ
+	tristate "CPU frequency scaling driver for Freescale QorIQ SoCs"
+	depends on OF && COMMON_CLK && (PPC_E500MC || ARM)
+	select CLK_QORIQ
+	help
+	  This adds the CPUFreq driver support for Freescale QorIQ SoCs
+	  which are capable of changing the CPU's frequency dynamically.
+
 endif
 endmenu
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 1b06fc4640e2..4f3dbc8cf729 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -108,6 +108,15 @@ config ARM_HIGHBANK_CPUFREQ
 
 	  If in doubt, say N.
 
+config ARM_HISI_ACPU_CPUFREQ
+	tristate "Hisilicon ACPU CPUfreq driver"
+	depends on ARCH_HISI && CPUFREQ_DT
+	select PM_OPP
+	help
+	  This enables the hisilicon ACPU CPUfreq driver.
+
+	  If in doubt, say N.
+
 config ARM_IMX6Q_CPUFREQ
 	tristate "Freescale i.MX6 cpufreq support"
 	depends on ARCH_MXC
diff --git a/drivers/cpufreq/Kconfig.powerpc b/drivers/cpufreq/Kconfig.powerpc
index 7ea24413cee6..3a0595b41eab 100644
--- a/drivers/cpufreq/Kconfig.powerpc
+++ b/drivers/cpufreq/Kconfig.powerpc
@@ -23,15 +23,6 @@ config CPU_FREQ_MAPLE
 	  This adds support for frequency switching on Maple 970FX
 	  Evaluation Board and compatible boards (IBM JS2x blades).
 
-config PPC_CORENET_CPUFREQ
-	tristate "CPU frequency scaling driver for Freescale E500MC SoCs"
-	depends on PPC_E500MC && OF && COMMON_CLK
-	select CLK_QORIQ
-	help
-	  This adds the CPUFreq driver support for Freescale e500mc,
-	  e5500 and e6500 series SoCs which are capable of changing
-	  the CPU's frequency dynamically.
-
 config CPU_FREQ_PMAC
 	bool "Support for Apple PowerBooks"
 	depends on ADB_PMU && PPC32
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index 82a1821471fd..cdce92ae2e8b 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -59,6 +59,7 @@ arm-exynos-cpufreq-$(CONFIG_ARM_EXYNOS4X12_CPUFREQ)	+= exynos4x12-cpufreq.o
 arm-exynos-cpufreq-$(CONFIG_ARM_EXYNOS5250_CPUFREQ)	+= exynos5250-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS5440_CPUFREQ)	+= exynos5440-cpufreq.o
 obj-$(CONFIG_ARM_HIGHBANK_CPUFREQ)	+= highbank-cpufreq.o
+obj-$(CONFIG_ARM_HISI_ACPU_CPUFREQ)	+= hisi-acpu-cpufreq.o
 obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)		+= imx6q-cpufreq.o
 obj-$(CONFIG_ARM_INTEGRATOR)		+= integrator-cpufreq.o
 obj-$(CONFIG_ARM_KIRKWOOD_CPUFREQ)	+= kirkwood-cpufreq.o
@@ -85,7 +86,7 @@ obj-$(CONFIG_CPU_FREQ_CBE)		+= ppc-cbe-cpufreq.o
 ppc-cbe-cpufreq-y			+= ppc_cbe_cpufreq_pervasive.o ppc_cbe_cpufreq.o
 obj-$(CONFIG_CPU_FREQ_CBE_PMI)		+= ppc_cbe_cpufreq_pmi.o
 obj-$(CONFIG_CPU_FREQ_MAPLE)		+= maple-cpufreq.o
-obj-$(CONFIG_PPC_CORENET_CPUFREQ)   += ppc-corenet-cpufreq.o
+obj-$(CONFIG_QORIQ_CPUFREQ)   		+= qoriq-cpufreq.o
 obj-$(CONFIG_CPU_FREQ_PMAC)		+= pmac32-cpufreq.o
 obj-$(CONFIG_CPU_FREQ_PMAC64)		+= pmac64-cpufreq.o
 obj-$(CONFIG_PPC_PASEMI_CPUFREQ)	+= pasemi-cpufreq.o
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/hisi-acpu-cpufreq.c b/drivers/cpufreq/hisi-acpu-cpufreq.c
new file mode 100644
index 000000000000..026d5b2224de
--- /dev/null
+++ b/drivers/cpufreq/hisi-acpu-cpufreq.c
@@ -0,0 +1,42 @@
+/*
+ * Hisilicon Platforms Using ACPU CPUFreq Support
+ *
+ * Copyright (c) 2015 Hisilicon Limited.
+ * Copyright (c) 2015 Linaro Limited.
+ *
+ * Leo Yan <leo.yan@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+static int __init hisi_acpu_cpufreq_driver_init(void)
+{
+	struct platform_device *pdev;
+
+	if (!of_machine_is_compatible("hisilicon,hi6220"))
+		return -ENODEV;
+
+	pdev = platform_device_register_simple("cpufreq-dt", -1, NULL, 0);
+	return PTR_ERR_OR_ZERO(pdev);
+}
+module_init(hisi_acpu_cpufreq_driver_init);
+
+MODULE_AUTHOR("Leo Yan <leo.yan@linaro.org>");
+MODULE_DESCRIPTION("Hisilicon acpu cpufreq driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 872c5772c5d3..c5b81beccc8e 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -614,6 +614,19 @@ static void core_set_pstate(struct cpudata *cpudata, int pstate)
 	wrmsrl_on_cpu(cpudata->cpu, MSR_IA32_PERF_CTL, val);
 }
 
+static int knl_get_turbo_pstate(void)
+{
+	u64 value;
+	int nont, ret;
+
+	rdmsrl(MSR_NHM_TURBO_RATIO_LIMIT, value);
+	nont = core_get_max_pstate();
+	ret = (((value) >> 8) & 0xFF);
+	if (ret <= nont)
+		ret = nont;
+	return ret;
+}
+
 static struct cpu_defaults core_params = {
 	.pid_policy = {
 		.sample_rate_ms = 10,
@@ -651,6 +664,23 @@ static struct cpu_defaults byt_params = {
 	},
 };
 
+static struct cpu_defaults knl_params = {
+	.pid_policy = {
+		.sample_rate_ms = 10,
+		.deadband = 0,
+		.setpoint = 97,
+		.p_gain_pct = 20,
+		.d_gain_pct = 0,
+		.i_gain_pct = 0,
+	},
+	.funcs = {
+		.get_max = core_get_max_pstate,
+		.get_min = core_get_min_pstate,
+		.get_turbo = knl_get_turbo_pstate,
+		.set = core_set_pstate,
+	},
+};
+
 static void intel_pstate_get_min_max(struct cpudata *cpu, int *min, int *max)
 {
 	int max_perf = cpu->pstate.turbo_pstate;
@@ -865,6 +895,7 @@ static const struct x86_cpu_id intel_pstate_cpu_ids[] = {
 	ICPU(0x4e, core_params),
 	ICPU(0x4f, core_params),
 	ICPU(0x56, core_params),
+	ICPU(0x57, knl_params),
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, intel_pstate_cpu_ids);
@@ -1024,25 +1055,11 @@ static unsigned int force_load;
 
 static int intel_pstate_msrs_not_valid(void)
 {
-	/* Check that all the msr's we are using are valid. */
-	u64 aperf, mperf, tmp;
-
-	rdmsrl(MSR_IA32_APERF, aperf);
-	rdmsrl(MSR_IA32_MPERF, mperf);
-
 	if (!pstate_funcs.get_max() ||
 	    !pstate_funcs.get_min() ||
 	    !pstate_funcs.get_turbo())
 		return -ENODEV;
 
-	rdmsrl(MSR_IA32_APERF, tmp);
-	if (!(tmp - aperf))
-		return -ENODEV;
-
-	rdmsrl(MSR_IA32_MPERF, tmp);
-	if (!(tmp - mperf))
-		return -ENODEV;
-
 	return 0;
 }
 
diff --git a/drivers/cpufreq/powernv-cpufreq.c b/drivers/cpufreq/powernv-cpufreq.c
index 2dfd4fdb5a52..ebef0d8279c7 100644
--- a/drivers/cpufreq/powernv-cpufreq.c
+++ b/drivers/cpufreq/powernv-cpufreq.c
@@ -34,9 +34,13 @@
 #include <asm/smp.h> /* Required for cpu_sibling_mask() in UP configs */
 
 #define POWERNV_MAX_PSTATES	256
+#define PMSR_PSAFE_ENABLE	(1UL << 30)
+#define PMSR_SPR_EM_DISABLE	(1UL << 31)
+#define PMSR_MAX(x)		((x >> 32) & 0xFF)
+#define PMSR_LP(x)		((x >> 48) & 0xFF)
 
 static struct cpufreq_frequency_table powernv_freqs[POWERNV_MAX_PSTATES+1];
-static bool rebooting;
+static bool rebooting, throttled;
 
 /*
  * Note: The set of pstates consists of contiguous integers, the
@@ -294,6 +298,44 @@ static inline unsigned int get_nominal_index(void)
 	return powernv_pstate_info.max - powernv_pstate_info.nominal;
 }
 
+static void powernv_cpufreq_throttle_check(unsigned int cpu)
+{
+	unsigned long pmsr;
+	int pmsr_pmax, pmsr_lp;
+
+	pmsr = get_pmspr(SPRN_PMSR);
+
+	/* Check for Pmax Capping */
+	pmsr_pmax = (s8)PMSR_MAX(pmsr);
+	if (pmsr_pmax != powernv_pstate_info.max) {
+		throttled = true;
+		pr_info("CPU %d Pmax is reduced to %d\n", cpu, pmsr_pmax);
+		pr_info("Max allowed Pstate is capped\n");
+	}
+
+	/*
+	 * Check for Psafe by reading LocalPstate
+	 * or check if Psafe_mode_active is set in PMSR.
+	 */
+	pmsr_lp = (s8)PMSR_LP(pmsr);
+	if ((pmsr_lp < powernv_pstate_info.min) ||
+				(pmsr & PMSR_PSAFE_ENABLE)) {
+		throttled = true;
+		pr_info("Pstate set to safe frequency\n");
+	}
+
+	/* Check if SPR_EM_DISABLE is set in PMSR */
+	if (pmsr & PMSR_SPR_EM_DISABLE) {
+		throttled = true;
+		pr_info("Frequency Control disabled from OS\n");
+	}
+
+	if (throttled) {
+		pr_info("PMSR = %16lx\n", pmsr);
+		pr_crit("CPU Frequency could be throttled\n");
+	}
+}
+
 /*
  * powernv_cpufreq_target_index: Sets the frequency corresponding to
  * the cpufreq table entry indexed by new_index on the cpus in the
@@ -307,6 +349,9 @@ static int powernv_cpufreq_target_index(struct cpufreq_policy *policy,
 	if (unlikely(rebooting) && new_index != get_nominal_index())
 		return 0;
 
+	if (!throttled)
+		powernv_cpufreq_throttle_check(smp_processor_id());
+
 	freq_data.pstate_id = powernv_freqs[new_index].driver_data;
 
 	/*
diff --git a/drivers/cpufreq/ppc-corenet-cpufreq.c b/drivers/cpufreq/qoriq-cpufreq.c
index 7cb4b766cf94..88b21ae0d6b0 100644
--- a/drivers/cpufreq/ppc-corenet-cpufreq.c
+++ b/drivers/cpufreq/qoriq-cpufreq.c
@@ -1,7 +1,7 @@
 /*
  * Copyright 2013 Freescale Semiconductor, Inc.
  *
- * CPU Frequency Scaling driver for Freescale PowerPC corenet SoCs.
+ * CPU Frequency Scaling driver for Freescale QorIQ SoCs.
  *
  * 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
@@ -20,12 +20,13 @@
 #include <linux/of.h>
 #include <linux/slab.h>
 #include <linux/smp.h>
-#include <sysdev/fsl_soc.h>
 
+#if !defined(CONFIG_ARM)
 #include <asm/smp.h>	/* for get_hard_smp_processor_id() in UP configs */
+#endif
 
 /**
- * struct cpu_data - per CPU data struct
+ * struct cpu_data
  * @parent: the parent node of cpu clock
  * @table: frequency table
  */
@@ -69,17 +70,78 @@ static const struct soc_data sdata[] = {
 static u32 min_cpufreq;
 static const u32 *fmask;
 
-static DEFINE_PER_CPU(struct cpu_data *, cpu_data);
+#if defined(CONFIG_ARM)
+static int get_cpu_physical_id(int cpu)
+{
+	return topology_core_id(cpu);
+}
+#else
+static int get_cpu_physical_id(int cpu)
+{
+	return get_hard_smp_processor_id(cpu);
+}
+#endif
 
-/* cpumask in a cluster */
-static DEFINE_PER_CPU(cpumask_var_t, cpu_mask);
+static u32 get_bus_freq(void)
+{
+	struct device_node *soc;
+	u32 sysfreq;
+
+	soc = of_find_node_by_type(NULL, "soc");
+	if (!soc)
+		return 0;
+
+	if (of_property_read_u32(soc, "bus-frequency", &sysfreq))
+		sysfreq = 0;
 
-#ifndef CONFIG_SMP
-static inline const struct cpumask *cpu_core_mask(int cpu)
+	of_node_put(soc);
+
+	return sysfreq;
+}
+
+static struct device_node *cpu_to_clk_node(int cpu)
 {
-	return cpumask_of(0);
+	struct device_node *np, *clk_np;
+
+	if (!cpu_present(cpu))
+		return NULL;
+
+	np = of_get_cpu_node(cpu, NULL);
+	if (!np)
+		return NULL;
+
+	clk_np = of_parse_phandle(np, "clocks", 0);
+	if (!clk_np)
+		return NULL;
+
+	of_node_put(np);
+
+	return clk_np;
+}
+
+/* traverse cpu nodes to get cpu mask of sharing clock wire */
+static void set_affected_cpus(struct cpufreq_policy *policy)
+{
+	struct device_node *np, *clk_np;
+	struct cpumask *dstp = policy->cpus;
+	int i;
+
+	np = cpu_to_clk_node(policy->cpu);
+	if (!np)
+		return;
+
+	for_each_present_cpu(i) {
+		clk_np = cpu_to_clk_node(i);
+		if (!clk_np)
+			continue;
+
+		if (clk_np == np)
+			cpumask_set_cpu(i, dstp);
+
+		of_node_put(clk_np);
+	}
+	of_node_put(np);
 }
-#endif
 
 /* reduce the duplicated frequencies in frequency table */
 static void freq_table_redup(struct cpufreq_frequency_table *freq_table,
@@ -107,6 +169,7 @@ static void freq_table_sort(struct cpufreq_frequency_table *freq_table,
 	int i, j, ind;
 	unsigned int freq, max_freq;
 	struct cpufreq_frequency_table table;
+
 	for (i = 0; i < count - 1; i++) {
 		max_freq = freq_table[i].frequency;
 		ind = i;
@@ -131,7 +194,7 @@ static void freq_table_sort(struct cpufreq_frequency_table *freq_table,
 	}
 }
 
-static int corenet_cpufreq_cpu_init(struct cpufreq_policy *policy)
+static int qoriq_cpufreq_cpu_init(struct cpufreq_policy *policy)
 {
 	struct device_node *np;
 	int i, count, ret;
@@ -147,10 +210,8 @@ static int corenet_cpufreq_cpu_init(struct cpufreq_policy *policy)
 		return -ENODEV;
 
 	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		pr_err("%s: no memory\n", __func__);
+	if (!data)
 		goto err_np;
-	}
 
 	policy->clk = of_clk_get(np, 0);
 	if (IS_ERR(policy->clk)) {
@@ -172,7 +233,7 @@ static int corenet_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	}
 
 	if (fmask)
-		mask = fmask[get_hard_smp_processor_id(cpu)];
+		mask = fmask[get_cpu_physical_id(cpu)];
 	else
 		mask = 0x0;
 
@@ -203,13 +264,12 @@ static int corenet_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	data->table = table;
 
 	/* update ->cpus if we have cluster, no harm if not */
-	cpumask_copy(policy->cpus, per_cpu(cpu_mask, cpu));
-	for_each_cpu(i, per_cpu(cpu_mask, cpu))
-		per_cpu(cpu_data, i) = data;
+	set_affected_cpus(policy);
+	policy->driver_data = data;
 
 	/* Minimum transition latency is 12 platform clocks */
 	u64temp = 12ULL * NSEC_PER_SEC;
-	do_div(u64temp, fsl_get_sys_freq());
+	do_div(u64temp, get_bus_freq());
 	policy->cpuinfo.transition_latency = u64temp + 1;
 
 	of_node_put(np);
@@ -221,7 +281,7 @@ err_nomem1:
 err_node:
 	of_node_put(data->parent);
 err_nomem2:
-	per_cpu(cpu_data, cpu) = NULL;
+	policy->driver_data = NULL;
 	kfree(data);
 err_np:
 	of_node_put(np);
@@ -229,43 +289,40 @@ err_np:
 	return -ENODEV;
 }
 
-static int __exit corenet_cpufreq_cpu_exit(struct cpufreq_policy *policy)
+static int __exit qoriq_cpufreq_cpu_exit(struct cpufreq_policy *policy)
 {
-	struct cpu_data *data = per_cpu(cpu_data, policy->cpu);
-	unsigned int cpu;
+	struct cpu_data *data = policy->driver_data;
 
 	of_node_put(data->parent);
 	kfree(data->table);
 	kfree(data);
-
-	for_each_cpu(cpu, per_cpu(cpu_mask, policy->cpu))
-		per_cpu(cpu_data, cpu) = NULL;
+	policy->driver_data = NULL;
 
 	return 0;
 }
 
-static int corenet_cpufreq_target(struct cpufreq_policy *policy,
+static int qoriq_cpufreq_target(struct cpufreq_policy *policy,
 		unsigned int index)
 {
 	struct clk *parent;
-	struct cpu_data *data = per_cpu(cpu_data, policy->cpu);
+	struct cpu_data *data = policy->driver_data;
 
 	parent = of_clk_get(data->parent, data->table[index].driver_data);
 	return clk_set_parent(policy->clk, parent);
 }
 
-static struct cpufreq_driver ppc_corenet_cpufreq_driver = {
-	.name		= "ppc_cpufreq",
+static struct cpufreq_driver qoriq_cpufreq_driver = {
+	.name		= "qoriq_cpufreq",
 	.flags		= CPUFREQ_CONST_LOOPS,
-	.init		= corenet_cpufreq_cpu_init,
-	.exit		= __exit_p(corenet_cpufreq_cpu_exit),
+	.init		= qoriq_cpufreq_cpu_init,
+	.exit		= __exit_p(qoriq_cpufreq_cpu_exit),
 	.verify		= cpufreq_generic_frequency_table_verify,
-	.target_index	= corenet_cpufreq_target,
+	.target_index	= qoriq_cpufreq_target,
 	.get		= cpufreq_generic_get,
 	.attr		= cpufreq_generic_attr,
 };
 
-static const struct of_device_id node_matches[] __initdata = {
+static const struct of_device_id node_matches[] __initconst = {
 	{ .compatible = "fsl,p2041-clockgen", .data = &sdata[0], },
 	{ .compatible = "fsl,p3041-clockgen", .data = &sdata[0], },
 	{ .compatible = "fsl,p5020-clockgen", .data = &sdata[1], },
@@ -275,61 +332,43 @@ static const struct of_device_id node_matches[] __initdata = {
 	{}
 };
 
-static int __init ppc_corenet_cpufreq_init(void)
+static int __init qoriq_cpufreq_init(void)
 {
 	int ret;
 	struct device_node  *np;
 	const struct of_device_id *match;
 	const struct soc_data *data;
-	unsigned int cpu;
 
 	np = of_find_matching_node(NULL, node_matches);
 	if (!np)
 		return -ENODEV;
 
-	for_each_possible_cpu(cpu) {
-		if (!alloc_cpumask_var(&per_cpu(cpu_mask, cpu), GFP_KERNEL))
-			goto err_mask;
-		cpumask_copy(per_cpu(cpu_mask, cpu), cpu_core_mask(cpu));
-	}
-
 	match = of_match_node(node_matches, np);
 	data = match->data;
 	if (data) {
 		if (data->flag)
 			fmask = data->freq_mask;
-		min_cpufreq = fsl_get_sys_freq();
+		min_cpufreq = get_bus_freq();
 	} else {
-		min_cpufreq = fsl_get_sys_freq() / 2;
+		min_cpufreq = get_bus_freq() / 2;
 	}
 
 	of_node_put(np);
 
-	ret = cpufreq_register_driver(&ppc_corenet_cpufreq_driver);
+	ret = cpufreq_register_driver(&qoriq_cpufreq_driver);
 	if (!ret)
-		pr_info("Freescale PowerPC corenet CPU frequency scaling driver\n");
+		pr_info("Freescale QorIQ CPU frequency scaling driver\n");
 
 	return ret;
-
-err_mask:
-	for_each_possible_cpu(cpu)
-		free_cpumask_var(per_cpu(cpu_mask, cpu));
-
-	return -ENOMEM;
 }
-module_init(ppc_corenet_cpufreq_init);
+module_init(qoriq_cpufreq_init);
 
-static void __exit ppc_corenet_cpufreq_exit(void)
+static void __exit qoriq_cpufreq_exit(void)
 {
-	unsigned int cpu;
-
-	for_each_possible_cpu(cpu)
-		free_cpumask_var(per_cpu(cpu_mask, cpu));
-
-	cpufreq_unregister_driver(&ppc_corenet_cpufreq_driver);
+	cpufreq_unregister_driver(&qoriq_cpufreq_driver);
 }
-module_exit(ppc_corenet_cpufreq_exit);
+module_exit(qoriq_cpufreq_exit);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Tang Yuantian <Yuantian.Tang@freescale.com>");
-MODULE_DESCRIPTION("cpufreq driver for Freescale e500mc series SoCs");
+MODULE_DESCRIPTION("cpufreq driver for Freescale QorIQ series SoCs");
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index c5029c1209b4..8c7930b5a65f 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -29,15 +29,10 @@ config DT_IDLE_STATES
 	bool
 
 menu "ARM CPU Idle Drivers"
-depends on ARM
+depends on ARM || ARM64
 source "drivers/cpuidle/Kconfig.arm"
 endmenu
 
-menu "ARM64 CPU Idle Drivers"
-depends on ARM64
-source "drivers/cpuidle/Kconfig.arm64"
-endmenu
-
 menu "MIPS CPU Idle Drivers"
 depends on MIPS
 source "drivers/cpuidle/Kconfig.mips"
diff --git a/drivers/cpuidle/Kconfig.arm b/drivers/cpuidle/Kconfig.arm
index 8e07c9419153..21340e0be73e 100644
--- a/drivers/cpuidle/Kconfig.arm
+++ b/drivers/cpuidle/Kconfig.arm
@@ -1,10 +1,20 @@
 #
 # ARM CPU Idle drivers
 #
+config ARM_CPUIDLE
+        bool "Generic ARM/ARM64 CPU idle Driver"
+        select DT_IDLE_STATES
+        help
+          Select this to enable generic cpuidle driver for ARM.
+          It provides a generic idle driver whose idle states are configured
+          at run-time through DT nodes. The CPUidle suspend backend is
+          initialized by calling the CPU operations init idle hook
+          provided by architecture code.
+
 config ARM_BIG_LITTLE_CPUIDLE
 	bool "Support for ARM big.LITTLE processors"
 	depends on ARCH_VEXPRESS_TC2_PM || ARCH_EXYNOS
-	depends on MCPM
+	depends on MCPM && !ARM64
 	select ARM_CPU_SUSPEND
 	select CPU_IDLE_MULTIPLE_DRIVERS
 	select DT_IDLE_STATES
@@ -16,51 +26,51 @@ config ARM_BIG_LITTLE_CPUIDLE
 
 config ARM_CLPS711X_CPUIDLE
 	bool "CPU Idle Driver for CLPS711X processors"
-	depends on ARCH_CLPS711X || COMPILE_TEST
+	depends on ARCH_CLPS711X && !ARM64 || COMPILE_TEST
 	help
 	  Select this to enable cpuidle on Cirrus Logic CLPS711X SOCs.
 
 config ARM_HIGHBANK_CPUIDLE
 	bool "CPU Idle Driver for Calxeda processors"
-	depends on ARM_PSCI
+	depends on ARM_PSCI && !ARM64
 	select ARM_CPU_SUSPEND
 	help
 	  Select this to enable cpuidle on Calxeda processors.
 
 config ARM_KIRKWOOD_CPUIDLE
 	bool "CPU Idle Driver for Marvell Kirkwood SoCs"
-	depends on MACH_KIRKWOOD
+	depends on MACH_KIRKWOOD && !ARM64
 	help
 	  This adds the CPU Idle driver for Marvell Kirkwood SoCs.
 
 config ARM_ZYNQ_CPUIDLE
 	bool "CPU Idle Driver for Xilinx Zynq processors"
-	depends on ARCH_ZYNQ
+	depends on ARCH_ZYNQ && !ARM64
 	help
 	  Select this to enable cpuidle on Xilinx Zynq processors.
 
 config ARM_U8500_CPUIDLE
 	bool "Cpu Idle Driver for the ST-E u8500 processors"
-	depends on ARCH_U8500
+	depends on ARCH_U8500 && !ARM64
 	help
 	  Select this to enable cpuidle for ST-E u8500 processors
 
 config ARM_AT91_CPUIDLE
 	bool "Cpu Idle Driver for the AT91 processors"
 	default y
-	depends on ARCH_AT91
+	depends on ARCH_AT91 && !ARM64
 	help
 	  Select this to enable cpuidle for AT91 processors
 
 config ARM_EXYNOS_CPUIDLE
 	bool "Cpu Idle Driver for the Exynos processors"
-	depends on ARCH_EXYNOS
+	depends on ARCH_EXYNOS && !ARM64
 	select ARCH_NEEDS_CPU_IDLE_COUPLED if SMP
 	help
 	  Select this to enable cpuidle for Exynos processors
 
 config ARM_MVEBU_V7_CPUIDLE
 	bool "CPU Idle Driver for mvebu v7 family processors"
-	depends on ARCH_MVEBU
+	depends on ARCH_MVEBU && !ARM64
 	help
 	  Select this to enable cpuidle on Armada 370, 38x and XP processors.
diff --git a/drivers/cpuidle/Kconfig.arm64 b/drivers/cpuidle/Kconfig.arm64
deleted file mode 100644
index 6effb3656735..000000000000
--- a/drivers/cpuidle/Kconfig.arm64
+++ /dev/null
@@ -1,13 +0,0 @@
-#
-# ARM64 CPU Idle drivers
-#
-
-config ARM64_CPUIDLE
-	bool "Generic ARM64 CPU idle Driver"
-	select DT_IDLE_STATES
-	help
-	  Select this to enable generic cpuidle driver for ARM64.
-	  It provides a generic idle driver whose idle states are configured
-	  at run-time through DT nodes. The CPUidle suspend backend is
-	  initialized by calling the CPU operations init idle hook
-	  provided by architecture code.
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index 4d177b916f75..3ba81b1dffad 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -17,16 +17,13 @@ obj-$(CONFIG_ARM_ZYNQ_CPUIDLE)		+= cpuidle-zynq.o
 obj-$(CONFIG_ARM_U8500_CPUIDLE)         += cpuidle-ux500.o
 obj-$(CONFIG_ARM_AT91_CPUIDLE)          += cpuidle-at91.o
 obj-$(CONFIG_ARM_EXYNOS_CPUIDLE)        += cpuidle-exynos.o
+obj-$(CONFIG_ARM_CPUIDLE)		+= cpuidle-arm.o
 
 ###############################################################################
 # MIPS drivers
 obj-$(CONFIG_MIPS_CPS_CPUIDLE)		+= cpuidle-cps.o
 
 ###############################################################################
-# ARM64 drivers
-obj-$(CONFIG_ARM64_CPUIDLE)		+= cpuidle-arm64.o
-
-###############################################################################
 # POWERPC drivers
 obj-$(CONFIG_PSERIES_CPUIDLE)		+= cpuidle-pseries.o
 obj-$(CONFIG_POWERNV_CPUIDLE)		+= cpuidle-powernv.o
diff --git a/drivers/cpuidle/cpuidle-arm64.c b/drivers/cpuidle/cpuidle-arm.c
index 39a2c62716c3..545069d5fdfb 100644
--- a/drivers/cpuidle/cpuidle-arm64.c
+++ b/drivers/cpuidle/cpuidle-arm.c
@@ -1,5 +1,5 @@
 /*
- * ARM64 generic CPU idle driver.
+ * ARM/ARM64 generic CPU idle driver.
  *
  * Copyright (C) 2014 ARM Ltd.
  * Author: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
@@ -9,7 +9,7 @@
  * published by the Free Software Foundation.
  */
 
-#define pr_fmt(fmt) "CPUidle arm64: " fmt
+#define pr_fmt(fmt) "CPUidle arm: " fmt
 
 #include <linux/cpuidle.h>
 #include <linux/cpumask.h>
@@ -17,13 +17,14 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/slab.h>
 
 #include <asm/cpuidle.h>
 
 #include "dt_idle_states.h"
 
 /*
- * arm64_enter_idle_state - Programs CPU to enter the specified state
+ * arm_enter_idle_state - Programs CPU to enter the specified state
  *
  * dev: cpuidle device
  * drv: cpuidle driver
@@ -32,8 +33,8 @@
  * Called from the CPUidle framework to program the device to the
  * specified target state selected by the governor.
  */
-static int arm64_enter_idle_state(struct cpuidle_device *dev,
-				  struct cpuidle_driver *drv, int idx)
+static int arm_enter_idle_state(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv, int idx)
 {
 	int ret;
 
@@ -49,7 +50,7 @@ static int arm64_enter_idle_state(struct cpuidle_device *dev,
 		 * call the CPU ops suspend protocol with idle index as a
 		 * parameter.
 		 */
-		ret = cpu_suspend(idx);
+		arm_cpuidle_suspend(idx);
 
 		cpu_pm_exit();
 	}
@@ -57,8 +58,8 @@ static int arm64_enter_idle_state(struct cpuidle_device *dev,
 	return ret ? -1 : idx;
 }
 
-static struct cpuidle_driver arm64_idle_driver = {
-	.name = "arm64_idle",
+static struct cpuidle_driver arm_idle_driver = {
+	.name = "arm_idle",
 	.owner = THIS_MODULE,
 	/*
 	 * State at index 0 is standby wfi and considered standard
@@ -68,32 +69,33 @@ static struct cpuidle_driver arm64_idle_driver = {
 	 * handler for idle state index 0.
 	 */
 	.states[0] = {
-		.enter                  = arm64_enter_idle_state,
+		.enter                  = arm_enter_idle_state,
 		.exit_latency           = 1,
 		.target_residency       = 1,
 		.power_usage		= UINT_MAX,
 		.name                   = "WFI",
-		.desc                   = "ARM64 WFI",
+		.desc                   = "ARM WFI",
 	}
 };
 
-static const struct of_device_id arm64_idle_state_match[] __initconst = {
+static const struct of_device_id arm_idle_state_match[] __initconst = {
 	{ .compatible = "arm,idle-state",
-	  .data = arm64_enter_idle_state },
+	  .data = arm_enter_idle_state },
 	{ },
 };
 
 /*
- * arm64_idle_init
+ * arm_idle_init
  *
- * Registers the arm64 specific cpuidle driver with the cpuidle
+ * Registers the arm specific cpuidle driver with the cpuidle
  * framework. It relies on core code to parse the idle states
  * and initialize them using driver data structures accordingly.
  */
-static int __init arm64_idle_init(void)
+static int __init arm_idle_init(void)
 {
 	int cpu, ret;
-	struct cpuidle_driver *drv = &arm64_idle_driver;
+	struct cpuidle_driver *drv = &arm_idle_driver;
+	struct cpuidle_device *dev;
 
 	/*
 	 * Initialize idle states data, starting at index 1.
@@ -101,22 +103,61 @@ static int __init arm64_idle_init(void)
 	 * let the driver initialization fail accordingly since there is no
 	 * reason to initialize the idle driver if only wfi is supported.
 	 */
-	ret = dt_init_idle_driver(drv, arm64_idle_state_match, 1);
+	ret = dt_init_idle_driver(drv, arm_idle_state_match, 1);
 	if (ret <= 0)
 		return ret ? : -ENODEV;
 
+	ret = cpuidle_register_driver(drv);
+	if (ret) {
+		pr_err("Failed to register cpuidle driver\n");
+		return ret;
+	}
+
 	/*
 	 * Call arch CPU operations in order to initialize
 	 * idle states suspend back-end specific data
 	 */
 	for_each_possible_cpu(cpu) {
-		ret = cpu_init_idle(cpu);
+		ret = arm_cpuidle_init(cpu);
+
+		/*
+		 * Skip the cpuidle device initialization if the reported
+		 * failure is a HW misconfiguration/breakage (-ENXIO).
+		 */
+		if (ret == -ENXIO)
+			continue;
+
 		if (ret) {
 			pr_err("CPU %d failed to init idle CPU ops\n", cpu);
-			return ret;
+			goto out_fail;
+		}
+
+		dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+		if (!dev) {
+			pr_err("Failed to allocate cpuidle device\n");
+			goto out_fail;
+		}
+		dev->cpu = cpu;
+
+		ret = cpuidle_register_device(dev);
+		if (ret) {
+			pr_err("Failed to register cpuidle device for CPU %d\n",
+			       cpu);
+			kfree(dev);
+			goto out_fail;
 		}
 	}
 
-	return cpuidle_register(drv, NULL);
+	return 0;
+out_fail:
+	while (--cpu >= 0) {
+		dev = per_cpu(cpuidle_devices, cpu);
+		cpuidle_unregister_device(dev);
+		kfree(dev);
+	}
+
+	cpuidle_unregister_driver(drv);
+
+	return ret;
 }
-device_initcall(arm64_idle_init);
+device_initcall(arm_idle_init);
diff --git a/drivers/cpuidle/cpuidle-at91.c b/drivers/cpuidle/cpuidle-at91.c
index aae7bfc1ea36..f2446c78d87c 100644
--- a/drivers/cpuidle/cpuidle-at91.c
+++ b/drivers/cpuidle/cpuidle-at91.c
@@ -19,7 +19,6 @@
 #include <linux/cpuidle.h>
 #include <linux/io.h>
 #include <linux/export.h>
-#include <asm/proc-fns.h>
 #include <asm/cpuidle.h>
 
 #define AT91_MAX_STATES	2
diff --git a/drivers/cpuidle/cpuidle-exynos.c b/drivers/cpuidle/cpuidle-exynos.c
index 26f5f29fdb03..0c06ea2f50bb 100644
--- a/drivers/cpuidle/cpuidle-exynos.c
+++ b/drivers/cpuidle/cpuidle-exynos.c
@@ -19,7 +19,6 @@
 #include <linux/of.h>
 #include <linux/platform_data/cpuidle-exynos.h>
 
-#include <asm/proc-fns.h>
 #include <asm/suspend.h>
 #include <asm/cpuidle.h>
 
diff --git a/drivers/cpuidle/cpuidle-kirkwood.c b/drivers/cpuidle/cpuidle-kirkwood.c
index cea0a6c4b1db..d23d8f468c12 100644
--- a/drivers/cpuidle/cpuidle-kirkwood.c
+++ b/drivers/cpuidle/cpuidle-kirkwood.c
@@ -21,7 +21,6 @@
 #include <linux/cpuidle.h>
 #include <linux/io.h>
 #include <linux/export.h>
-#include <asm/proc-fns.h>
 #include <asm/cpuidle.h>
 
 #define KIRKWOOD_MAX_STATES	2
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-ux500.c b/drivers/cpuidle/cpuidle-ux500.c
index 66f81e410f0d..8bf895c0017d 100644
--- a/drivers/cpuidle/cpuidle-ux500.c
+++ b/drivers/cpuidle/cpuidle-ux500.c
@@ -19,7 +19,6 @@
 #include <linux/platform_device.h>
 
 #include <asm/cpuidle.h>
-#include <asm/proc-fns.h>
 
 static atomic_t master = ATOMIC_INIT(0);
 static DEFINE_SPINLOCK(master_lock);
diff --git a/drivers/cpuidle/cpuidle-zynq.c b/drivers/cpuidle/cpuidle-zynq.c
index 002b8c9f98f5..543292b1d38e 100644
--- a/drivers/cpuidle/cpuidle-zynq.c
+++ b/drivers/cpuidle/cpuidle-zynq.c
@@ -28,7 +28,6 @@
 #include <linux/init.h>
 #include <linux/cpuidle.h>
 #include <linux/platform_device.h>
-#include <asm/proc-fns.h>
 #include <asm/cpuidle.h>
 
 #define ZYNQ_MAX_STATES		2
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 080bd2dbde4b..7a73a279e179 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -330,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/crypto/Kconfig b/drivers/crypto/Kconfig
index 2fb0fdfc87df..800bf41718e1 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -391,7 +391,7 @@ config CRYPTO_DEV_ATMEL_SHA
 
 config CRYPTO_DEV_CCP
 	bool "Support for AMD Cryptographic Coprocessor"
-	depends on (X86 && PCI) || ARM64
+	depends on ((X86 && PCI) || (ARM64 && (OF_ADDRESS || ACPI))) && HAS_IOMEM
 	default n
 	help
 	  The AMD Cryptographic Coprocessor provides hardware support
@@ -436,4 +436,26 @@ config CRYPTO_DEV_QCE
 	  hardware. To compile this driver as a module, choose M here. The
 	  module will be called qcrypto.
 
+config CRYPTO_DEV_VMX
+	bool "Support for VMX cryptographic acceleration instructions"
+	depends on PPC64
+	default n
+	help
+	  Support for VMX cryptographic acceleration instructions.
+
+source "drivers/crypto/vmx/Kconfig"
+
+config CRYPTO_DEV_IMGTEC_HASH
+	depends on MIPS || COMPILE_TEST
+	tristate "Imagination Technologies hardware hash accelerator"
+	select CRYPTO_ALGAPI
+	select CRYPTO_MD5
+	select CRYPTO_SHA1
+	select CRYPTO_SHA256
+	select CRYPTO_HASH
+	help
+	  This driver interfaces with the Imagination Technologies
+	  hardware hash accelerator. Supporting MD5/SHA1/SHA224/SHA256
+	  hashing algorithms.
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 3924f93d5774..fb84be7e6be5 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_CRYPTO_DEV_CCP) += ccp/
 obj-$(CONFIG_CRYPTO_DEV_FSL_CAAM) += caam/
 obj-$(CONFIG_CRYPTO_DEV_GEODE) += geode-aes.o
 obj-$(CONFIG_CRYPTO_DEV_HIFN_795X) += hifn_795x.o
+obj-$(CONFIG_CRYPTO_DEV_IMGTEC_HASH) += img-hash.o
 obj-$(CONFIG_CRYPTO_DEV_IXP4XX) += ixp4xx_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_MV_CESA) += mv_cesa.o
 obj-$(CONFIG_CRYPTO_DEV_MXS_DCP) += mxs-dcp.o
@@ -25,3 +26,4 @@ obj-$(CONFIG_CRYPTO_DEV_TALITOS) += talitos.o
 obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
 obj-$(CONFIG_CRYPTO_DEV_QAT) += qat/
 obj-$(CONFIG_CRYPTO_DEV_QCE) += qce/
+obj-$(CONFIG_CRYPTO_DEV_VMX) += vmx/
diff --git a/drivers/crypto/amcc/crypto4xx_core.c b/drivers/crypto/amcc/crypto4xx_core.c
index d02b77150070..3b28e8c3de28 100644
--- a/drivers/crypto/amcc/crypto4xx_core.c
+++ b/drivers/crypto/amcc/crypto4xx_core.c
@@ -1155,7 +1155,7 @@ struct crypto4xx_alg_common crypto4xx_alg[] = {
 /**
  * Module Initialization Routine
  */
-static int __init crypto4xx_probe(struct platform_device *ofdev)
+static int crypto4xx_probe(struct platform_device *ofdev)
 {
 	int rc;
 	struct resource res;
@@ -1263,7 +1263,7 @@ err_alloc_dev:
 	return rc;
 }
 
-static int __exit crypto4xx_remove(struct platform_device *ofdev)
+static int crypto4xx_remove(struct platform_device *ofdev)
 {
 	struct device *dev = &ofdev->dev;
 	struct crypto4xx_core_device *core_dev = dev_get_drvdata(dev);
@@ -1291,7 +1291,7 @@ static struct platform_driver crypto4xx_driver = {
 		.of_match_table = crypto4xx_match,
 	},
 	.probe		= crypto4xx_probe,
-	.remove		= __exit_p(crypto4xx_remove),
+	.remove		= crypto4xx_remove,
 };
 
 module_platform_driver(crypto4xx_driver);
diff --git a/drivers/crypto/atmel-aes.c b/drivers/crypto/atmel-aes.c
index 6597aac9905d..0f9a9dc06a83 100644
--- a/drivers/crypto/atmel-aes.c
+++ b/drivers/crypto/atmel-aes.c
@@ -315,10 +315,10 @@ static int atmel_aes_crypt_dma(struct atmel_aes_dev *dd,
 
 	dd->dma_size = length;
 
-	if (!(dd->flags & AES_FLAGS_FAST)) {
-		dma_sync_single_for_device(dd->dev, dma_addr_in, length,
-					   DMA_TO_DEVICE);
-	}
+	dma_sync_single_for_device(dd->dev, dma_addr_in, length,
+				   DMA_TO_DEVICE);
+	dma_sync_single_for_device(dd->dev, dma_addr_out, length,
+				   DMA_FROM_DEVICE);
 
 	if (dd->flags & AES_FLAGS_CFB8) {
 		dd->dma_lch_in.dma_conf.dst_addr_width =
@@ -391,6 +391,11 @@ static int atmel_aes_crypt_cpu_start(struct atmel_aes_dev *dd)
 {
 	dd->flags &= ~AES_FLAGS_DMA;
 
+	dma_sync_single_for_cpu(dd->dev, dd->dma_addr_in,
+				dd->dma_size, DMA_TO_DEVICE);
+	dma_sync_single_for_cpu(dd->dev, dd->dma_addr_out,
+				dd->dma_size, DMA_FROM_DEVICE);
+
 	/* use cache buffers */
 	dd->nb_in_sg = atmel_aes_sg_length(dd->req, dd->in_sg);
 	if (!dd->nb_in_sg)
@@ -459,6 +464,9 @@ static int atmel_aes_crypt_dma_start(struct atmel_aes_dev *dd)
 		dd->flags |= AES_FLAGS_FAST;
 
 	} else {
+		dma_sync_single_for_cpu(dd->dev, dd->dma_addr_in,
+					dd->dma_size, DMA_TO_DEVICE);
+
 		/* use cache buffers */
 		count = atmel_aes_sg_copy(&dd->in_sg, &dd->in_offset,
 				dd->buf_in, dd->buflen, dd->total, 0);
@@ -619,7 +627,7 @@ static int atmel_aes_crypt_dma_stop(struct atmel_aes_dev *dd)
 			dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_FROM_DEVICE);
 			dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
 		} else {
-			dma_sync_single_for_device(dd->dev, dd->dma_addr_out,
+			dma_sync_single_for_cpu(dd->dev, dd->dma_addr_out,
 				dd->dma_size, DMA_FROM_DEVICE);
 
 			/* copy data */
@@ -1246,6 +1254,11 @@ static void atmel_aes_get_cap(struct atmel_aes_dev *dd)
 
 	/* keep only major version number */
 	switch (dd->hw_version & 0xff0) {
+	case 0x200:
+		dd->caps.has_dualbuff = 1;
+		dd->caps.has_cfb64 = 1;
+		dd->caps.max_burst_size = 4;
+		break;
 	case 0x130:
 		dd->caps.has_dualbuff = 1;
 		dd->caps.has_cfb64 = 1;
@@ -1336,6 +1349,7 @@ static int atmel_aes_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, aes_dd);
 
 	INIT_LIST_HEAD(&aes_dd->list);
+	spin_lock_init(&aes_dd->lock);
 
 	tasklet_init(&aes_dd->done_task, atmel_aes_done_task,
 					(unsigned long)aes_dd);
@@ -1374,7 +1388,7 @@ static int atmel_aes_probe(struct platform_device *pdev)
 	/* Initializing the clock */
 	aes_dd->iclk = clk_get(&pdev->dev, "aes_clk");
 	if (IS_ERR(aes_dd->iclk)) {
-		dev_err(dev, "clock intialization failed.\n");
+		dev_err(dev, "clock initialization failed.\n");
 		err = PTR_ERR(aes_dd->iclk);
 		goto clk_err;
 	}
diff --git a/drivers/crypto/atmel-sha.c b/drivers/crypto/atmel-sha.c
index 34db04addc18..5b35433c5399 100644
--- a/drivers/crypto/atmel-sha.c
+++ b/drivers/crypto/atmel-sha.c
@@ -163,8 +163,20 @@ static size_t atmel_sha_append_sg(struct atmel_sha_reqctx *ctx)
 		count = min(ctx->sg->length - ctx->offset, ctx->total);
 		count = min(count, ctx->buflen - ctx->bufcnt);
 
-		if (count <= 0)
-			break;
+		if (count <= 0) {
+			/*
+			* Check if count <= 0 because the buffer is full or
+			* because the sg length is 0. In the latest case,
+			* check if there is another sg in the list, a 0 length
+			* sg doesn't necessarily mean the end of the sg list.
+			*/
+			if ((ctx->sg->length == 0) && !sg_is_last(ctx->sg)) {
+				ctx->sg = sg_next(ctx->sg);
+				continue;
+			} else {
+				break;
+			}
+		}
 
 		scatterwalk_map_and_copy(ctx->buffer + ctx->bufcnt, ctx->sg,
 			ctx->offset, count, 0);
@@ -420,14 +432,8 @@ static int atmel_sha_xmit_dma(struct atmel_sha_dev *dd, dma_addr_t dma_addr1,
 	dev_dbg(dd->dev, "xmit_dma: digcnt: 0x%llx 0x%llx, length: %d, final: %d\n",
 		ctx->digcnt[1], ctx->digcnt[0], length1, final);
 
-	if (ctx->flags & (SHA_FLAGS_SHA1 | SHA_FLAGS_SHA224 |
-			SHA_FLAGS_SHA256)) {
-		dd->dma_lch_in.dma_conf.src_maxburst = 16;
-		dd->dma_lch_in.dma_conf.dst_maxburst = 16;
-	} else {
-		dd->dma_lch_in.dma_conf.src_maxburst = 32;
-		dd->dma_lch_in.dma_conf.dst_maxburst = 32;
-	}
+	dd->dma_lch_in.dma_conf.src_maxburst = 16;
+	dd->dma_lch_in.dma_conf.dst_maxburst = 16;
 
 	dmaengine_slave_config(dd->dma_lch_in.chan, &dd->dma_lch_in.dma_conf);
 
@@ -529,7 +535,7 @@ static int atmel_sha_update_dma_slow(struct atmel_sha_dev *dd)
 	if (final)
 		atmel_sha_fill_padding(ctx, 0);
 
-	if (final || (ctx->bufcnt == ctx->buflen && ctx->total)) {
+	if (final || (ctx->bufcnt == ctx->buflen)) {
 		count = ctx->bufcnt;
 		ctx->bufcnt = 0;
 		return atmel_sha_xmit_dma_map(dd, ctx, count, final);
@@ -1266,6 +1272,12 @@ static void atmel_sha_get_cap(struct atmel_sha_dev *dd)
 
 	/* keep only major version number */
 	switch (dd->hw_version & 0xff0) {
+	case 0x420:
+		dd->caps.has_dma = 1;
+		dd->caps.has_dualbuff = 1;
+		dd->caps.has_sha224 = 1;
+		dd->caps.has_sha_384_512 = 1;
+		break;
 	case 0x410:
 		dd->caps.has_dma = 1;
 		dd->caps.has_dualbuff = 1;
@@ -1349,6 +1361,7 @@ static int atmel_sha_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, sha_dd);
 
 	INIT_LIST_HEAD(&sha_dd->list);
+	spin_lock_init(&sha_dd->lock);
 
 	tasklet_init(&sha_dd->done_task, atmel_sha_done_task,
 					(unsigned long)sha_dd);
@@ -1385,7 +1398,7 @@ static int atmel_sha_probe(struct platform_device *pdev)
 	/* Initializing the clock */
 	sha_dd->iclk = clk_get(&pdev->dev, "sha_clk");
 	if (IS_ERR(sha_dd->iclk)) {
-		dev_err(dev, "clock intialization failed.\n");
+		dev_err(dev, "clock initialization failed.\n");
 		err = PTR_ERR(sha_dd->iclk);
 		goto clk_err;
 	}
diff --git a/drivers/crypto/atmel-tdes.c b/drivers/crypto/atmel-tdes.c
index 258772d9b22f..ca2999709eb4 100644
--- a/drivers/crypto/atmel-tdes.c
+++ b/drivers/crypto/atmel-tdes.c
@@ -1370,6 +1370,7 @@ static int atmel_tdes_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, tdes_dd);
 
 	INIT_LIST_HEAD(&tdes_dd->list);
+	spin_lock_init(&tdes_dd->lock);
 
 	tasklet_init(&tdes_dd->done_task, atmel_tdes_done_task,
 					(unsigned long)tdes_dd);
@@ -1408,7 +1409,7 @@ static int atmel_tdes_probe(struct platform_device *pdev)
 	/* Initializing the clock */
 	tdes_dd->iclk = clk_get(&pdev->dev, "tdes_clk");
 	if (IS_ERR(tdes_dd->iclk)) {
-		dev_err(dev, "clock intialization failed.\n");
+		dev_err(dev, "clock initialization failed.\n");
 		err = PTR_ERR(tdes_dd->iclk);
 		goto clk_err;
 	}
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index f347ab7eea95..ba0532efd3ae 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -1172,6 +1172,7 @@ static int ahash_final_no_ctx(struct ahash_request *req)
 		return -ENOMEM;
 	}
 
+	edesc->sec4_sg_bytes = 0;
 	sh_len = desc_len(sh_desc);
 	desc = edesc->hw_desc;
 	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
diff --git a/drivers/crypto/caam/caamrng.c b/drivers/crypto/caam/caamrng.c
index ae31e555793c..26a544b505f1 100644
--- a/drivers/crypto/caam/caamrng.c
+++ b/drivers/crypto/caam/caamrng.c
@@ -52,7 +52,7 @@
 
 /* length of descriptors */
 #define DESC_JOB_O_LEN			(CAAM_CMD_SZ * 2 + CAAM_PTR_SZ * 2)
-#define DESC_RNG_LEN			(10 * CAAM_CMD_SZ)
+#define DESC_RNG_LEN			(4 * CAAM_CMD_SZ)
 
 /* Buffer, its dma address and lock */
 struct buf_data {
@@ -90,8 +90,8 @@ static inline void rng_unmap_ctx(struct caam_rng_ctx *ctx)
 	struct device *jrdev = ctx->jrdev;
 
 	if (ctx->sh_desc_dma)
-		dma_unmap_single(jrdev, ctx->sh_desc_dma, DESC_RNG_LEN,
-				 DMA_TO_DEVICE);
+		dma_unmap_single(jrdev, ctx->sh_desc_dma,
+				 desc_bytes(ctx->sh_desc), DMA_TO_DEVICE);
 	rng_unmap_buf(jrdev, &ctx->bufs[0]);
 	rng_unmap_buf(jrdev, &ctx->bufs[1]);
 }
diff --git a/drivers/crypto/ccp/Makefile b/drivers/crypto/ccp/Makefile
index 7f592d8d07bb..55a1f3951578 100644
--- a/drivers/crypto/ccp/Makefile
+++ b/drivers/crypto/ccp/Makefile
@@ -1,11 +1,6 @@
 obj-$(CONFIG_CRYPTO_DEV_CCP_DD) += ccp.o
-ccp-objs := ccp-dev.o ccp-ops.o
-ifdef CONFIG_X86
-ccp-objs += ccp-pci.o
-endif
-ifdef CONFIG_ARM64
-ccp-objs += ccp-platform.o
-endif
+ccp-objs := ccp-dev.o ccp-ops.o ccp-platform.o
+ccp-$(CONFIG_PCI) += ccp-pci.o
 
 obj-$(CONFIG_CRYPTO_DEV_CCP_CRYPTO) += ccp-crypto.o
 ccp-crypto-objs := ccp-crypto-main.o \
diff --git a/drivers/crypto/ccp/ccp-crypto-aes-cmac.c b/drivers/crypto/ccp/ccp-crypto-aes-cmac.c
index 8e162ad82085..ea7e8446956a 100644
--- a/drivers/crypto/ccp/ccp-crypto-aes-cmac.c
+++ b/drivers/crypto/ccp/ccp-crypto-aes-cmac.c
@@ -23,7 +23,6 @@
 
 #include "ccp-crypto.h"
 
-
 static int ccp_aes_cmac_complete(struct crypto_async_request *async_req,
 				 int ret)
 {
@@ -38,11 +37,13 @@ static int ccp_aes_cmac_complete(struct crypto_async_request *async_req,
 	if (rctx->hash_rem) {
 		/* Save remaining data to buffer */
 		unsigned int offset = rctx->nbytes - rctx->hash_rem;
+
 		scatterwalk_map_and_copy(rctx->buf, rctx->src,
 					 offset, rctx->hash_rem, 0);
 		rctx->buf_count = rctx->hash_rem;
-	} else
+	} else {
 		rctx->buf_count = 0;
+	}
 
 	/* Update result area if supplied */
 	if (req->result)
@@ -202,7 +203,7 @@ static int ccp_aes_cmac_digest(struct ahash_request *req)
 }
 
 static int ccp_aes_cmac_setkey(struct crypto_ahash *tfm, const u8 *key,
-			   unsigned int key_len)
+			       unsigned int key_len)
 {
 	struct ccp_ctx *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
 	struct ccp_crypto_ahash_alg *alg =
@@ -292,7 +293,8 @@ static int ccp_aes_cmac_cra_init(struct crypto_tfm *tfm)
 	crypto_ahash_set_reqsize(ahash, sizeof(struct ccp_aes_cmac_req_ctx));
 
 	cipher_tfm = crypto_alloc_cipher("aes", 0,
-			CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+					 CRYPTO_ALG_ASYNC |
+					 CRYPTO_ALG_NEED_FALLBACK);
 	if (IS_ERR(cipher_tfm)) {
 		pr_warn("could not load aes cipher driver\n");
 		return PTR_ERR(cipher_tfm);
@@ -354,7 +356,7 @@ int ccp_register_aes_cmac_algs(struct list_head *head)
 	ret = crypto_register_ahash(alg);
 	if (ret) {
 		pr_err("%s ahash algorithm registration error (%d)\n",
-			base->cra_name, ret);
+		       base->cra_name, ret);
 		kfree(ccp_alg);
 		return ret;
 	}
diff --git a/drivers/crypto/ccp/ccp-crypto-aes-xts.c b/drivers/crypto/ccp/ccp-crypto-aes-xts.c
index 0cc5594b7de3..52c7395cb8d8 100644
--- a/drivers/crypto/ccp/ccp-crypto-aes-xts.c
+++ b/drivers/crypto/ccp/ccp-crypto-aes-xts.c
@@ -21,7 +21,6 @@
 
 #include "ccp-crypto.h"
 
-
 struct ccp_aes_xts_def {
 	const char *name;
 	const char *drv_name;
@@ -216,7 +215,6 @@ static void ccp_aes_xts_cra_exit(struct crypto_tfm *tfm)
 	ctx->u.aes.tfm_ablkcipher = NULL;
 }
 
-
 static int ccp_register_aes_xts_alg(struct list_head *head,
 				    const struct ccp_aes_xts_def *def)
 {
@@ -255,7 +253,7 @@ static int ccp_register_aes_xts_alg(struct list_head *head,
 	ret = crypto_register_alg(alg);
 	if (ret) {
 		pr_err("%s ablkcipher algorithm registration error (%d)\n",
-			alg->cra_name, ret);
+		       alg->cra_name, ret);
 		kfree(ccp_alg);
 		return ret;
 	}
diff --git a/drivers/crypto/ccp/ccp-crypto-aes.c b/drivers/crypto/ccp/ccp-crypto-aes.c
index e46490db0f63..7984f910884d 100644
--- a/drivers/crypto/ccp/ccp-crypto-aes.c
+++ b/drivers/crypto/ccp/ccp-crypto-aes.c
@@ -22,7 +22,6 @@
 
 #include "ccp-crypto.h"
 
-
 static int ccp_aes_complete(struct crypto_async_request *async_req, int ret)
 {
 	struct ablkcipher_request *req = ablkcipher_request_cast(async_req);
@@ -345,7 +344,7 @@ static int ccp_register_aes_alg(struct list_head *head,
 	ret = crypto_register_alg(alg);
 	if (ret) {
 		pr_err("%s ablkcipher algorithm registration error (%d)\n",
-			alg->cra_name, ret);
+		       alg->cra_name, ret);
 		kfree(ccp_alg);
 		return ret;
 	}
diff --git a/drivers/crypto/ccp/ccp-crypto-main.c b/drivers/crypto/ccp/ccp-crypto-main.c
index 4d4e016d755b..bdec01ec608f 100644
--- a/drivers/crypto/ccp/ccp-crypto-main.c
+++ b/drivers/crypto/ccp/ccp-crypto-main.c
@@ -33,7 +33,6 @@ static unsigned int sha_disable;
 module_param(sha_disable, uint, 0444);
 MODULE_PARM_DESC(sha_disable, "Disable use of SHA - any non-zero value");
 
-
 /* List heads for the supported algorithms */
 static LIST_HEAD(hash_algs);
 static LIST_HEAD(cipher_algs);
@@ -48,6 +47,7 @@ struct ccp_crypto_queue {
 	struct list_head *backlog;
 	unsigned int cmd_count;
 };
+
 #define CCP_CRYPTO_MAX_QLEN	100
 
 static struct ccp_crypto_queue req_queue;
@@ -77,7 +77,6 @@ struct ccp_crypto_cpu {
 	int err;
 };
 
-
 static inline bool ccp_crypto_success(int err)
 {
 	if (err && (err != -EINPROGRESS) && (err != -EBUSY))
@@ -143,7 +142,7 @@ static void ccp_crypto_complete(void *data, int err)
 	int ret;
 
 	if (err == -EINPROGRESS) {
-		/* Only propogate the -EINPROGRESS if necessary */
+		/* Only propagate the -EINPROGRESS if necessary */
 		if (crypto_cmd->ret == -EBUSY) {
 			crypto_cmd->ret = -EINPROGRESS;
 			req->complete(req, -EINPROGRESS);
diff --git a/drivers/crypto/ccp/ccp-crypto-sha.c b/drivers/crypto/ccp/ccp-crypto-sha.c
index 96531571f7cf..507b34e0cc19 100644
--- a/drivers/crypto/ccp/ccp-crypto-sha.c
+++ b/drivers/crypto/ccp/ccp-crypto-sha.c
@@ -23,7 +23,6 @@
 
 #include "ccp-crypto.h"
 
-
 static int ccp_sha_complete(struct crypto_async_request *async_req, int ret)
 {
 	struct ahash_request *req = ahash_request_cast(async_req);
@@ -37,11 +36,13 @@ static int ccp_sha_complete(struct crypto_async_request *async_req, int ret)
 	if (rctx->hash_rem) {
 		/* Save remaining data to buffer */
 		unsigned int offset = rctx->nbytes - rctx->hash_rem;
+
 		scatterwalk_map_and_copy(rctx->buf, rctx->src,
 					 offset, rctx->hash_rem, 0);
 		rctx->buf_count = rctx->hash_rem;
-	} else
+	} else {
 		rctx->buf_count = 0;
+	}
 
 	/* Update result area if supplied */
 	if (req->result)
@@ -227,8 +228,9 @@ static int ccp_sha_setkey(struct crypto_ahash *tfm, const u8 *key,
 		}
 
 		key_len = digest_size;
-	} else
+	} else {
 		memcpy(ctx->u.sha.key, key, key_len);
+	}
 
 	for (i = 0; i < block_size; i++) {
 		ctx->u.sha.ipad[i] = ctx->u.sha.key[i] ^ 0x36;
@@ -355,7 +357,7 @@ static int ccp_register_hmac_alg(struct list_head *head,
 	ret = crypto_register_ahash(alg);
 	if (ret) {
 		pr_err("%s ahash algorithm registration error (%d)\n",
-			base->cra_name, ret);
+		       base->cra_name, ret);
 		kfree(ccp_alg);
 		return ret;
 	}
@@ -410,7 +412,7 @@ static int ccp_register_sha_alg(struct list_head *head,
 	ret = crypto_register_ahash(alg);
 	if (ret) {
 		pr_err("%s ahash algorithm registration error (%d)\n",
-			base->cra_name, ret);
+		       base->cra_name, ret);
 		kfree(ccp_alg);
 		return ret;
 	}
diff --git a/drivers/crypto/ccp/ccp-crypto.h b/drivers/crypto/ccp/ccp-crypto.h
index 9aa4ae184f7f..76a96f0f44c6 100644
--- a/drivers/crypto/ccp/ccp-crypto.h
+++ b/drivers/crypto/ccp/ccp-crypto.h
@@ -13,7 +13,6 @@
 #ifndef __CCP_CRYPTO_H__
 #define __CCP_CRYPTO_H__
 
-
 #include <linux/list.h>
 #include <linux/wait.h>
 #include <linux/pci.h>
@@ -25,7 +24,6 @@
 #include <crypto/hash.h>
 #include <crypto/sha.h>
 
-
 #define CCP_CRA_PRIORITY	300
 
 struct ccp_crypto_ablkcipher_alg {
@@ -68,7 +66,6 @@ static inline struct ccp_crypto_ahash_alg *
 	return container_of(ahash_alg, struct ccp_crypto_ahash_alg, alg);
 }
 
-
 /***** AES related defines *****/
 struct ccp_aes_ctx {
 	/* Fallback cipher for XTS with unsupported unit sizes */
diff --git a/drivers/crypto/ccp/ccp-dev.c b/drivers/crypto/ccp/ccp-dev.c
index ca29c120b85f..861bacc1bb94 100644
--- a/drivers/crypto/ccp/ccp-dev.c
+++ b/drivers/crypto/ccp/ccp-dev.c
@@ -37,7 +37,6 @@ struct ccp_tasklet_data {
 	struct ccp_cmd *cmd;
 };
 
-
 static struct ccp_device *ccp_dev;
 static inline struct ccp_device *ccp_get_device(void)
 {
@@ -296,11 +295,9 @@ struct ccp_device *ccp_alloc_struct(struct device *dev)
 {
 	struct ccp_device *ccp;
 
-	ccp = kzalloc(sizeof(*ccp), GFP_KERNEL);
-	if (ccp == NULL) {
-		dev_err(dev, "unable to allocate device struct\n");
+	ccp = devm_kzalloc(dev, sizeof(*ccp), GFP_KERNEL);
+	if (!ccp)
 		return NULL;
-	}
 	ccp->dev = dev;
 
 	INIT_LIST_HEAD(&ccp->cmd);
diff --git a/drivers/crypto/ccp/ccp-dev.h b/drivers/crypto/ccp/ccp-dev.h
index 62ff35a6b9ec..6ff89031fb96 100644
--- a/drivers/crypto/ccp/ccp-dev.h
+++ b/drivers/crypto/ccp/ccp-dev.h
@@ -21,7 +21,7 @@
 #include <linux/wait.h>
 #include <linux/dmapool.h>
 #include <linux/hw_random.h>
-
+#include <linux/bitops.h>
 
 #define MAX_DMAPOOL_NAME_LEN		32
 
@@ -33,7 +33,6 @@
 #define CACHE_NONE			0x00
 #define CACHE_WB_NO_ALLOC		0xb7
 
-
 /****** Register Mappings ******/
 #define Q_MASK_REG			0x000
 #define TRNG_OUT_REG			0x00c
@@ -54,8 +53,8 @@
 #define CMD_Q_CACHE_BASE		0x228
 #define CMD_Q_CACHE_INC			0x20
 
-#define CMD_Q_ERROR(__qs)		((__qs) & 0x0000003f);
-#define CMD_Q_DEPTH(__qs)		(((__qs) >> 12) & 0x0000000f);
+#define CMD_Q_ERROR(__qs)		((__qs) & 0x0000003f)
+#define CMD_Q_DEPTH(__qs)		(((__qs) >> 12) & 0x0000000f)
 
 /****** REQ0 Related Values ******/
 #define REQ0_WAIT_FOR_WRITE		0x00000004
@@ -103,7 +102,6 @@
 /****** REQ6 Related Values ******/
 #define REQ6_MEMTYPE_SHIFT		16
 
-
 /****** Key Storage Block ******/
 #define KSB_START			77
 #define KSB_END				127
@@ -114,7 +112,7 @@
 #define CCP_JOBID_MASK			0x0000003f
 
 #define CCP_DMAPOOL_MAX_SIZE		64
-#define CCP_DMAPOOL_ALIGN		(1 << 5)
+#define CCP_DMAPOOL_ALIGN		BIT(5)
 
 #define CCP_REVERSE_BUF_SIZE		64
 
@@ -142,7 +140,6 @@
 #define CCP_ECC_RESULT_OFFSET		60
 #define CCP_ECC_RESULT_SUCCESS		0x0001
 
-
 struct ccp_device;
 struct ccp_cmd;
 
@@ -261,7 +258,6 @@ struct ccp_device {
 	unsigned int axcache;
 };
 
-
 int ccp_pci_init(void);
 void ccp_pci_exit(void);
 
diff --git a/drivers/crypto/ccp/ccp-ops.c b/drivers/crypto/ccp/ccp-ops.c
index 8729364261d7..71f2e3c89424 100644
--- a/drivers/crypto/ccp/ccp-ops.c
+++ b/drivers/crypto/ccp/ccp-ops.c
@@ -27,7 +27,6 @@
 
 #include "ccp-dev.h"
 
-
 enum ccp_memtype {
 	CCP_MEMTYPE_SYSTEM = 0,
 	CCP_MEMTYPE_KSB,
@@ -515,7 +514,6 @@ static int ccp_init_sg_workarea(struct ccp_sg_workarea *wa, struct device *dev,
 	if (!wa->dma_count)
 		return -ENOMEM;
 
-
 	return 0;
 }
 
@@ -763,8 +761,9 @@ static void ccp_prepare_data(struct ccp_data *src, struct ccp_data *dst,
 		sg_dst_len = sg_dma_len(dst->sg_wa.sg) - dst->sg_wa.sg_used;
 		sg_dst_len = min_t(u64, src->sg_wa.bytes_left, sg_dst_len);
 		op_len = min(sg_src_len, sg_dst_len);
-	} else
+	} else {
 		op_len = sg_src_len;
+	}
 
 	/* The data operation length will be at least block_size in length
 	 * or the smaller of available sg room remaining for the source or
@@ -1131,9 +1130,9 @@ static int ccp_run_aes_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	if (ret)
 		goto e_ctx;
 
-	if (in_place)
+	if (in_place) {
 		dst = src;
-	else {
+	} else {
 		ret = ccp_init_data(&dst, cmd_q, aes->dst, aes->src_len,
 				    AES_BLOCK_SIZE, DMA_FROM_DEVICE);
 		if (ret)
@@ -1304,9 +1303,9 @@ static int ccp_run_xts_aes_cmd(struct ccp_cmd_queue *cmd_q,
 	if (ret)
 		goto e_ctx;
 
-	if (in_place)
+	if (in_place) {
 		dst = src;
-	else {
+	} else {
 		ret = ccp_init_data(&dst, cmd_q, xts->dst, xts->src_len,
 				    unit_size, DMA_FROM_DEVICE);
 		if (ret)
@@ -1451,8 +1450,9 @@ static int ccp_run_sha_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 			goto e_ctx;
 		}
 		memcpy(ctx.address, init, CCP_SHA_CTXSIZE);
-	} else
+	} else {
 		ccp_set_dm_area(&ctx, 0, sha->ctx, 0, sha->ctx_len);
+	}
 
 	ret = ccp_copy_to_ksb(cmd_q, &ctx, op.jobid, op.ksb_ctx,
 			      CCP_PASSTHRU_BYTESWAP_256BIT);
@@ -1732,9 +1732,9 @@ static int ccp_run_passthru_cmd(struct ccp_cmd_queue *cmd_q,
 	if (ret)
 		goto e_mask;
 
-	if (in_place)
+	if (in_place) {
 		dst = src;
-	else {
+	} else {
 		ret = ccp_init_data(&dst, cmd_q, pt->dst, pt->src_len,
 				    CCP_PASSTHRU_MASKSIZE, DMA_FROM_DEVICE);
 		if (ret)
@@ -1974,7 +1974,7 @@ static int ccp_run_ecc_pm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 	src.address += CCP_ECC_OPERAND_SIZE;
 
 	/* Set the first point Z coordianate to 1 */
-	*(src.address) = 0x01;
+	*src.address = 0x01;
 	src.address += CCP_ECC_OPERAND_SIZE;
 
 	if (ecc->function == CCP_ECC_FUNCTION_PADD_384BIT) {
@@ -1989,7 +1989,7 @@ static int ccp_run_ecc_pm_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 		src.address += CCP_ECC_OPERAND_SIZE;
 
 		/* Set the second point Z coordianate to 1 */
-		*(src.address) = 0x01;
+		*src.address = 0x01;
 		src.address += CCP_ECC_OPERAND_SIZE;
 	} else {
 		/* Copy the Domain "a" parameter */
diff --git a/drivers/crypto/ccp/ccp-pci.c b/drivers/crypto/ccp/ccp-pci.c
index 7f89c946adfe..af190d4795a8 100644
--- a/drivers/crypto/ccp/ccp-pci.c
+++ b/drivers/crypto/ccp/ccp-pci.c
@@ -174,11 +174,10 @@ static int ccp_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (!ccp)
 		goto e_err;
 
-	ccp_pci = kzalloc(sizeof(*ccp_pci), GFP_KERNEL);
-	if (!ccp_pci) {
-		ret = -ENOMEM;
-		goto e_free1;
-	}
+	ccp_pci = devm_kzalloc(dev, sizeof(*ccp_pci), GFP_KERNEL);
+	if (!ccp_pci)
+		goto e_err;
+
 	ccp->dev_specific = ccp_pci;
 	ccp->get_irq = ccp_get_irqs;
 	ccp->free_irq = ccp_free_irqs;
@@ -186,7 +185,7 @@ static int ccp_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	ret = pci_request_regions(pdev, "ccp");
 	if (ret) {
 		dev_err(dev, "pci_request_regions failed (%d)\n", ret);
-		goto e_free2;
+		goto e_err;
 	}
 
 	ret = pci_enable_device(pdev);
@@ -204,7 +203,7 @@ static int ccp_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	ret = -EIO;
 	ccp->io_map = pci_iomap(pdev, bar, 0);
-	if (ccp->io_map == NULL) {
+	if (!ccp->io_map) {
 		dev_err(dev, "pci_iomap failed\n");
 		goto e_device;
 	}
@@ -239,12 +238,6 @@ e_device:
 e_regions:
 	pci_release_regions(pdev);
 
-e_free2:
-	kfree(ccp_pci);
-
-e_free1:
-	kfree(ccp);
-
 e_err:
 	dev_notice(dev, "initialization failed\n");
 	return ret;
@@ -266,8 +259,6 @@ static void ccp_pci_remove(struct pci_dev *pdev)
 
 	pci_release_regions(pdev);
 
-	kfree(ccp);
-
 	dev_notice(dev, "disabled\n");
 }
 
diff --git a/drivers/crypto/ccp/ccp-platform.c b/drivers/crypto/ccp/ccp-platform.c
index 8c50bad25f7e..b1c20b2b5647 100644
--- a/drivers/crypto/ccp/ccp-platform.c
+++ b/drivers/crypto/ccp/ccp-platform.c
@@ -23,9 +23,15 @@
 #include <linux/delay.h>
 #include <linux/ccp.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/acpi.h>
 
 #include "ccp-dev.h"
 
+struct ccp_platform {
+	int use_acpi;
+	int coherent;
+};
 
 static int ccp_get_irq(struct ccp_device *ccp)
 {
@@ -84,10 +90,64 @@ static struct resource *ccp_find_mmio_area(struct ccp_device *ccp)
 	return NULL;
 }
 
+#ifdef CONFIG_ACPI
+static int ccp_acpi_support(struct ccp_device *ccp)
+{
+	struct ccp_platform *ccp_platform = ccp->dev_specific;
+	struct acpi_device *adev = ACPI_COMPANION(ccp->dev);
+	acpi_handle handle;
+	acpi_status status;
+	unsigned long long data;
+	int cca;
+
+	/* Retrieve the device cache coherency value */
+	handle = adev->handle;
+	do {
+		status = acpi_evaluate_integer(handle, "_CCA", NULL, &data);
+		if (!ACPI_FAILURE(status)) {
+			cca = data;
+			break;
+		}
+	} while (!ACPI_FAILURE(status));
+
+	if (ACPI_FAILURE(status)) {
+		dev_err(ccp->dev, "error obtaining acpi coherency value\n");
+		return -EINVAL;
+	}
+
+	ccp_platform->coherent = !!cca;
+
+	return 0;
+}
+#else	/* CONFIG_ACPI */
+static int ccp_acpi_support(struct ccp_device *ccp)
+{
+	return -EINVAL;
+}
+#endif
+
+#ifdef CONFIG_OF
+static int ccp_of_support(struct ccp_device *ccp)
+{
+	struct ccp_platform *ccp_platform = ccp->dev_specific;
+
+	ccp_platform->coherent = of_dma_is_coherent(ccp->dev->of_node);
+
+	return 0;
+}
+#else
+static int ccp_of_support(struct ccp_device *ccp)
+{
+	return -EINVAL;
+}
+#endif
+
 static int ccp_platform_probe(struct platform_device *pdev)
 {
 	struct ccp_device *ccp;
+	struct ccp_platform *ccp_platform;
 	struct device *dev = &pdev->dev;
+	struct acpi_device *adev = ACPI_COMPANION(dev);
 	struct resource *ior;
 	int ret;
 
@@ -96,24 +156,40 @@ static int ccp_platform_probe(struct platform_device *pdev)
 	if (!ccp)
 		goto e_err;
 
-	ccp->dev_specific = NULL;
+	ccp_platform = devm_kzalloc(dev, sizeof(*ccp_platform), GFP_KERNEL);
+	if (!ccp_platform)
+		goto e_err;
+
+	ccp->dev_specific = ccp_platform;
 	ccp->get_irq = ccp_get_irqs;
 	ccp->free_irq = ccp_free_irqs;
 
+	ccp_platform->use_acpi = (!adev || acpi_disabled) ? 0 : 1;
+
 	ior = ccp_find_mmio_area(ccp);
 	ccp->io_map = devm_ioremap_resource(dev, ior);
 	if (IS_ERR(ccp->io_map)) {
 		ret = PTR_ERR(ccp->io_map);
-		goto e_free;
+		goto e_err;
 	}
 	ccp->io_regs = ccp->io_map;
 
 	if (!dev->dma_mask)
 		dev->dma_mask = &dev->coherent_dma_mask;
-	*(dev->dma_mask) = DMA_BIT_MASK(48);
-	dev->coherent_dma_mask = DMA_BIT_MASK(48);
+	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(48));
+	if (ret) {
+		dev_err(dev, "dma_set_mask_and_coherent failed (%d)\n", ret);
+		goto e_err;
+	}
+
+	if (ccp_platform->use_acpi)
+		ret = ccp_acpi_support(ccp);
+	else
+		ret = ccp_of_support(ccp);
+	if (ret)
+		goto e_err;
 
-	if (of_property_read_bool(dev->of_node, "dma-coherent"))
+	if (ccp_platform->coherent)
 		ccp->axcache = CACHE_WB_NO_ALLOC;
 	else
 		ccp->axcache = CACHE_NONE;
@@ -122,15 +198,12 @@ static int ccp_platform_probe(struct platform_device *pdev)
 
 	ret = ccp_init(ccp);
 	if (ret)
-		goto e_free;
+		goto e_err;
 
 	dev_notice(dev, "enabled\n");
 
 	return 0;
 
-e_free:
-	kfree(ccp);
-
 e_err:
 	dev_notice(dev, "initialization failed\n");
 	return ret;
@@ -143,8 +216,6 @@ static int ccp_platform_remove(struct platform_device *pdev)
 
 	ccp_destroy(ccp);
 
-	kfree(ccp);
-
 	dev_notice(dev, "disabled\n");
 
 	return 0;
@@ -200,15 +271,29 @@ static int ccp_platform_resume(struct platform_device *pdev)
 }
 #endif
 
-static const struct of_device_id ccp_platform_ids[] = {
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ccp_acpi_match[] = {
+	{ "AMDI0C00", 0 },
+	{ },
+};
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id ccp_of_match[] = {
 	{ .compatible = "amd,ccp-seattle-v1a" },
 	{ },
 };
+#endif
 
 static struct platform_driver ccp_platform_driver = {
 	.driver = {
 		.name = "AMD Cryptographic Coprocessor",
-		.of_match_table = ccp_platform_ids,
+#ifdef CONFIG_ACPI
+		.acpi_match_table = ccp_acpi_match,
+#endif
+#ifdef CONFIG_OF
+		.of_match_table = ccp_of_match,
+#endif
 	},
 	.probe = ccp_platform_probe,
 	.remove = ccp_platform_remove,
diff --git a/drivers/crypto/img-hash.c b/drivers/crypto/img-hash.c
new file mode 100644
index 000000000000..ad47d0d61098
--- /dev/null
+++ b/drivers/crypto/img-hash.c
@@ -0,0 +1,1029 @@
+/*
+ * Copyright (c) 2014 Imagination Technologies
+ * Authors:  Will Thomas, James Hartley
+ *
+ * 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.
+ *
+ *	Interface structure taken from omap-sham driver
+ */
+
+#include <linux/clk.h>
+#include <linux/dmaengine.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+
+#include <crypto/internal/hash.h>
+#include <crypto/md5.h>
+#include <crypto/sha.h>
+
+#define CR_RESET			0
+#define CR_RESET_SET			1
+#define CR_RESET_UNSET			0
+
+#define CR_MESSAGE_LENGTH_H		0x4
+#define CR_MESSAGE_LENGTH_L		0x8
+
+#define CR_CONTROL			0xc
+#define CR_CONTROL_BYTE_ORDER_3210	0
+#define CR_CONTROL_BYTE_ORDER_0123	1
+#define CR_CONTROL_BYTE_ORDER_2310	2
+#define CR_CONTROL_BYTE_ORDER_1032	3
+#define CR_CONTROL_BYTE_ORDER_SHIFT	8
+#define CR_CONTROL_ALGO_MD5	0
+#define CR_CONTROL_ALGO_SHA1	1
+#define CR_CONTROL_ALGO_SHA224	2
+#define CR_CONTROL_ALGO_SHA256	3
+
+#define CR_INTSTAT			0x10
+#define CR_INTENAB			0x14
+#define CR_INTCLEAR			0x18
+#define CR_INT_RESULTS_AVAILABLE	BIT(0)
+#define CR_INT_NEW_RESULTS_SET		BIT(1)
+#define CR_INT_RESULT_READ_ERR		BIT(2)
+#define CR_INT_MESSAGE_WRITE_ERROR	BIT(3)
+#define CR_INT_STATUS			BIT(8)
+
+#define CR_RESULT_QUEUE		0x1c
+#define CR_RSD0				0x40
+#define CR_CORE_REV			0x50
+#define CR_CORE_DES1		0x60
+#define CR_CORE_DES2		0x70
+
+#define DRIVER_FLAGS_BUSY		BIT(0)
+#define DRIVER_FLAGS_FINAL		BIT(1)
+#define DRIVER_FLAGS_DMA_ACTIVE		BIT(2)
+#define DRIVER_FLAGS_OUTPUT_READY	BIT(3)
+#define DRIVER_FLAGS_INIT		BIT(4)
+#define DRIVER_FLAGS_CPU		BIT(5)
+#define DRIVER_FLAGS_DMA_READY		BIT(6)
+#define DRIVER_FLAGS_ERROR		BIT(7)
+#define DRIVER_FLAGS_SG			BIT(8)
+#define DRIVER_FLAGS_SHA1		BIT(18)
+#define DRIVER_FLAGS_SHA224		BIT(19)
+#define DRIVER_FLAGS_SHA256		BIT(20)
+#define DRIVER_FLAGS_MD5		BIT(21)
+
+#define IMG_HASH_QUEUE_LENGTH		20
+#define IMG_HASH_DMA_THRESHOLD		64
+
+#ifdef __LITTLE_ENDIAN
+#define IMG_HASH_BYTE_ORDER		CR_CONTROL_BYTE_ORDER_3210
+#else
+#define IMG_HASH_BYTE_ORDER		CR_CONTROL_BYTE_ORDER_0123
+#endif
+
+struct img_hash_dev;
+
+struct img_hash_request_ctx {
+	struct img_hash_dev	*hdev;
+	u8 digest[SHA256_DIGEST_SIZE] __aligned(sizeof(u32));
+	unsigned long		flags;
+	size_t			digsize;
+
+	dma_addr_t		dma_addr;
+	size_t			dma_ct;
+
+	/* sg root */
+	struct scatterlist	*sgfirst;
+	/* walk state */
+	struct scatterlist	*sg;
+	size_t			nents;
+	size_t			offset;
+	unsigned int		total;
+	size_t			sent;
+
+	unsigned long		op;
+
+	size_t			bufcnt;
+	u8 buffer[0] __aligned(sizeof(u32));
+	struct ahash_request	fallback_req;
+};
+
+struct img_hash_ctx {
+	struct img_hash_dev	*hdev;
+	unsigned long		flags;
+	struct crypto_ahash	*fallback;
+};
+
+struct img_hash_dev {
+	struct list_head	list;
+	struct device		*dev;
+	struct clk		*hash_clk;
+	struct clk		*sys_clk;
+	void __iomem		*io_base;
+
+	phys_addr_t		bus_addr;
+	void __iomem		*cpu_addr;
+
+	spinlock_t		lock;
+	int			err;
+	struct tasklet_struct	done_task;
+	struct tasklet_struct	dma_task;
+
+	unsigned long		flags;
+	struct crypto_queue	queue;
+	struct ahash_request	*req;
+
+	struct dma_chan		*dma_lch;
+};
+
+struct img_hash_drv {
+	struct list_head dev_list;
+	spinlock_t lock;
+};
+
+static struct img_hash_drv img_hash = {
+	.dev_list = LIST_HEAD_INIT(img_hash.dev_list),
+	.lock = __SPIN_LOCK_UNLOCKED(img_hash.lock),
+};
+
+static inline u32 img_hash_read(struct img_hash_dev *hdev, u32 offset)
+{
+	return readl_relaxed(hdev->io_base + offset);
+}
+
+static inline void img_hash_write(struct img_hash_dev *hdev,
+				  u32 offset, u32 value)
+{
+	writel_relaxed(value, hdev->io_base + offset);
+}
+
+static inline u32 img_hash_read_result_queue(struct img_hash_dev *hdev)
+{
+	return be32_to_cpu(img_hash_read(hdev, CR_RESULT_QUEUE));
+}
+
+static void img_hash_start(struct img_hash_dev *hdev, bool dma)
+{
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(hdev->req);
+	u32 cr = IMG_HASH_BYTE_ORDER << CR_CONTROL_BYTE_ORDER_SHIFT;
+
+	if (ctx->flags & DRIVER_FLAGS_MD5)
+		cr |= CR_CONTROL_ALGO_MD5;
+	else if (ctx->flags & DRIVER_FLAGS_SHA1)
+		cr |= CR_CONTROL_ALGO_SHA1;
+	else if (ctx->flags & DRIVER_FLAGS_SHA224)
+		cr |= CR_CONTROL_ALGO_SHA224;
+	else if (ctx->flags & DRIVER_FLAGS_SHA256)
+		cr |= CR_CONTROL_ALGO_SHA256;
+	dev_dbg(hdev->dev, "Starting hash process\n");
+	img_hash_write(hdev, CR_CONTROL, cr);
+
+	/*
+	 * The hardware block requires two cycles between writing the control
+	 * register and writing the first word of data in non DMA mode, to
+	 * ensure the first data write is not grouped in burst with the control
+	 * register write a read is issued to 'flush' the bus.
+	 */
+	if (!dma)
+		img_hash_read(hdev, CR_CONTROL);
+}
+
+static int img_hash_xmit_cpu(struct img_hash_dev *hdev, const u8 *buf,
+			     size_t length, int final)
+{
+	u32 count, len32;
+	const u32 *buffer = (const u32 *)buf;
+
+	dev_dbg(hdev->dev, "xmit_cpu:  length: %zu bytes\n", length);
+
+	if (final)
+		hdev->flags |= DRIVER_FLAGS_FINAL;
+
+	len32 = DIV_ROUND_UP(length, sizeof(u32));
+
+	for (count = 0; count < len32; count++)
+		writel_relaxed(buffer[count], hdev->cpu_addr);
+
+	return -EINPROGRESS;
+}
+
+static void img_hash_dma_callback(void *data)
+{
+	struct img_hash_dev *hdev = (struct img_hash_dev *)data;
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(hdev->req);
+
+	if (ctx->bufcnt) {
+		img_hash_xmit_cpu(hdev, ctx->buffer, ctx->bufcnt, 0);
+		ctx->bufcnt = 0;
+	}
+	if (ctx->sg)
+		tasklet_schedule(&hdev->dma_task);
+}
+
+static int img_hash_xmit_dma(struct img_hash_dev *hdev, struct scatterlist *sg)
+{
+	struct dma_async_tx_descriptor *desc;
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(hdev->req);
+
+	ctx->dma_ct = dma_map_sg(hdev->dev, sg, 1, DMA_MEM_TO_DEV);
+	if (ctx->dma_ct == 0) {
+		dev_err(hdev->dev, "Invalid DMA sg\n");
+		hdev->err = -EINVAL;
+		return -EINVAL;
+	}
+
+	desc = dmaengine_prep_slave_sg(hdev->dma_lch,
+				       sg,
+				       ctx->dma_ct,
+				       DMA_MEM_TO_DEV,
+				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc) {
+		dev_err(hdev->dev, "Null DMA descriptor\n");
+		hdev->err = -EINVAL;
+		dma_unmap_sg(hdev->dev, sg, 1, DMA_MEM_TO_DEV);
+		return -EINVAL;
+	}
+	desc->callback = img_hash_dma_callback;
+	desc->callback_param = hdev;
+	dmaengine_submit(desc);
+	dma_async_issue_pending(hdev->dma_lch);
+
+	return 0;
+}
+
+static int img_hash_write_via_cpu(struct img_hash_dev *hdev)
+{
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(hdev->req);
+
+	ctx->bufcnt = sg_copy_to_buffer(hdev->req->src, sg_nents(ctx->sg),
+					ctx->buffer, hdev->req->nbytes);
+
+	ctx->total = hdev->req->nbytes;
+	ctx->bufcnt = 0;
+
+	hdev->flags |= (DRIVER_FLAGS_CPU | DRIVER_FLAGS_FINAL);
+
+	img_hash_start(hdev, false);
+
+	return img_hash_xmit_cpu(hdev, ctx->buffer, ctx->total, 1);
+}
+
+static int img_hash_finish(struct ahash_request *req)
+{
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(req);
+
+	if (!req->result)
+		return -EINVAL;
+
+	memcpy(req->result, ctx->digest, ctx->digsize);
+
+	return 0;
+}
+
+static void img_hash_copy_hash(struct ahash_request *req)
+{
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(req);
+	u32 *hash = (u32 *)ctx->digest;
+	int i;
+
+	for (i = (ctx->digsize / sizeof(u32)) - 1; i >= 0; i--)
+		hash[i] = img_hash_read_result_queue(ctx->hdev);
+}
+
+static void img_hash_finish_req(struct ahash_request *req, int err)
+{
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(req);
+	struct img_hash_dev *hdev =  ctx->hdev;
+
+	if (!err) {
+		img_hash_copy_hash(req);
+		if (DRIVER_FLAGS_FINAL & hdev->flags)
+			err = img_hash_finish(req);
+	} else {
+		dev_warn(hdev->dev, "Hash failed with error %d\n", err);
+		ctx->flags |= DRIVER_FLAGS_ERROR;
+	}
+
+	hdev->flags &= ~(DRIVER_FLAGS_DMA_READY | DRIVER_FLAGS_OUTPUT_READY |
+		DRIVER_FLAGS_CPU | DRIVER_FLAGS_BUSY | DRIVER_FLAGS_FINAL);
+
+	if (req->base.complete)
+		req->base.complete(&req->base, err);
+}
+
+static int img_hash_write_via_dma(struct img_hash_dev *hdev)
+{
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(hdev->req);
+
+	img_hash_start(hdev, true);
+
+	dev_dbg(hdev->dev, "xmit dma size: %d\n", ctx->total);
+
+	if (!ctx->total)
+		hdev->flags |= DRIVER_FLAGS_FINAL;
+
+	hdev->flags |= DRIVER_FLAGS_DMA_ACTIVE | DRIVER_FLAGS_FINAL;
+
+	tasklet_schedule(&hdev->dma_task);
+
+	return -EINPROGRESS;
+}
+
+static int img_hash_dma_init(struct img_hash_dev *hdev)
+{
+	struct dma_slave_config dma_conf;
+	int err = -EINVAL;
+
+	hdev->dma_lch = dma_request_slave_channel(hdev->dev, "tx");
+	if (!hdev->dma_lch) {
+		dev_err(hdev->dev, "Couldn't aquire a slave DMA channel.\n");
+		return -EBUSY;
+	}
+	dma_conf.direction = DMA_MEM_TO_DEV;
+	dma_conf.dst_addr = hdev->bus_addr;
+	dma_conf.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	dma_conf.dst_maxburst = 16;
+	dma_conf.device_fc = false;
+
+	err = dmaengine_slave_config(hdev->dma_lch,  &dma_conf);
+	if (err) {
+		dev_err(hdev->dev, "Couldn't configure DMA slave.\n");
+		dma_release_channel(hdev->dma_lch);
+		return err;
+	}
+
+	return 0;
+}
+
+static void img_hash_dma_task(unsigned long d)
+{
+	struct img_hash_dev *hdev = (struct img_hash_dev *)d;
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(hdev->req);
+	u8 *addr;
+	size_t nbytes, bleft, wsend, len, tbc;
+	struct scatterlist tsg;
+
+	if (!ctx->sg)
+		return;
+
+	addr = sg_virt(ctx->sg);
+	nbytes = ctx->sg->length - ctx->offset;
+
+	/*
+	 * The hash accelerator does not support a data valid mask. This means
+	 * that if each dma (i.e. per page) is not a multiple of 4 bytes, the
+	 * padding bytes in the last word written by that dma would erroneously
+	 * be included in the hash. To avoid this we round down the transfer,
+	 * and add the excess to the start of the next dma. It does not matter
+	 * that the final dma may not be a multiple of 4 bytes as the hashing
+	 * block is programmed to accept the correct number of bytes.
+	 */
+
+	bleft = nbytes % 4;
+	wsend = (nbytes / 4);
+
+	if (wsend) {
+		sg_init_one(&tsg, addr + ctx->offset, wsend * 4);
+		if (img_hash_xmit_dma(hdev, &tsg)) {
+			dev_err(hdev->dev, "DMA failed, falling back to CPU");
+			ctx->flags |= DRIVER_FLAGS_CPU;
+			hdev->err = 0;
+			img_hash_xmit_cpu(hdev, addr + ctx->offset,
+					  wsend * 4, 0);
+			ctx->sent += wsend * 4;
+			wsend = 0;
+		} else {
+			ctx->sent += wsend * 4;
+		}
+	}
+
+	if (bleft) {
+		ctx->bufcnt = sg_pcopy_to_buffer(ctx->sgfirst, ctx->nents,
+						 ctx->buffer, bleft, ctx->sent);
+		tbc = 0;
+		ctx->sg = sg_next(ctx->sg);
+		while (ctx->sg && (ctx->bufcnt < 4)) {
+			len = ctx->sg->length;
+			if (likely(len > (4 - ctx->bufcnt)))
+				len = 4 - ctx->bufcnt;
+			tbc = sg_pcopy_to_buffer(ctx->sgfirst, ctx->nents,
+						 ctx->buffer + ctx->bufcnt, len,
+					ctx->sent + ctx->bufcnt);
+			ctx->bufcnt += tbc;
+			if (tbc >= ctx->sg->length) {
+				ctx->sg = sg_next(ctx->sg);
+				tbc = 0;
+			}
+		}
+
+		ctx->sent += ctx->bufcnt;
+		ctx->offset = tbc;
+
+		if (!wsend)
+			img_hash_dma_callback(hdev);
+	} else {
+		ctx->offset = 0;
+		ctx->sg = sg_next(ctx->sg);
+	}
+}
+
+static int img_hash_write_via_dma_stop(struct img_hash_dev *hdev)
+{
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(hdev->req);
+
+	if (ctx->flags & DRIVER_FLAGS_SG)
+		dma_unmap_sg(hdev->dev, ctx->sg, ctx->dma_ct, DMA_TO_DEVICE);
+
+	return 0;
+}
+
+static int img_hash_process_data(struct img_hash_dev *hdev)
+{
+	struct ahash_request *req = hdev->req;
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(req);
+	int err = 0;
+
+	ctx->bufcnt = 0;
+
+	if (req->nbytes >= IMG_HASH_DMA_THRESHOLD) {
+		dev_dbg(hdev->dev, "process data request(%d bytes) using DMA\n",
+			req->nbytes);
+		err = img_hash_write_via_dma(hdev);
+	} else {
+		dev_dbg(hdev->dev, "process data request(%d bytes) using CPU\n",
+			req->nbytes);
+		err = img_hash_write_via_cpu(hdev);
+	}
+	return err;
+}
+
+static int img_hash_hw_init(struct img_hash_dev *hdev)
+{
+	unsigned long long nbits;
+	u32 u, l;
+
+	img_hash_write(hdev, CR_RESET, CR_RESET_SET);
+	img_hash_write(hdev, CR_RESET, CR_RESET_UNSET);
+	img_hash_write(hdev, CR_INTENAB, CR_INT_NEW_RESULTS_SET);
+
+	nbits = (u64)hdev->req->nbytes << 3;
+	u = nbits >> 32;
+	l = nbits;
+	img_hash_write(hdev, CR_MESSAGE_LENGTH_H, u);
+	img_hash_write(hdev, CR_MESSAGE_LENGTH_L, l);
+
+	if (!(DRIVER_FLAGS_INIT & hdev->flags)) {
+		hdev->flags |= DRIVER_FLAGS_INIT;
+		hdev->err = 0;
+	}
+	dev_dbg(hdev->dev, "hw initialized, nbits: %llx\n", nbits);
+	return 0;
+}
+
+static int img_hash_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct img_hash_request_ctx *rctx = ahash_request_ctx(req);
+	struct img_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback);
+	rctx->fallback_req.base.flags =	req->base.flags
+		& CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	return crypto_ahash_init(&rctx->fallback_req);
+}
+
+static int img_hash_handle_queue(struct img_hash_dev *hdev,
+				 struct ahash_request *req)
+{
+	struct crypto_async_request *async_req, *backlog;
+	struct img_hash_request_ctx *ctx;
+	unsigned long flags;
+	int err = 0, res = 0;
+
+	spin_lock_irqsave(&hdev->lock, flags);
+
+	if (req)
+		res = ahash_enqueue_request(&hdev->queue, req);
+
+	if (DRIVER_FLAGS_BUSY & hdev->flags) {
+		spin_unlock_irqrestore(&hdev->lock, flags);
+		return res;
+	}
+
+	backlog = crypto_get_backlog(&hdev->queue);
+	async_req = crypto_dequeue_request(&hdev->queue);
+	if (async_req)
+		hdev->flags |= DRIVER_FLAGS_BUSY;
+
+	spin_unlock_irqrestore(&hdev->lock, flags);
+
+	if (!async_req)
+		return res;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	req = ahash_request_cast(async_req);
+	hdev->req = req;
+
+	ctx = ahash_request_ctx(req);
+
+	dev_info(hdev->dev, "processing req, op: %lu, bytes: %d\n",
+		 ctx->op, req->nbytes);
+
+	err = img_hash_hw_init(hdev);
+
+	if (!err)
+		err = img_hash_process_data(hdev);
+
+	if (err != -EINPROGRESS) {
+		/* done_task will not finish so do it here */
+		img_hash_finish_req(req, err);
+	}
+	return res;
+}
+
+static int img_hash_update(struct ahash_request *req)
+{
+	struct img_hash_request_ctx *rctx = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct img_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback);
+	rctx->fallback_req.base.flags = req->base.flags
+		& CRYPTO_TFM_REQ_MAY_SLEEP;
+	rctx->fallback_req.nbytes = req->nbytes;
+	rctx->fallback_req.src = req->src;
+
+	return crypto_ahash_update(&rctx->fallback_req);
+}
+
+static int img_hash_final(struct ahash_request *req)
+{
+	struct img_hash_request_ctx *rctx = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct img_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback);
+	rctx->fallback_req.base.flags = req->base.flags
+		& CRYPTO_TFM_REQ_MAY_SLEEP;
+	rctx->fallback_req.result = req->result;
+
+	return crypto_ahash_final(&rctx->fallback_req);
+}
+
+static int img_hash_finup(struct ahash_request *req)
+{
+	struct img_hash_request_ctx *rctx = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct img_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback);
+	rctx->fallback_req.base.flags = req->base.flags
+		& CRYPTO_TFM_REQ_MAY_SLEEP;
+	rctx->fallback_req.nbytes = req->nbytes;
+	rctx->fallback_req.src = req->src;
+	rctx->fallback_req.result = req->result;
+
+	return crypto_ahash_finup(&rctx->fallback_req);
+}
+
+static int img_hash_digest(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct img_hash_ctx *tctx = crypto_ahash_ctx(tfm);
+	struct img_hash_request_ctx *ctx = ahash_request_ctx(req);
+	struct img_hash_dev *hdev = NULL;
+	struct img_hash_dev *tmp;
+	int err;
+
+	spin_lock(&img_hash.lock);
+	if (!tctx->hdev) {
+		list_for_each_entry(tmp, &img_hash.dev_list, list) {
+			hdev = tmp;
+			break;
+		}
+		tctx->hdev = hdev;
+
+	} else {
+		hdev = tctx->hdev;
+	}
+
+	spin_unlock(&img_hash.lock);
+	ctx->hdev = hdev;
+	ctx->flags = 0;
+	ctx->digsize = crypto_ahash_digestsize(tfm);
+
+	switch (ctx->digsize) {
+	case SHA1_DIGEST_SIZE:
+		ctx->flags |= DRIVER_FLAGS_SHA1;
+		break;
+	case SHA256_DIGEST_SIZE:
+		ctx->flags |= DRIVER_FLAGS_SHA256;
+		break;
+	case SHA224_DIGEST_SIZE:
+		ctx->flags |= DRIVER_FLAGS_SHA224;
+		break;
+	case MD5_DIGEST_SIZE:
+		ctx->flags |= DRIVER_FLAGS_MD5;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ctx->bufcnt = 0;
+	ctx->offset = 0;
+	ctx->sent = 0;
+	ctx->total = req->nbytes;
+	ctx->sg = req->src;
+	ctx->sgfirst = req->src;
+	ctx->nents = sg_nents(ctx->sg);
+
+	err = img_hash_handle_queue(tctx->hdev, req);
+
+	return err;
+}
+
+static int img_hash_cra_init(struct crypto_tfm *tfm)
+{
+	struct img_hash_ctx *ctx = crypto_tfm_ctx(tfm);
+	const char *alg_name = crypto_tfm_alg_name(tfm);
+	int err = -ENOMEM;
+
+	ctx->fallback = crypto_alloc_ahash(alg_name, 0,
+					   CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(ctx->fallback)) {
+		pr_err("img_hash: Could not load fallback driver.\n");
+		err = PTR_ERR(ctx->fallback);
+		goto err;
+	}
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct img_hash_request_ctx) +
+				 IMG_HASH_DMA_THRESHOLD);
+
+	return 0;
+
+err:
+	return err;
+}
+
+static void img_hash_cra_exit(struct crypto_tfm *tfm)
+{
+	struct img_hash_ctx *tctx = crypto_tfm_ctx(tfm);
+
+	crypto_free_ahash(tctx->fallback);
+}
+
+static irqreturn_t img_irq_handler(int irq, void *dev_id)
+{
+	struct img_hash_dev *hdev = dev_id;
+	u32 reg;
+
+	reg = img_hash_read(hdev, CR_INTSTAT);
+	img_hash_write(hdev, CR_INTCLEAR, reg);
+
+	if (reg & CR_INT_NEW_RESULTS_SET) {
+		dev_dbg(hdev->dev, "IRQ CR_INT_NEW_RESULTS_SET\n");
+		if (DRIVER_FLAGS_BUSY & hdev->flags) {
+			hdev->flags |= DRIVER_FLAGS_OUTPUT_READY;
+			if (!(DRIVER_FLAGS_CPU & hdev->flags))
+				hdev->flags |= DRIVER_FLAGS_DMA_READY;
+			tasklet_schedule(&hdev->done_task);
+		} else {
+			dev_warn(hdev->dev,
+				 "HASH interrupt when no active requests.\n");
+		}
+	} else if (reg & CR_INT_RESULTS_AVAILABLE) {
+		dev_warn(hdev->dev,
+			 "IRQ triggered before the hash had completed\n");
+	} else if (reg & CR_INT_RESULT_READ_ERR) {
+		dev_warn(hdev->dev,
+			 "Attempt to read from an empty result queue\n");
+	} else if (reg & CR_INT_MESSAGE_WRITE_ERROR) {
+		dev_warn(hdev->dev,
+			 "Data written before the hardware was configured\n");
+	}
+	return IRQ_HANDLED;
+}
+
+static struct ahash_alg img_algs[] = {
+	{
+		.init = img_hash_init,
+		.update = img_hash_update,
+		.final = img_hash_final,
+		.finup = img_hash_finup,
+		.digest = img_hash_digest,
+		.halg = {
+			.digestsize = MD5_DIGEST_SIZE,
+			.base = {
+				.cra_name = "md5",
+				.cra_driver_name = "img-md5",
+				.cra_priority = 300,
+				.cra_flags =
+				CRYPTO_ALG_ASYNC |
+				CRYPTO_ALG_NEED_FALLBACK,
+				.cra_blocksize = MD5_HMAC_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct img_hash_ctx),
+				.cra_init = img_hash_cra_init,
+				.cra_exit = img_hash_cra_exit,
+				.cra_module = THIS_MODULE,
+			}
+		}
+	},
+	{
+		.init = img_hash_init,
+		.update = img_hash_update,
+		.final = img_hash_final,
+		.finup = img_hash_finup,
+		.digest = img_hash_digest,
+		.halg = {
+			.digestsize = SHA1_DIGEST_SIZE,
+			.base = {
+				.cra_name = "sha1",
+				.cra_driver_name = "img-sha1",
+				.cra_priority = 300,
+				.cra_flags =
+				CRYPTO_ALG_ASYNC |
+				CRYPTO_ALG_NEED_FALLBACK,
+				.cra_blocksize = SHA1_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct img_hash_ctx),
+				.cra_init = img_hash_cra_init,
+				.cra_exit = img_hash_cra_exit,
+				.cra_module = THIS_MODULE,
+			}
+		}
+	},
+	{
+		.init = img_hash_init,
+		.update = img_hash_update,
+		.final = img_hash_final,
+		.finup = img_hash_finup,
+		.digest = img_hash_digest,
+		.halg = {
+			.digestsize = SHA224_DIGEST_SIZE,
+			.base = {
+				.cra_name = "sha224",
+				.cra_driver_name = "img-sha224",
+				.cra_priority = 300,
+				.cra_flags =
+				CRYPTO_ALG_ASYNC |
+				CRYPTO_ALG_NEED_FALLBACK,
+				.cra_blocksize = SHA224_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct img_hash_ctx),
+				.cra_init = img_hash_cra_init,
+				.cra_exit = img_hash_cra_exit,
+				.cra_module = THIS_MODULE,
+			}
+		}
+	},
+	{
+		.init = img_hash_init,
+		.update = img_hash_update,
+		.final = img_hash_final,
+		.finup = img_hash_finup,
+		.digest = img_hash_digest,
+		.halg = {
+			.digestsize = SHA256_DIGEST_SIZE,
+			.base = {
+				.cra_name = "sha256",
+				.cra_driver_name = "img-sha256",
+				.cra_priority = 300,
+				.cra_flags =
+				CRYPTO_ALG_ASYNC |
+				CRYPTO_ALG_NEED_FALLBACK,
+				.cra_blocksize = SHA256_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct img_hash_ctx),
+				.cra_init = img_hash_cra_init,
+				.cra_exit = img_hash_cra_exit,
+				.cra_module = THIS_MODULE,
+			}
+		}
+	}
+};
+
+static int img_register_algs(struct img_hash_dev *hdev)
+{
+	int i, err;
+
+	for (i = 0; i < ARRAY_SIZE(img_algs); i++) {
+		err = crypto_register_ahash(&img_algs[i]);
+		if (err)
+			goto err_reg;
+	}
+	return 0;
+
+err_reg:
+	for (; i--; )
+		crypto_unregister_ahash(&img_algs[i]);
+
+	return err;
+}
+
+static int img_unregister_algs(struct img_hash_dev *hdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(img_algs); i++)
+		crypto_unregister_ahash(&img_algs[i]);
+	return 0;
+}
+
+static void img_hash_done_task(unsigned long data)
+{
+	struct img_hash_dev *hdev = (struct img_hash_dev *)data;
+	int err = 0;
+
+	if (hdev->err == -EINVAL) {
+		err = hdev->err;
+		goto finish;
+	}
+
+	if (!(DRIVER_FLAGS_BUSY & hdev->flags)) {
+		img_hash_handle_queue(hdev, NULL);
+		return;
+	}
+
+	if (DRIVER_FLAGS_CPU & hdev->flags) {
+		if (DRIVER_FLAGS_OUTPUT_READY & hdev->flags) {
+			hdev->flags &= ~DRIVER_FLAGS_OUTPUT_READY;
+			goto finish;
+		}
+	} else if (DRIVER_FLAGS_DMA_READY & hdev->flags) {
+		if (DRIVER_FLAGS_DMA_ACTIVE & hdev->flags) {
+			hdev->flags &= ~DRIVER_FLAGS_DMA_ACTIVE;
+			img_hash_write_via_dma_stop(hdev);
+			if (hdev->err) {
+				err = hdev->err;
+				goto finish;
+			}
+		}
+		if (DRIVER_FLAGS_OUTPUT_READY & hdev->flags) {
+			hdev->flags &= ~(DRIVER_FLAGS_DMA_READY |
+					DRIVER_FLAGS_OUTPUT_READY);
+			goto finish;
+		}
+	}
+	return;
+
+finish:
+	img_hash_finish_req(hdev->req, err);
+}
+
+static const struct of_device_id img_hash_match[] = {
+	{ .compatible = "img,hash-accelerator" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, img_hash_match);
+
+static int img_hash_probe(struct platform_device *pdev)
+{
+	struct img_hash_dev *hdev;
+	struct device *dev = &pdev->dev;
+	struct resource *hash_res;
+	int	irq;
+	int err;
+
+	hdev = devm_kzalloc(dev, sizeof(*hdev), GFP_KERNEL);
+	if (hdev == NULL)
+		return -ENOMEM;
+
+	spin_lock_init(&hdev->lock);
+
+	hdev->dev = dev;
+
+	platform_set_drvdata(pdev, hdev);
+
+	INIT_LIST_HEAD(&hdev->list);
+
+	tasklet_init(&hdev->done_task, img_hash_done_task, (unsigned long)hdev);
+	tasklet_init(&hdev->dma_task, img_hash_dma_task, (unsigned long)hdev);
+
+	crypto_init_queue(&hdev->queue, IMG_HASH_QUEUE_LENGTH);
+
+	/* Register bank */
+	hash_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	hdev->io_base = devm_ioremap_resource(dev, hash_res);
+	if (IS_ERR(hdev->io_base)) {
+		err = PTR_ERR(hdev->io_base);
+		dev_err(dev, "can't ioremap, returned %d\n", err);
+
+		goto res_err;
+	}
+
+	/* Write port (DMA or CPU) */
+	hash_res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	hdev->cpu_addr = devm_ioremap_resource(dev, hash_res);
+	if (IS_ERR(hdev->cpu_addr)) {
+		dev_err(dev, "can't ioremap write port\n");
+		err = PTR_ERR(hdev->cpu_addr);
+		goto res_err;
+	}
+	hdev->bus_addr = hash_res->start;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "no IRQ resource info\n");
+		err = irq;
+		goto res_err;
+	}
+
+	err = devm_request_irq(dev, irq, img_irq_handler, 0,
+			       dev_name(dev), hdev);
+	if (err) {
+		dev_err(dev, "unable to request irq\n");
+		goto res_err;
+	}
+	dev_dbg(dev, "using IRQ channel %d\n", irq);
+
+	hdev->hash_clk = devm_clk_get(&pdev->dev, "hash");
+	if (IS_ERR(hdev->hash_clk)) {
+		dev_err(dev, "clock initialization failed.\n");
+		err = PTR_ERR(hdev->hash_clk);
+		goto res_err;
+	}
+
+	hdev->sys_clk = devm_clk_get(&pdev->dev, "sys");
+	if (IS_ERR(hdev->sys_clk)) {
+		dev_err(dev, "clock initialization failed.\n");
+		err = PTR_ERR(hdev->sys_clk);
+		goto res_err;
+	}
+
+	err = clk_prepare_enable(hdev->hash_clk);
+	if (err)
+		goto res_err;
+
+	err = clk_prepare_enable(hdev->sys_clk);
+	if (err)
+		goto clk_err;
+
+	err = img_hash_dma_init(hdev);
+	if (err)
+		goto dma_err;
+
+	dev_dbg(dev, "using %s for DMA transfers\n",
+		dma_chan_name(hdev->dma_lch));
+
+	spin_lock(&img_hash.lock);
+	list_add_tail(&hdev->list, &img_hash.dev_list);
+	spin_unlock(&img_hash.lock);
+
+	err = img_register_algs(hdev);
+	if (err)
+		goto err_algs;
+	dev_dbg(dev, "Img MD5/SHA1/SHA224/SHA256 Hardware accelerator initialized\n");
+
+	return 0;
+
+err_algs:
+	spin_lock(&img_hash.lock);
+	list_del(&hdev->list);
+	spin_unlock(&img_hash.lock);
+	dma_release_channel(hdev->dma_lch);
+dma_err:
+	clk_disable_unprepare(hdev->sys_clk);
+clk_err:
+	clk_disable_unprepare(hdev->hash_clk);
+res_err:
+	tasklet_kill(&hdev->done_task);
+	tasklet_kill(&hdev->dma_task);
+
+	return err;
+}
+
+static int img_hash_remove(struct platform_device *pdev)
+{
+	static struct img_hash_dev *hdev;
+
+	hdev = platform_get_drvdata(pdev);
+	spin_lock(&img_hash.lock);
+	list_del(&hdev->list);
+	spin_unlock(&img_hash.lock);
+
+	img_unregister_algs(hdev);
+
+	tasklet_kill(&hdev->done_task);
+	tasklet_kill(&hdev->dma_task);
+
+	dma_release_channel(hdev->dma_lch);
+
+	clk_disable_unprepare(hdev->hash_clk);
+	clk_disable_unprepare(hdev->sys_clk);
+
+	return 0;
+}
+
+static struct platform_driver img_hash_driver = {
+	.probe		= img_hash_probe,
+	.remove		= img_hash_remove,
+	.driver		= {
+		.name	= "img-hash-accelerator",
+		.of_match_table	= of_match_ptr(img_hash_match),
+	}
+};
+module_platform_driver(img_hash_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Imgtec SHA1/224/256 & MD5 hw accelerator driver");
+MODULE_AUTHOR("Will Thomas.");
+MODULE_AUTHOR("James Hartley <james.hartley@imgtec.com>");
diff --git a/drivers/crypto/mxs-dcp.c b/drivers/crypto/mxs-dcp.c
index 829d6394fb33..59ed54e464a9 100644
--- a/drivers/crypto/mxs-dcp.c
+++ b/drivers/crypto/mxs-dcp.c
@@ -153,7 +153,7 @@ static int mxs_dcp_start_dma(struct dcp_async_ctx *actx)
 	struct dcp *sdcp = global_sdcp;
 	const int chan = actx->chan;
 	uint32_t stat;
-	int ret;
+	unsigned long ret;
 	struct dcp_dma_desc *desc = &sdcp->coh->desc[actx->chan];
 
 	dma_addr_t desc_phys = dma_map_single(sdcp->dev, desc, sizeof(*desc),
diff --git a/drivers/crypto/omap-aes.c b/drivers/crypto/omap-aes.c
index 42f95a4326b0..9a28b7e07c71 100644
--- a/drivers/crypto/omap-aes.c
+++ b/drivers/crypto/omap-aes.c
@@ -554,15 +554,23 @@ static int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd)
 	return err;
 }
 
-static int omap_aes_check_aligned(struct scatterlist *sg)
+static int omap_aes_check_aligned(struct scatterlist *sg, int total)
 {
+	int len = 0;
+
 	while (sg) {
 		if (!IS_ALIGNED(sg->offset, 4))
 			return -1;
 		if (!IS_ALIGNED(sg->length, AES_BLOCK_SIZE))
 			return -1;
+
+		len += sg->length;
 		sg = sg_next(sg);
 	}
+
+	if (len != total)
+		return -1;
+
 	return 0;
 }
 
@@ -633,8 +641,8 @@ static int omap_aes_handle_queue(struct omap_aes_dev *dd,
 	dd->in_sg = req->src;
 	dd->out_sg = req->dst;
 
-	if (omap_aes_check_aligned(dd->in_sg) ||
-	    omap_aes_check_aligned(dd->out_sg)) {
+	if (omap_aes_check_aligned(dd->in_sg, dd->total) ||
+	    omap_aes_check_aligned(dd->out_sg, dd->total)) {
 		if (omap_aes_copy_sgs(dd))
 			pr_err("Failed to copy SGs for unaligned cases\n");
 		dd->sgs_copied = 1;
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index 3c76696ee578..4d63e0d4da9a 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -640,6 +640,7 @@ static size_t omap_sham_append_sg(struct omap_sham_reqctx *ctx)
 
 	while (ctx->sg) {
 		vaddr = kmap_atomic(sg_page(ctx->sg));
+		vaddr += ctx->sg->offset;
 
 		count = omap_sham_append_buffer(ctx,
 				vaddr + ctx->offset,
@@ -1945,6 +1946,7 @@ static int omap_sham_probe(struct platform_device *pdev)
 	dd->flags |= dd->pdata->flags;
 
 	pm_runtime_enable(dev);
+	pm_runtime_irq_safe(dev);
 	pm_runtime_get_sync(dev);
 	rev = omap_sham_read(dd, SHA_REG_REV(dd));
 	pm_runtime_put_sync(&pdev->dev);
diff --git a/drivers/crypto/qat/qat_common/adf_accel_devices.h b/drivers/crypto/qat/qat_common/adf_accel_devices.h
index 19c0efa29ab3..f22ce7169fa5 100644
--- a/drivers/crypto/qat/qat_common/adf_accel_devices.h
+++ b/drivers/crypto/qat/qat_common/adf_accel_devices.h
@@ -52,7 +52,6 @@
 #include <linux/io.h>
 #include "adf_cfg_common.h"
 
-#define PCI_VENDOR_ID_INTEL 0x8086
 #define ADF_DH895XCC_DEVICE_NAME "dh895xcc"
 #define ADF_DH895XCC_PCI_DEVICE_ID 0x435
 #define ADF_PCI_MAX_BARS 3
diff --git a/drivers/crypto/qat/qat_common/adf_accel_engine.c b/drivers/crypto/qat/qat_common/adf_accel_engine.c
index c77453b900a3..7f8b66c915ed 100644
--- a/drivers/crypto/qat/qat_common/adf_accel_engine.c
+++ b/drivers/crypto/qat/qat_common/adf_accel_engine.c
@@ -60,36 +60,40 @@ int adf_ae_fw_load(struct adf_accel_dev *accel_dev)
 
 	if (request_firmware(&loader_data->uof_fw, hw_device->fw_name,
 			     &accel_dev->accel_pci_dev.pci_dev->dev)) {
-		pr_err("QAT: Failed to load firmware %s\n", hw_device->fw_name);
+		dev_err(&GET_DEV(accel_dev), "Failed to load firmware %s\n",
+			hw_device->fw_name);
 		return -EFAULT;
 	}
 
 	uof_size = loader_data->uof_fw->size;
 	uof_addr = (void *)loader_data->uof_fw->data;
 	if (qat_uclo_map_uof_obj(loader_data->fw_loader, uof_addr, uof_size)) {
-		pr_err("QAT: Failed to map UOF\n");
+		dev_err(&GET_DEV(accel_dev), "Failed to map UOF\n");
 		goto out_err;
 	}
 	if (qat_uclo_wr_all_uimage(loader_data->fw_loader)) {
-		pr_err("QAT: Failed to map UOF\n");
+		dev_err(&GET_DEV(accel_dev), "Failed to map UOF\n");
 		goto out_err;
 	}
 	return 0;
 
 out_err:
-	release_firmware(loader_data->uof_fw);
+	adf_ae_fw_release(accel_dev);
 	return -EFAULT;
 }
 
-int adf_ae_fw_release(struct adf_accel_dev *accel_dev)
+void adf_ae_fw_release(struct adf_accel_dev *accel_dev)
 {
 	struct adf_fw_loader_data *loader_data = accel_dev->fw_loader;
 
-	release_firmware(loader_data->uof_fw);
 	qat_uclo_del_uof_obj(loader_data->fw_loader);
 	qat_hal_deinit(loader_data->fw_loader);
+
+	if (loader_data->uof_fw)
+		release_firmware(loader_data->uof_fw);
+
+	loader_data->uof_fw = NULL;
 	loader_data->fw_loader = NULL;
-	return 0;
 }
 
 int adf_ae_start(struct adf_accel_dev *accel_dev)
@@ -104,8 +108,9 @@ int adf_ae_start(struct adf_accel_dev *accel_dev)
 			ae_ctr++;
 		}
 	}
-	pr_info("QAT: qat_dev%d started %d acceleration engines\n",
-		accel_dev->accel_id, ae_ctr);
+	dev_info(&GET_DEV(accel_dev),
+		 "qat_dev%d started %d acceleration engines\n",
+		 accel_dev->accel_id, ae_ctr);
 	return 0;
 }
 
@@ -121,8 +126,9 @@ int adf_ae_stop(struct adf_accel_dev *accel_dev)
 			ae_ctr++;
 		}
 	}
-	pr_info("QAT: qat_dev%d stopped %d acceleration engines\n",
-		accel_dev->accel_id, ae_ctr);
+	dev_info(&GET_DEV(accel_dev),
+		 "qat_dev%d stopped %d acceleration engines\n",
+		 accel_dev->accel_id, ae_ctr);
 	return 0;
 }
 
@@ -147,12 +153,12 @@ int adf_ae_init(struct adf_accel_dev *accel_dev)
 
 	accel_dev->fw_loader = loader_data;
 	if (qat_hal_init(accel_dev)) {
-		pr_err("QAT: Failed to init the AEs\n");
+		dev_err(&GET_DEV(accel_dev), "Failed to init the AEs\n");
 		kfree(loader_data);
 		return -EFAULT;
 	}
 	if (adf_ae_reset(accel_dev, 0)) {
-		pr_err("QAT: Failed to reset the AEs\n");
+		dev_err(&GET_DEV(accel_dev), "Failed to reset the AEs\n");
 		qat_hal_deinit(loader_data->fw_loader);
 		kfree(loader_data);
 		return -EFAULT;
@@ -162,6 +168,9 @@ int adf_ae_init(struct adf_accel_dev *accel_dev)
 
 int adf_ae_shutdown(struct adf_accel_dev *accel_dev)
 {
+	struct adf_fw_loader_data *loader_data = accel_dev->fw_loader;
+
+	qat_hal_deinit(loader_data->fw_loader);
 	kfree(accel_dev->fw_loader);
 	accel_dev->fw_loader = NULL;
 	return 0;
diff --git a/drivers/crypto/qat/qat_common/adf_aer.c b/drivers/crypto/qat/qat_common/adf_aer.c
index fa1fef824de2..2dbc733b8ab2 100644
--- a/drivers/crypto/qat/qat_common/adf_aer.c
+++ b/drivers/crypto/qat/qat_common/adf_aer.c
@@ -60,14 +60,14 @@ static pci_ers_result_t adf_error_detected(struct pci_dev *pdev,
 {
 	struct adf_accel_dev *accel_dev = adf_devmgr_pci_to_accel_dev(pdev);
 
-	pr_info("QAT: Acceleration driver hardware error detected.\n");
+	dev_info(&pdev->dev, "Acceleration driver hardware error detected.\n");
 	if (!accel_dev) {
-		pr_err("QAT: Can't find acceleration device\n");
+		dev_err(&pdev->dev, "Can't find acceleration device\n");
 		return PCI_ERS_RESULT_DISCONNECT;
 	}
 
 	if (state == pci_channel_io_perm_failure) {
-		pr_err("QAT: Can't recover from device error\n");
+		dev_err(&pdev->dev, "Can't recover from device error\n");
 		return PCI_ERS_RESULT_DISCONNECT;
 	}
 
@@ -88,10 +88,12 @@ static void adf_dev_restore(struct adf_accel_dev *accel_dev)
 	struct pci_dev *parent = pdev->bus->self;
 	uint16_t bridge_ctl = 0;
 
-	pr_info("QAT: Resetting device qat_dev%d\n", accel_dev->accel_id);
+	dev_info(&GET_DEV(accel_dev), "Resetting device qat_dev%d\n",
+		 accel_dev->accel_id);
 
 	if (!pci_wait_for_pending_transaction(pdev))
-		pr_info("QAT: Transaction still in progress. Proceeding\n");
+		dev_info(&GET_DEV(accel_dev),
+			 "Transaction still in progress. Proceeding\n");
 
 	pci_read_config_word(parent, PCI_BRIDGE_CONTROL, &bridge_ctl);
 	bridge_ctl |= PCI_BRIDGE_CTL_BUS_RESET;
@@ -158,7 +160,8 @@ static int adf_dev_aer_schedule_reset(struct adf_accel_dev *accel_dev,
 		unsigned long timeout = wait_for_completion_timeout(
 				   &reset_data->compl, wait_jiffies);
 		if (!timeout) {
-			pr_err("QAT: Reset device timeout expired\n");
+			dev_err(&GET_DEV(accel_dev),
+				"Reset device timeout expired\n");
 			ret = -EFAULT;
 		}
 		kfree(reset_data);
@@ -184,8 +187,8 @@ static pci_ers_result_t adf_slot_reset(struct pci_dev *pdev)
 
 static void adf_resume(struct pci_dev *pdev)
 {
-	pr_info("QAT: Acceleration driver reset completed\n");
-	pr_info("QAT: Device is up and runnig\n");
+	dev_info(&pdev->dev, "Acceleration driver reset completed\n");
+	dev_info(&pdev->dev, "Device is up and runnig\n");
 }
 
 static struct pci_error_handlers adf_err_handler = {
@@ -236,7 +239,7 @@ EXPORT_SYMBOL_GPL(adf_disable_aer);
 int adf_init_aer(void)
 {
 	device_reset_wq = create_workqueue("qat_device_reset_wq");
-	return (device_reset_wq == NULL) ? -EFAULT : 0;
+	return !device_reset_wq ? -EFAULT : 0;
 }
 
 void adf_exit_aer(void)
diff --git a/drivers/crypto/qat/qat_common/adf_cfg.c b/drivers/crypto/qat/qat_common/adf_cfg.c
index de16da9070a5..ab65bc274561 100644
--- a/drivers/crypto/qat/qat_common/adf_cfg.c
+++ b/drivers/crypto/qat/qat_common/adf_cfg.c
@@ -142,7 +142,8 @@ int adf_cfg_dev_add(struct adf_accel_dev *accel_dev)
 						  dev_cfg_data,
 						  &qat_dev_cfg_fops);
 	if (!dev_cfg_data->debug) {
-		pr_err("QAT: Failed to create qat cfg debugfs entry.\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to create qat cfg debugfs entry.\n");
 		kfree(dev_cfg_data);
 		accel_dev->cfg = NULL;
 		return -EFAULT;
@@ -305,7 +306,7 @@ int adf_cfg_add_key_value_param(struct adf_accel_dev *accel_dev,
 		snprintf(key_val->val, ADF_CFG_MAX_VAL_LEN_IN_BYTES,
 			 "0x%lx", (unsigned long)val);
 	} else {
-		pr_err("QAT: Unknown type given.\n");
+		dev_err(&GET_DEV(accel_dev), "Unknown type given.\n");
 		kfree(key_val);
 		return -1;
 	}
diff --git a/drivers/crypto/qat/qat_common/adf_cfg_strings.h b/drivers/crypto/qat/qat_common/adf_cfg_strings.h
index c7ac758ebc90..13575111382c 100644
--- a/drivers/crypto/qat/qat_common/adf_cfg_strings.h
+++ b/drivers/crypto/qat/qat_common/adf_cfg_strings.h
@@ -59,7 +59,7 @@
 #define ADF_RING_SYM_TX "RingSymTx"
 #define ADF_RING_RND_TX "RingNrbgTx"
 #define ADF_RING_ASYM_RX "RingAsymRx"
-#define ADF_RING_SYM_RX "RinSymRx"
+#define ADF_RING_SYM_RX "RingSymRx"
 #define ADF_RING_RND_RX "RingNrbgRx"
 #define ADF_RING_DC_TX "RingTx"
 #define ADF_RING_DC_RX "RingRx"
@@ -69,15 +69,15 @@
 #define ADF_DC "Dc"
 #define ADF_ETRMGR_COALESCING_ENABLED "InterruptCoalescingEnabled"
 #define ADF_ETRMGR_COALESCING_ENABLED_FORMAT \
-	ADF_ETRMGR_BANK"%d"ADF_ETRMGR_COALESCING_ENABLED
+	ADF_ETRMGR_BANK "%d" ADF_ETRMGR_COALESCING_ENABLED
 #define ADF_ETRMGR_COALESCE_TIMER "InterruptCoalescingTimerNs"
 #define ADF_ETRMGR_COALESCE_TIMER_FORMAT \
-	ADF_ETRMGR_BANK"%d"ADF_ETRMGR_COALESCE_TIMER
+	ADF_ETRMGR_BANK "%d" ADF_ETRMGR_COALESCE_TIMER
 #define ADF_ETRMGR_COALESCING_MSG_ENABLED "InterruptCoalescingNumResponses"
 #define ADF_ETRMGR_COALESCING_MSG_ENABLED_FORMAT \
-	ADF_ETRMGR_BANK"%d"ADF_ETRMGR_COALESCING_MSG_ENABLED
+	ADF_ETRMGR_BANK "%d" ADF_ETRMGR_COALESCING_MSG_ENABLED
 #define ADF_ETRMGR_CORE_AFFINITY "CoreAffinity"
 #define ADF_ETRMGR_CORE_AFFINITY_FORMAT \
-	ADF_ETRMGR_BANK"%d"ADF_ETRMGR_CORE_AFFINITY
+	ADF_ETRMGR_BANK "%d" ADF_ETRMGR_CORE_AFFINITY
 #define ADF_ACCEL_STR "Accelerator%d"
 #endif
diff --git a/drivers/crypto/qat/qat_common/adf_common_drv.h b/drivers/crypto/qat/qat_common/adf_common_drv.h
index a62e485c8786..0666ee6a3360 100644
--- a/drivers/crypto/qat/qat_common/adf_common_drv.h
+++ b/drivers/crypto/qat/qat_common/adf_common_drv.h
@@ -115,7 +115,7 @@ int adf_dev_restarted_notify(struct adf_accel_dev *accel_dev);
 int adf_ae_init(struct adf_accel_dev *accel_dev);
 int adf_ae_shutdown(struct adf_accel_dev *accel_dev);
 int adf_ae_fw_load(struct adf_accel_dev *accel_dev);
-int adf_ae_fw_release(struct adf_accel_dev *accel_dev);
+void adf_ae_fw_release(struct adf_accel_dev *accel_dev);
 int adf_ae_start(struct adf_accel_dev *accel_dev);
 int adf_ae_stop(struct adf_accel_dev *accel_dev);
 
diff --git a/drivers/crypto/qat/qat_common/adf_ctl_drv.c b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
index 74207a6f0516..cb5f066e93a6 100644
--- a/drivers/crypto/qat/qat_common/adf_ctl_drv.c
+++ b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
@@ -77,14 +77,14 @@ struct adf_ctl_drv_info {
 	struct class *drv_class;
 };
 
-static struct adf_ctl_drv_info adt_ctl_drv;
+static struct adf_ctl_drv_info adf_ctl_drv;
 
 static void adf_chr_drv_destroy(void)
 {
-	device_destroy(adt_ctl_drv.drv_class, MKDEV(adt_ctl_drv.major, 0));
-	cdev_del(&adt_ctl_drv.drv_cdev);
-	class_destroy(adt_ctl_drv.drv_class);
-	unregister_chrdev_region(MKDEV(adt_ctl_drv.major, 0), 1);
+	device_destroy(adf_ctl_drv.drv_class, MKDEV(adf_ctl_drv.major, 0));
+	cdev_del(&adf_ctl_drv.drv_cdev);
+	class_destroy(adf_ctl_drv.drv_class);
+	unregister_chrdev_region(MKDEV(adf_ctl_drv.major, 0), 1);
 }
 
 static int adf_chr_drv_create(void)
@@ -97,20 +97,20 @@ static int adf_chr_drv_create(void)
 		return -EFAULT;
 	}
 
-	adt_ctl_drv.drv_class = class_create(THIS_MODULE, DEVICE_NAME);
-	if (IS_ERR(adt_ctl_drv.drv_class)) {
+	adf_ctl_drv.drv_class = class_create(THIS_MODULE, DEVICE_NAME);
+	if (IS_ERR(adf_ctl_drv.drv_class)) {
 		pr_err("QAT: class_create failed for adf_ctl\n");
 		goto err_chrdev_unreg;
 	}
-	adt_ctl_drv.major = MAJOR(dev_id);
-	cdev_init(&adt_ctl_drv.drv_cdev, &adf_ctl_ops);
-	if (cdev_add(&adt_ctl_drv.drv_cdev, dev_id, 1)) {
+	adf_ctl_drv.major = MAJOR(dev_id);
+	cdev_init(&adf_ctl_drv.drv_cdev, &adf_ctl_ops);
+	if (cdev_add(&adf_ctl_drv.drv_cdev, dev_id, 1)) {
 		pr_err("QAT: cdev add failed\n");
 		goto err_class_destr;
 	}
 
-	drv_device = device_create(adt_ctl_drv.drv_class, NULL,
-				   MKDEV(adt_ctl_drv.major, 0),
+	drv_device = device_create(adf_ctl_drv.drv_class, NULL,
+				   MKDEV(adf_ctl_drv.major, 0),
 				   NULL, DEVICE_NAME);
 	if (IS_ERR(drv_device)) {
 		pr_err("QAT: failed to create device\n");
@@ -118,9 +118,9 @@ static int adf_chr_drv_create(void)
 	}
 	return 0;
 err_cdev_del:
-	cdev_del(&adt_ctl_drv.drv_cdev);
+	cdev_del(&adf_ctl_drv.drv_cdev);
 err_class_destr:
-	class_destroy(adt_ctl_drv.drv_class);
+	class_destroy(adf_ctl_drv.drv_class);
 err_chrdev_unreg:
 	unregister_chrdev_region(dev_id, 1);
 	return -EFAULT;
@@ -159,14 +159,16 @@ static int adf_add_key_value_data(struct adf_accel_dev *accel_dev,
 		if (adf_cfg_add_key_value_param(accel_dev, section,
 						key_val->key, (void *)val,
 						key_val->type)) {
-			pr_err("QAT: failed to add keyvalue.\n");
+			dev_err(&GET_DEV(accel_dev),
+				"failed to add hex keyvalue.\n");
 			return -EFAULT;
 		}
 	} else {
 		if (adf_cfg_add_key_value_param(accel_dev, section,
 						key_val->key, key_val->val,
 						key_val->type)) {
-			pr_err("QAT: failed to add keyvalue.\n");
+			dev_err(&GET_DEV(accel_dev),
+				"failed to add keyvalue.\n");
 			return -EFAULT;
 		}
 	}
@@ -185,12 +187,14 @@ static int adf_copy_key_value_data(struct adf_accel_dev *accel_dev,
 	while (section_head) {
 		if (copy_from_user(&section, (void __user *)section_head,
 				   sizeof(*section_head))) {
-			pr_err("QAT: failed to copy section info\n");
+			dev_err(&GET_DEV(accel_dev),
+				"failed to copy section info\n");
 			goto out_err;
 		}
 
 		if (adf_cfg_section_add(accel_dev, section.name)) {
-			pr_err("QAT: failed to add section.\n");
+			dev_err(&GET_DEV(accel_dev),
+				"failed to add section.\n");
 			goto out_err;
 		}
 
@@ -199,7 +203,8 @@ static int adf_copy_key_value_data(struct adf_accel_dev *accel_dev,
 		while (params_head) {
 			if (copy_from_user(&key_val, (void __user *)params_head,
 					   sizeof(key_val))) {
-				pr_err("QAT: Failed to copy keyvalue.\n");
+				dev_err(&GET_DEV(accel_dev),
+					"Failed to copy keyvalue.\n");
 				goto out_err;
 			}
 			if (adf_add_key_value_data(accel_dev, section.name,
@@ -258,8 +263,9 @@ static int adf_ctl_is_device_in_use(int id)
 
 		if (id == dev->accel_id || id == ADF_CFG_ALL_DEVICES) {
 			if (adf_devmgr_in_reset(dev) || adf_dev_in_use(dev)) {
-				pr_info("QAT: device qat_dev%d is busy\n",
-					dev->accel_id);
+				dev_info(&GET_DEV(dev),
+					 "device qat_dev%d is busy\n",
+					 dev->accel_id);
 				return -EBUSY;
 			}
 		}
@@ -280,7 +286,8 @@ static int adf_ctl_stop_devices(uint32_t id)
 				continue;
 
 			if (adf_dev_stop(accel_dev)) {
-				pr_err("QAT: Failed to stop qat_dev%d\n", id);
+				dev_err(&GET_DEV(accel_dev),
+					"Failed to stop qat_dev%d\n", id);
 				ret = -EFAULT;
 			} else {
 				adf_dev_shutdown(accel_dev);
@@ -343,17 +350,20 @@ static int adf_ctl_ioctl_dev_start(struct file *fp, unsigned int cmd,
 	}
 
 	if (!adf_dev_started(accel_dev)) {
-		pr_info("QAT: Starting acceleration device qat_dev%d.\n",
-			ctl_data->device_id);
+		dev_info(&GET_DEV(accel_dev),
+			 "Starting acceleration device qat_dev%d.\n",
+			 ctl_data->device_id);
 		ret = adf_dev_init(accel_dev);
 		if (!ret)
 			ret = adf_dev_start(accel_dev);
 	} else {
-		pr_info("QAT: Acceleration device qat_dev%d already started.\n",
-			ctl_data->device_id);
+		dev_info(&GET_DEV(accel_dev),
+			 "Acceleration device qat_dev%d already started.\n",
+			 ctl_data->device_id);
 	}
 	if (ret) {
-		pr_err("QAT: Failed to start qat_dev%d\n", ctl_data->device_id);
+		dev_err(&GET_DEV(accel_dev), "Failed to start qat_dev%d\n",
+			ctl_data->device_id);
 		adf_dev_stop(accel_dev);
 		adf_dev_shutdown(accel_dev);
 	}
@@ -408,7 +418,7 @@ static int adf_ctl_ioctl_get_status(struct file *fp, unsigned int cmd,
 
 	if (copy_to_user((void __user *)arg, &dev_info,
 			 sizeof(struct adf_dev_status_info))) {
-		pr_err("QAT: failed to copy status.\n");
+		dev_err(&GET_DEV(accel_dev), "failed to copy status.\n");
 		return -EFAULT;
 	}
 	return 0;
diff --git a/drivers/crypto/qat/qat_common/adf_dev_mgr.c b/drivers/crypto/qat/qat_common/adf_dev_mgr.c
index 4a0a829d4500..3f0ff9e7d840 100644
--- a/drivers/crypto/qat/qat_common/adf_dev_mgr.c
+++ b/drivers/crypto/qat/qat_common/adf_dev_mgr.c
@@ -67,7 +67,8 @@ int adf_devmgr_add_dev(struct adf_accel_dev *accel_dev)
 	struct list_head *itr;
 
 	if (num_devices == ADF_MAX_DEVICES) {
-		pr_err("QAT: Only support up to %d devices\n", ADF_MAX_DEVICES);
+		dev_err(&GET_DEV(accel_dev), "Only support up to %d devices\n",
+			ADF_MAX_DEVICES);
 		return -EFAULT;
 	}
 
diff --git a/drivers/crypto/qat/qat_common/adf_init.c b/drivers/crypto/qat/qat_common/adf_init.c
index 8f0ca498ab87..245f43237a2d 100644
--- a/drivers/crypto/qat/qat_common/adf_init.c
+++ b/drivers/crypto/qat/qat_common/adf_init.c
@@ -124,12 +124,12 @@ int adf_dev_init(struct adf_accel_dev *accel_dev)
 
 	if (!hw_data) {
 		dev_err(&GET_DEV(accel_dev),
-			"QAT: Failed to init device - hw_data not set\n");
+			"Failed to init device - hw_data not set\n");
 		return -EFAULT;
 	}
 
 	if (!test_bit(ADF_STATUS_CONFIGURED, &accel_dev->status)) {
-		pr_info("QAT: Device not configured\n");
+		dev_err(&GET_DEV(accel_dev), "Device not configured\n");
 		return -EFAULT;
 	}
 
@@ -151,20 +151,21 @@ int adf_dev_init(struct adf_accel_dev *accel_dev)
 	hw_data->enable_ints(accel_dev);
 
 	if (adf_ae_init(accel_dev)) {
-		pr_err("QAT: Failed to initialise Acceleration Engine\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to initialise Acceleration Engine\n");
 		return -EFAULT;
 	}
 	set_bit(ADF_STATUS_AE_INITIALISED, &accel_dev->status);
 
 	if (adf_ae_fw_load(accel_dev)) {
-		pr_err("QAT: Failed to load acceleration FW\n");
-		adf_ae_fw_release(accel_dev);
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to load acceleration FW\n");
 		return -EFAULT;
 	}
 	set_bit(ADF_STATUS_AE_UCODE_LOADED, &accel_dev->status);
 
 	if (hw_data->alloc_irq(accel_dev)) {
-		pr_err("QAT: Failed to allocate interrupts\n");
+		dev_err(&GET_DEV(accel_dev), "Failed to allocate interrupts\n");
 		return -EFAULT;
 	}
 	set_bit(ADF_STATUS_IRQ_ALLOCATED, &accel_dev->status);
@@ -179,8 +180,9 @@ int adf_dev_init(struct adf_accel_dev *accel_dev)
 		if (!service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_INIT)) {
-			pr_err("QAT: Failed to initialise service %s\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to initialise service %s\n",
+				service->name);
 			return -EFAULT;
 		}
 		set_bit(accel_dev->accel_id, &service->init_status);
@@ -190,8 +192,9 @@ int adf_dev_init(struct adf_accel_dev *accel_dev)
 		if (service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_INIT)) {
-			pr_err("QAT: Failed to initialise service %s\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to initialise service %s\n",
+				service->name);
 			return -EFAULT;
 		}
 		set_bit(accel_dev->accel_id, &service->init_status);
@@ -221,7 +224,7 @@ int adf_dev_start(struct adf_accel_dev *accel_dev)
 	set_bit(ADF_STATUS_STARTING, &accel_dev->status);
 
 	if (adf_ae_start(accel_dev)) {
-		pr_err("QAT: AE Start Failed\n");
+		dev_err(&GET_DEV(accel_dev), "AE Start Failed\n");
 		return -EFAULT;
 	}
 	set_bit(ADF_STATUS_AE_STARTED, &accel_dev->status);
@@ -231,8 +234,9 @@ int adf_dev_start(struct adf_accel_dev *accel_dev)
 		if (!service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_START)) {
-			pr_err("QAT: Failed to start service %s\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to start service %s\n",
+				service->name);
 			return -EFAULT;
 		}
 		set_bit(accel_dev->accel_id, &service->start_status);
@@ -242,8 +246,9 @@ int adf_dev_start(struct adf_accel_dev *accel_dev)
 		if (service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_START)) {
-			pr_err("QAT: Failed to start service %s\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to start service %s\n",
+				service->name);
 			return -EFAULT;
 		}
 		set_bit(accel_dev->accel_id, &service->start_status);
@@ -253,7 +258,8 @@ int adf_dev_start(struct adf_accel_dev *accel_dev)
 	set_bit(ADF_STATUS_STARTED, &accel_dev->status);
 
 	if (qat_algs_register()) {
-		pr_err("QAT: Failed to register crypto algs\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to register crypto algs\n");
 		set_bit(ADF_STATUS_STARTING, &accel_dev->status);
 		clear_bit(ADF_STATUS_STARTED, &accel_dev->status);
 		return -EFAULT;
@@ -287,7 +293,8 @@ int adf_dev_stop(struct adf_accel_dev *accel_dev)
 	clear_bit(ADF_STATUS_STARTED, &accel_dev->status);
 
 	if (qat_algs_unregister())
-		pr_err("QAT: Failed to unregister crypto algs\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to unregister crypto algs\n");
 
 	list_for_each(list_itr, &service_table) {
 		service = list_entry(list_itr, struct service_hndl, list);
@@ -310,8 +317,9 @@ int adf_dev_stop(struct adf_accel_dev *accel_dev)
 		if (!test_bit(accel_dev->accel_id, &service->start_status))
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_STOP))
-			pr_err("QAT: Failed to shutdown service %s\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to shutdown service %s\n",
+				service->name);
 		else
 			clear_bit(accel_dev->accel_id, &service->start_status);
 	}
@@ -321,7 +329,7 @@ int adf_dev_stop(struct adf_accel_dev *accel_dev)
 
 	if (test_bit(ADF_STATUS_AE_STARTED, &accel_dev->status)) {
 		if (adf_ae_stop(accel_dev))
-			pr_err("QAT: failed to stop AE\n");
+			dev_err(&GET_DEV(accel_dev), "failed to stop AE\n");
 		else
 			clear_bit(ADF_STATUS_AE_STARTED, &accel_dev->status);
 	}
@@ -350,16 +358,14 @@ void adf_dev_shutdown(struct adf_accel_dev *accel_dev)
 	}
 
 	if (test_bit(ADF_STATUS_AE_UCODE_LOADED, &accel_dev->status)) {
-		if (adf_ae_fw_release(accel_dev))
-			pr_err("QAT: Failed to release the ucode\n");
-		else
-			clear_bit(ADF_STATUS_AE_UCODE_LOADED,
-				  &accel_dev->status);
+		adf_ae_fw_release(accel_dev);
+		clear_bit(ADF_STATUS_AE_UCODE_LOADED, &accel_dev->status);
 	}
 
 	if (test_bit(ADF_STATUS_AE_INITIALISED, &accel_dev->status)) {
 		if (adf_ae_shutdown(accel_dev))
-			pr_err("QAT: Failed to shutdown Accel Engine\n");
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to shutdown Accel Engine\n");
 		else
 			clear_bit(ADF_STATUS_AE_INITIALISED,
 				  &accel_dev->status);
@@ -372,8 +378,9 @@ void adf_dev_shutdown(struct adf_accel_dev *accel_dev)
 		if (!test_bit(accel_dev->accel_id, &service->init_status))
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_SHUTDOWN))
-			pr_err("QAT: Failed to shutdown service %s\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to shutdown service %s\n",
+				service->name);
 		else
 			clear_bit(accel_dev->accel_id, &service->init_status);
 	}
@@ -384,8 +391,9 @@ void adf_dev_shutdown(struct adf_accel_dev *accel_dev)
 		if (!test_bit(accel_dev->accel_id, &service->init_status))
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_SHUTDOWN))
-			pr_err("QAT: Failed to shutdown service %s\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to shutdown service %s\n",
+				service->name);
 		else
 			clear_bit(accel_dev->accel_id, &service->init_status);
 	}
@@ -419,16 +427,18 @@ int adf_dev_restarting_notify(struct adf_accel_dev *accel_dev)
 		if (service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_RESTARTING))
-			pr_err("QAT: Failed to restart service %s.\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to restart service %s.\n",
+				service->name);
 	}
 	list_for_each(list_itr, &service_table) {
 		service = list_entry(list_itr, struct service_hndl, list);
 		if (!service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_RESTARTING))
-			pr_err("QAT: Failed to restart service %s.\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to restart service %s.\n",
+				service->name);
 	}
 	return 0;
 }
@@ -443,16 +453,18 @@ int adf_dev_restarted_notify(struct adf_accel_dev *accel_dev)
 		if (service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_RESTARTED))
-			pr_err("QAT: Failed to restart service %s.\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to restart service %s.\n",
+				service->name);
 	}
 	list_for_each(list_itr, &service_table) {
 		service = list_entry(list_itr, struct service_hndl, list);
 		if (!service->admin)
 			continue;
 		if (service->event_hld(accel_dev, ADF_EVENT_RESTARTED))
-			pr_err("QAT: Failed to restart service %s.\n",
-			       service->name);
+			dev_err(&GET_DEV(accel_dev),
+				"Failed to restart service %s.\n",
+				service->name);
 	}
 	return 0;
 }
diff --git a/drivers/crypto/qat/qat_common/adf_transport.c b/drivers/crypto/qat/qat_common/adf_transport.c
index 7dd54aaee9fa..ccec327489da 100644
--- a/drivers/crypto/qat/qat_common/adf_transport.c
+++ b/drivers/crypto/qat/qat_common/adf_transport.c
@@ -195,7 +195,7 @@ static int adf_init_ring(struct adf_etr_ring_data *ring)
 	memset(ring->base_addr, 0x7F, ring_size_bytes);
 	/* The base_addr has to be aligned to the size of the buffer */
 	if (adf_check_ring_alignment(ring->dma_addr, ring_size_bytes)) {
-		pr_err("QAT: Ring address not aligned\n");
+		dev_err(&GET_DEV(accel_dev), "Ring address not aligned\n");
 		dma_free_coherent(&GET_DEV(accel_dev), ring_size_bytes,
 				  ring->base_addr, ring->dma_addr);
 		return -EFAULT;
@@ -242,32 +242,33 @@ int adf_create_ring(struct adf_accel_dev *accel_dev, const char *section,
 	int ret;
 
 	if (bank_num >= GET_MAX_BANKS(accel_dev)) {
-		pr_err("QAT: Invalid bank number\n");
+		dev_err(&GET_DEV(accel_dev), "Invalid bank number\n");
 		return -EFAULT;
 	}
 	if (msg_size > ADF_MSG_SIZE_TO_BYTES(ADF_MAX_MSG_SIZE)) {
-		pr_err("QAT: Invalid msg size\n");
+		dev_err(&GET_DEV(accel_dev), "Invalid msg size\n");
 		return -EFAULT;
 	}
 	if (ADF_MAX_INFLIGHTS(adf_verify_ring_size(msg_size, num_msgs),
 			      ADF_BYTES_TO_MSG_SIZE(msg_size)) < 2) {
-		pr_err("QAT: Invalid ring size for given msg size\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Invalid ring size for given msg size\n");
 		return -EFAULT;
 	}
 	if (adf_cfg_get_param_value(accel_dev, section, ring_name, val)) {
-		pr_err("QAT: Section %s, no such entry : %s\n",
-		       section, ring_name);
+		dev_err(&GET_DEV(accel_dev), "Section %s, no such entry : %s\n",
+			section, ring_name);
 		return -EFAULT;
 	}
 	if (kstrtouint(val, 10, &ring_num)) {
-		pr_err("QAT: Can't get ring number\n");
+		dev_err(&GET_DEV(accel_dev), "Can't get ring number\n");
 		return -EFAULT;
 	}
 
 	bank = &transport_data->banks[bank_num];
 	if (adf_reserve_ring(bank, ring_num)) {
-		pr_err("QAT: Ring %d, %s already exists.\n",
-		       ring_num, ring_name);
+		dev_err(&GET_DEV(accel_dev), "Ring %d, %s already exists.\n",
+			ring_num, ring_name);
 		return -EFAULT;
 	}
 	ring = &bank->rings[ring_num];
@@ -287,7 +288,8 @@ int adf_create_ring(struct adf_accel_dev *accel_dev, const char *section,
 	accel_dev->hw_device->hw_arb_ring_enable(ring);
 
 	if (adf_ring_debugfs_add(ring, ring_name)) {
-		pr_err("QAT: Couldn't add ring debugfs entry\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Couldn't add ring debugfs entry\n");
 		ret = -EFAULT;
 		goto err;
 	}
@@ -428,7 +430,8 @@ static int adf_init_bank(struct adf_accel_dev *accel_dev,
 				goto err;
 		} else {
 			if (i < hw_data->tx_rx_gap) {
-				pr_err("QAT: Invalid tx rings mask config\n");
+				dev_err(&GET_DEV(accel_dev),
+					"Invalid tx rings mask config\n");
 				goto err;
 			}
 			tx_ring = &bank->rings[i - hw_data->tx_rx_gap];
@@ -436,7 +439,8 @@ static int adf_init_bank(struct adf_accel_dev *accel_dev,
 		}
 	}
 	if (adf_bank_debugfs_add(bank)) {
-		pr_err("QAT: Failed to add bank debugfs entry\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to add bank debugfs entry\n");
 		goto err;
 	}
 
@@ -492,7 +496,8 @@ int adf_init_etr_data(struct adf_accel_dev *accel_dev)
 	etr_data->debug = debugfs_create_dir("transport",
 					     accel_dev->debugfs_dir);
 	if (!etr_data->debug) {
-		pr_err("QAT: Unable to create transport debugfs entry\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Unable to create transport debugfs entry\n");
 		ret = -ENOENT;
 		goto err_bank_debug;
 	}
diff --git a/drivers/crypto/qat/qat_common/adf_transport_debug.c b/drivers/crypto/qat/qat_common/adf_transport_debug.c
index 6b6974553514..e41986967294 100644
--- a/drivers/crypto/qat/qat_common/adf_transport_debug.c
+++ b/drivers/crypto/qat/qat_common/adf_transport_debug.c
@@ -100,6 +100,8 @@ static int adf_ring_show(struct seq_file *sfile, void *v)
 		empty = READ_CSR_E_STAT(csr, bank->bank_number);
 
 		seq_puts(sfile, "------- Ring configuration -------\n");
+		seq_printf(sfile, "ring name: %s\n",
+			   ring->ring_debug->ring_name);
 		seq_printf(sfile, "ring num %d, bank num %d\n",
 			   ring->ring_number, ring->bank->bank_number);
 		seq_printf(sfile, "head %x, tail %x, empty: %d\n",
diff --git a/drivers/crypto/qat/qat_common/icp_qat_hw.h b/drivers/crypto/qat/qat_common/icp_qat_hw.h
index 68f191b653b0..121d5e6e46ca 100644
--- a/drivers/crypto/qat/qat_common/icp_qat_hw.h
+++ b/drivers/crypto/qat/qat_common/icp_qat_hw.h
@@ -145,7 +145,7 @@ struct icp_qat_hw_auth_setup {
 };
 
 #define QAT_HW_DEFAULT_ALIGNMENT 8
-#define QAT_HW_ROUND_UP(val, n) (((val) + ((n)-1)) & (~(n-1)))
+#define QAT_HW_ROUND_UP(val, n) (((val) + ((n) - 1)) & (~(n - 1)))
 #define ICP_QAT_HW_NULL_STATE1_SZ 32
 #define ICP_QAT_HW_MD5_STATE1_SZ 16
 #define ICP_QAT_HW_SHA1_STATE1_SZ 20
diff --git a/drivers/crypto/qat/qat_common/qat_crypto.c b/drivers/crypto/qat/qat_common/qat_crypto.c
index 828f2a686aab..3bd705ca5973 100644
--- a/drivers/crypto/qat/qat_common/qat_crypto.c
+++ b/drivers/crypto/qat/qat_common/qat_crypto.c
@@ -110,13 +110,13 @@ struct qat_crypto_instance *qat_crypto_get_instance_node(int node)
 	list_for_each(itr, adf_devmgr_get_head()) {
 		accel_dev = list_entry(itr, struct adf_accel_dev, list);
 		if ((node == dev_to_node(&GET_DEV(accel_dev)) ||
-			dev_to_node(&GET_DEV(accel_dev)) < 0)
-				&& adf_dev_started(accel_dev))
+		     dev_to_node(&GET_DEV(accel_dev)) < 0) &&
+		    adf_dev_started(accel_dev))
 			break;
 		accel_dev = NULL;
 	}
 	if (!accel_dev) {
-		pr_err("QAT: Could not find device on node %d\n", node);
+		pr_err("QAT: Could not find a device on node %d\n", node);
 		accel_dev = adf_devmgr_get_first();
 	}
 	if (!accel_dev || !adf_dev_started(accel_dev))
@@ -137,7 +137,8 @@ struct qat_crypto_instance *qat_crypto_get_instance_node(int node)
 		if (atomic_add_return(1, &inst_best->refctr) == 1) {
 			if (adf_dev_get(accel_dev)) {
 				atomic_dec(&inst_best->refctr);
-				pr_err("QAT: Could increment dev refctr\n");
+				dev_err(&GET_DEV(accel_dev),
+					"Could not increment dev refctr\n");
 				return NULL;
 			}
 		}
diff --git a/drivers/crypto/qat/qat_common/qat_hal.c b/drivers/crypto/qat/qat_common/qat_hal.c
index b818c19713bf..274ff7e9de6e 100644
--- a/drivers/crypto/qat/qat_common/qat_hal.c
+++ b/drivers/crypto/qat/qat_common/qat_hal.c
@@ -434,8 +434,8 @@ static void qat_hal_reset_timestamp(struct icp_qat_fw_loader_handle *handle)
 	SET_GLB_CSR(handle, MISC_CONTROL, misc_ctl | MC_TIMESTAMP_ENABLE);
 }
 
-#define ESRAM_AUTO_TINIT (1<<2)
-#define ESRAM_AUTO_TINIT_DONE (1<<3)
+#define ESRAM_AUTO_TINIT	BIT(2)
+#define ESRAM_AUTO_TINIT_DONE	BIT(3)
 #define ESRAM_AUTO_INIT_USED_CYCLES (1640)
 #define ESRAM_AUTO_INIT_CSR_OFFSET 0xC1C
 static int qat_hal_init_esram(struct icp_qat_fw_loader_handle *handle)
@@ -718,7 +718,7 @@ int qat_hal_init(struct adf_accel_dev *accel_dev)
 	handle->hal_handle->ae_max_num = max_en_ae_id + 1;
 	/* take all AEs out of reset */
 	if (qat_hal_clr_reset(handle)) {
-		pr_err("QAT: qat_hal_clr_reset error\n");
+		dev_err(&GET_DEV(accel_dev), "qat_hal_clr_reset error\n");
 		goto out_err;
 	}
 	if (qat_hal_clear_gpr(handle))
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_admin.c b/drivers/crypto/qat/qat_dh895xcc/adf_admin.c
index 53c491b59f07..e4666065c399 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_admin.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_admin.c
@@ -93,7 +93,8 @@ int adf_put_admin_msg_sync(struct adf_accel_dev *accel_dev,
 		memcpy(out, admin->virt_addr + offset +
 		       ADF_ADMINMSG_LEN, ADF_ADMINMSG_LEN);
 	else
-		pr_err("QAT: Failed to send admin msg to accelerator\n");
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to send admin msg to accelerator\n");
 
 	mutex_unlock(&admin->lock);
 	return received ? 0 : -EFAULT;
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c
index 6a735d5c0e37..b1386922d7a2 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c
@@ -150,7 +150,8 @@ void adf_get_arbiter_mapping(struct adf_accel_dev *accel_dev,
 		*arb_map_config = thrd_to_arb_map_sku6;
 		break;
 	default:
-		pr_err("QAT: The configuration doesn't match any SKU");
+		dev_err(&GET_DEV(accel_dev),
+			"The configuration doesn't match any SKU");
 		*arb_map_config = NULL;
 	}
 }
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
index 01e0be21e93a..25269a9f24a2 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
@@ -73,11 +73,11 @@
 /* Error detection and correction */
 #define ADF_DH895XCC_AE_CTX_ENABLES(i) (i * 0x1000 + 0x20818)
 #define ADF_DH895XCC_AE_MISC_CONTROL(i) (i * 0x1000 + 0x20960)
-#define ADF_DH895XCC_ENABLE_AE_ECC_ERR (1 << 28)
-#define ADF_DH895XCC_ENABLE_AE_ECC_PARITY_CORR (1 << 24 | 1 << 12)
+#define ADF_DH895XCC_ENABLE_AE_ECC_ERR BIT(28)
+#define ADF_DH895XCC_ENABLE_AE_ECC_PARITY_CORR (BIT(24) | BIT(12))
 #define ADF_DH895XCC_UERRSSMSH(i) (i * 0x4000 + 0x18)
 #define ADF_DH895XCC_CERRSSMSH(i) (i * 0x4000 + 0x10)
-#define ADF_DH895XCC_ERRSSMSH_EN (1 << 3)
+#define ADF_DH895XCC_ERRSSMSH_EN BIT(3)
 
 /* Admin Messages Registers */
 #define ADF_DH895XCC_ADMINMSGUR_OFFSET (0x3A000 + 0x574)
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_drv.c b/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
index 8ffdb95c9804..9decea2779c6 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
@@ -236,7 +236,7 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	accel_dev = kzalloc_node(sizeof(*accel_dev), GFP_KERNEL,
-			         dev_to_node(&pdev->dev));
+				 dev_to_node(&pdev->dev));
 	if (!accel_dev)
 		return -ENOMEM;
 
@@ -379,7 +379,7 @@ out_err:
 	return ret;
 }
 
-static void __exit adf_remove(struct pci_dev *pdev)
+static void adf_remove(struct pci_dev *pdev)
 {
 	struct adf_accel_dev *accel_dev = adf_devmgr_pci_to_accel_dev(pdev);
 
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c b/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
index fe8f89697ad8..0d03c109c2d3 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
@@ -73,7 +73,7 @@ static int adf_enable_msix(struct adf_accel_dev *accel_dev)
 	if (pci_enable_msix_exact(pci_dev_info->pci_dev,
 				  pci_dev_info->msix_entries.entries,
 				  msix_num_entries)) {
-		pr_err("QAT: Failed to enable MSIX IRQ\n");
+		dev_err(&GET_DEV(accel_dev), "Failed to enable MSIX IRQ\n");
 		return -EFAULT;
 	}
 	return 0;
@@ -97,7 +97,8 @@ static irqreturn_t adf_msix_isr_ae(int irq, void *dev_ptr)
 {
 	struct adf_accel_dev *accel_dev = dev_ptr;
 
-	pr_info("QAT: qat_dev%d spurious AE interrupt\n", accel_dev->accel_id);
+	dev_info(&GET_DEV(accel_dev), "qat_dev%d spurious AE interrupt\n",
+		 accel_dev->accel_id);
 	return IRQ_HANDLED;
 }
 
@@ -121,8 +122,9 @@ static int adf_request_irqs(struct adf_accel_dev *accel_dev)
 		ret = request_irq(msixe[i].vector,
 				  adf_msix_isr_bundle, 0, name, bank);
 		if (ret) {
-			pr_err("QAT: failed to enable irq %d for %s\n",
-			       msixe[i].vector, name);
+			dev_err(&GET_DEV(accel_dev),
+				"failed to enable irq %d for %s\n",
+				msixe[i].vector, name);
 			return ret;
 		}
 
@@ -136,8 +138,9 @@ static int adf_request_irqs(struct adf_accel_dev *accel_dev)
 		 "qat%d-ae-cluster", accel_dev->accel_id);
 	ret = request_irq(msixe[i].vector, adf_msix_isr_ae, 0, name, accel_dev);
 	if (ret) {
-		pr_err("QAT: failed to enable irq %d, for %s\n",
-		       msixe[i].vector, name);
+		dev_err(&GET_DEV(accel_dev),
+			"failed to enable irq %d, for %s\n",
+			msixe[i].vector, name);
 		return ret;
 	}
 	return ret;
diff --git a/drivers/crypto/sahara.c b/drivers/crypto/sahara.c
index 290a7f0a681f..6be377f6b9e7 100644
--- a/drivers/crypto/sahara.c
+++ b/drivers/crypto/sahara.c
@@ -479,6 +479,7 @@ static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 	struct scatterlist *sg;
 	int ret;
 	int i, j;
+	int idx = 0;
 
 	/* Copy new key if necessary */
 	if (ctx->flags & FLAGS_NEW_KEY) {
@@ -486,17 +487,20 @@ static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 		ctx->flags &= ~FLAGS_NEW_KEY;
 
 		if (dev->flags & FLAGS_CBC) {
-			dev->hw_desc[0]->len1 = AES_BLOCK_SIZE;
-			dev->hw_desc[0]->p1 = dev->iv_phys_base;
+			dev->hw_desc[idx]->len1 = AES_BLOCK_SIZE;
+			dev->hw_desc[idx]->p1 = dev->iv_phys_base;
 		} else {
-			dev->hw_desc[0]->len1 = 0;
-			dev->hw_desc[0]->p1 = 0;
+			dev->hw_desc[idx]->len1 = 0;
+			dev->hw_desc[idx]->p1 = 0;
 		}
-		dev->hw_desc[0]->len2 = ctx->keylen;
-		dev->hw_desc[0]->p2 = dev->key_phys_base;
-		dev->hw_desc[0]->next = dev->hw_phys_desc[1];
+		dev->hw_desc[idx]->len2 = ctx->keylen;
+		dev->hw_desc[idx]->p2 = dev->key_phys_base;
+		dev->hw_desc[idx]->next = dev->hw_phys_desc[1];
+
+		dev->hw_desc[idx]->hdr = sahara_aes_key_hdr(dev);
+
+		idx++;
 	}
-	dev->hw_desc[0]->hdr = sahara_aes_key_hdr(dev);
 
 	dev->nb_in_sg = sahara_sg_length(dev->in_sg, dev->total);
 	dev->nb_out_sg = sahara_sg_length(dev->out_sg, dev->total);
@@ -520,7 +524,7 @@ static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 	}
 
 	/* Create input links */
-	dev->hw_desc[1]->p1 = dev->hw_phys_link[0];
+	dev->hw_desc[idx]->p1 = dev->hw_phys_link[0];
 	sg = dev->in_sg;
 	for (i = 0; i < dev->nb_in_sg; i++) {
 		dev->hw_link[i]->len = sg->length;
@@ -534,7 +538,7 @@ static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 	}
 
 	/* Create output links */
-	dev->hw_desc[1]->p2 = dev->hw_phys_link[i];
+	dev->hw_desc[idx]->p2 = dev->hw_phys_link[i];
 	sg = dev->out_sg;
 	for (j = i; j < dev->nb_out_sg + i; j++) {
 		dev->hw_link[j]->len = sg->length;
@@ -548,10 +552,10 @@ static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 	}
 
 	/* Fill remaining fields of hw_desc[1] */
-	dev->hw_desc[1]->hdr = sahara_aes_data_link_hdr(dev);
-	dev->hw_desc[1]->len1 = dev->total;
-	dev->hw_desc[1]->len2 = dev->total;
-	dev->hw_desc[1]->next = 0;
+	dev->hw_desc[idx]->hdr = sahara_aes_data_link_hdr(dev);
+	dev->hw_desc[idx]->len1 = dev->total;
+	dev->hw_desc[idx]->len2 = dev->total;
+	dev->hw_desc[idx]->next = 0;
 
 	sahara_dump_descriptors(dev);
 	sahara_dump_links(dev);
@@ -576,6 +580,7 @@ static int sahara_aes_process(struct ablkcipher_request *req)
 	struct sahara_ctx *ctx;
 	struct sahara_aes_reqctx *rctx;
 	int ret;
+	unsigned long timeout;
 
 	/* Request is ready to be dispatched by the device */
 	dev_dbg(dev->device,
@@ -601,10 +606,12 @@ static int sahara_aes_process(struct ablkcipher_request *req)
 	reinit_completion(&dev->dma_completion);
 
 	ret = sahara_hw_descriptor_create(dev);
+	if (ret)
+		return -EINVAL;
 
-	ret = wait_for_completion_timeout(&dev->dma_completion,
+	timeout = wait_for_completion_timeout(&dev->dma_completion,
 				msecs_to_jiffies(SAHARA_TIMEOUT_MS));
-	if (!ret) {
+	if (!timeout) {
 		dev_err(dev->device, "AES timeout\n");
 		return -ETIMEDOUT;
 	}
@@ -1044,7 +1051,8 @@ static int sahara_sha_process(struct ahash_request *req)
 {
 	struct sahara_dev *dev = dev_ptr;
 	struct sahara_sha_reqctx *rctx = ahash_request_ctx(req);
-	int ret = -EINPROGRESS;
+	int ret;
+	unsigned long timeout;
 
 	ret = sahara_sha_prepare_request(req);
 	if (!ret)
@@ -1070,9 +1078,9 @@ static int sahara_sha_process(struct ahash_request *req)
 
 	sahara_write(dev, dev->hw_phys_desc[0], SAHARA_REG_DAR);
 
-	ret = wait_for_completion_timeout(&dev->dma_completion,
+	timeout = wait_for_completion_timeout(&dev->dma_completion,
 				msecs_to_jiffies(SAHARA_TIMEOUT_MS));
-	if (!ret) {
+	if (!timeout) {
 		dev_err(dev->device, "SHA timeout\n");
 		return -ETIMEDOUT;
 	}
@@ -1092,15 +1100,20 @@ static int sahara_queue_manage(void *data)
 {
 	struct sahara_dev *dev = (struct sahara_dev *)data;
 	struct crypto_async_request *async_req;
+	struct crypto_async_request *backlog;
 	int ret = 0;
 
 	do {
 		__set_current_state(TASK_INTERRUPTIBLE);
 
 		mutex_lock(&dev->queue_mutex);
+		backlog = crypto_get_backlog(&dev->queue);
 		async_req = crypto_dequeue_request(&dev->queue);
 		mutex_unlock(&dev->queue_mutex);
 
+		if (backlog)
+			backlog->complete(backlog, -EINPROGRESS);
+
 		if (async_req) {
 			if (crypto_tfm_alg_type(async_req->tfm) ==
 			    CRYPTO_ALG_TYPE_AHASH) {
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index ebbae8d3ce0d..857414afa29a 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -637,8 +637,6 @@ static void talitos_unregister_rng(struct device *dev)
 #define TALITOS_MAX_KEY_SIZE		96
 #define TALITOS_MAX_IV_LENGTH		16 /* max of AES_BLOCK_SIZE, DES3_EDE_BLOCK_SIZE */
 
-#define MD5_BLOCK_SIZE    64
-
 struct talitos_ctx {
 	struct device *dev;
 	int ch;
@@ -2195,7 +2193,7 @@ static struct talitos_alg_template driver_algs[] = {
 			.halg.base = {
 				.cra_name = "md5",
 				.cra_driver_name = "md5-talitos",
-				.cra_blocksize = MD5_BLOCK_SIZE,
+				.cra_blocksize = MD5_HMAC_BLOCK_SIZE,
 				.cra_flags = CRYPTO_ALG_TYPE_AHASH |
 					     CRYPTO_ALG_ASYNC,
 			}
@@ -2285,7 +2283,7 @@ static struct talitos_alg_template driver_algs[] = {
 			.halg.base = {
 				.cra_name = "hmac(md5)",
 				.cra_driver_name = "hmac-md5-talitos",
-				.cra_blocksize = MD5_BLOCK_SIZE,
+				.cra_blocksize = MD5_HMAC_BLOCK_SIZE,
 				.cra_flags = CRYPTO_ALG_TYPE_AHASH |
 					     CRYPTO_ALG_ASYNC,
 			}
@@ -2706,20 +2704,16 @@ static int talitos_probe(struct platform_device *ofdev)
 		goto err_out;
 	}
 
+	priv->fifo_len = roundup_pow_of_two(priv->chfifo_len);
+
 	for (i = 0; i < priv->num_channels; i++) {
 		priv->chan[i].reg = priv->reg + TALITOS_CH_STRIDE * (i + 1);
 		if (!priv->irq[1] || !(i & 1))
 			priv->chan[i].reg += TALITOS_CH_BASE_OFFSET;
-	}
 
-	for (i = 0; i < priv->num_channels; i++) {
 		spin_lock_init(&priv->chan[i].head_lock);
 		spin_lock_init(&priv->chan[i].tail_lock);
-	}
 
-	priv->fifo_len = roundup_pow_of_two(priv->chfifo_len);
-
-	for (i = 0; i < priv->num_channels; i++) {
 		priv->chan[i].fifo = kzalloc(sizeof(struct talitos_request) *
 					     priv->fifo_len, GFP_KERNEL);
 		if (!priv->chan[i].fifo) {
@@ -2727,11 +2721,10 @@ static int talitos_probe(struct platform_device *ofdev)
 			err = -ENOMEM;
 			goto err_out;
 		}
-	}
 
-	for (i = 0; i < priv->num_channels; i++)
 		atomic_set(&priv->chan[i].submit_count,
 			   -(priv->chfifo_len - 1));
+	}
 
 	dma_set_mask(dev, DMA_BIT_MASK(36));
 
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
index 187a8fd7eee7..5f5f360628fc 100644
--- a/drivers/crypto/ux500/hash/hash_core.c
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -184,7 +184,7 @@ static int hash_set_dma_transfer(struct hash_ctx *ctx, struct scatterlist *sg,
 			direction, DMA_CTRL_ACK | DMA_PREP_INTERRUPT);
 	if (!desc) {
 		dev_err(ctx->device->dev,
-			"%s: device_prep_slave_sg() failed!\n", __func__);
+			"%s: dmaengine_prep_slave_sg() failed!\n", __func__);
 		return -EFAULT;
 	}
 
diff --git a/drivers/crypto/vmx/Kconfig b/drivers/crypto/vmx/Kconfig
new file mode 100644
index 000000000000..771babf16aa0
--- /dev/null
+++ b/drivers/crypto/vmx/Kconfig
@@ -0,0 +1,8 @@
+config CRYPTO_DEV_VMX_ENCRYPT
+	tristate "Encryption acceleration support on P8 CPU"
+	depends on PPC64 && CRYPTO_DEV_VMX
+	default y
+	help
+	  Support for VMX cryptographic acceleration instructions on Power8 CPU.
+	  This module supports acceleration for AES and GHASH in hardware. If you
+	  choose 'M' here, this module will be called vmx-crypto.
diff --git a/drivers/crypto/vmx/Makefile b/drivers/crypto/vmx/Makefile
new file mode 100644
index 000000000000..c699c6e6c82e
--- /dev/null
+++ b/drivers/crypto/vmx/Makefile
@@ -0,0 +1,19 @@
+obj-$(CONFIG_CRYPTO_DEV_VMX_ENCRYPT) += vmx-crypto.o
+vmx-crypto-objs := vmx.o aesp8-ppc.o ghashp8-ppc.o aes.o aes_cbc.o aes_ctr.o ghash.o
+
+ifeq ($(CONFIG_CPU_LITTLE_ENDIAN),y)
+TARGET := linux-ppc64le
+else
+TARGET := linux-pcc64
+endif
+
+quiet_cmd_perl = PERL $@
+      cmd_perl = $(PERL) $(<) $(TARGET) > $(@)
+
+$(src)/aesp8-ppc.S: $(src)/aesp8-ppc.pl
+	$(call cmd,perl)
+  
+$(src)/ghashp8-ppc.S: $(src)/ghashp8-ppc.pl
+	$(call cmd,perl)
+
+.PRECIOUS: $(obj)/aesp8-ppc.S $(obj)/ghashp8-ppc.S
diff --git a/drivers/crypto/vmx/aes.c b/drivers/crypto/vmx/aes.c
new file mode 100644
index 000000000000..ab300ea19434
--- /dev/null
+++ b/drivers/crypto/vmx/aes.c
@@ -0,0 +1,139 @@
+/**
+ * AES routines supporting VMX instructions on the Power 8
+ *
+ * Copyright (C) 2015 International Business Machines Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 only.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ * Author: Marcelo Henrique Cerri <mhcerri@br.ibm.com>
+ */
+
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/crypto.h>
+#include <linux/delay.h>
+#include <linux/hardirq.h>
+#include <asm/switch_to.h>
+#include <crypto/aes.h>
+
+#include "aesp8-ppc.h"
+
+struct p8_aes_ctx {
+    struct crypto_cipher *fallback;
+    struct aes_key enc_key;
+    struct aes_key dec_key;
+};
+
+static int p8_aes_init(struct crypto_tfm *tfm)
+{
+    const char *alg;
+    struct crypto_cipher *fallback;
+    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (!(alg = crypto_tfm_alg_name(tfm))) {
+        printk(KERN_ERR "Failed to get algorithm name.\n");
+        return -ENOENT;
+    }
+
+    fallback = crypto_alloc_cipher(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
+    if (IS_ERR(fallback)) {
+        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
+                alg, PTR_ERR(fallback));
+        return PTR_ERR(fallback);
+    }
+    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+            crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
+
+    crypto_cipher_set_flags(fallback,
+            crypto_cipher_get_flags((struct crypto_cipher *) tfm));
+    ctx->fallback = fallback;
+
+    return 0;
+}
+
+static void p8_aes_exit(struct crypto_tfm *tfm)
+{
+    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (ctx->fallback) {
+        crypto_free_cipher(ctx->fallback);
+        ctx->fallback = NULL;
+    }
+}
+
+static int p8_aes_setkey(struct crypto_tfm *tfm, const u8 *key,
+    unsigned int keylen)
+{
+    int ret;
+    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    pagefault_disable();
+    enable_kernel_altivec();
+    ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
+    ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
+    pagefault_enable();
+    
+    ret += crypto_cipher_setkey(ctx->fallback, key, keylen);
+    return ret;
+}
+
+static void p8_aes_encrypt(struct crypto_tfm *tfm, u8 *dst, const u8 *src)
+{
+    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (in_interrupt()) {
+        crypto_cipher_encrypt_one(ctx->fallback, dst, src);
+    } else {
+        pagefault_disable();
+        enable_kernel_altivec();
+        aes_p8_encrypt(src, dst, &ctx->enc_key);
+        pagefault_enable();
+    }
+}
+
+static void p8_aes_decrypt(struct crypto_tfm *tfm, u8 *dst, const u8 *src)
+{
+    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (in_interrupt()) {
+        crypto_cipher_decrypt_one(ctx->fallback, dst, src);
+    } else {
+        pagefault_disable();
+        enable_kernel_altivec();
+        aes_p8_decrypt(src, dst, &ctx->dec_key);
+        pagefault_enable();
+    }
+}
+
+struct crypto_alg p8_aes_alg = {
+    .cra_name = "aes",
+    .cra_driver_name = "p8_aes",
+    .cra_module = THIS_MODULE,
+    .cra_priority = 1000,
+    .cra_type = NULL,
+    .cra_flags = CRYPTO_ALG_TYPE_CIPHER | CRYPTO_ALG_NEED_FALLBACK,
+    .cra_alignmask = 0,
+    .cra_blocksize = AES_BLOCK_SIZE,
+    .cra_ctxsize = sizeof(struct p8_aes_ctx),
+    .cra_init = p8_aes_init,
+    .cra_exit = p8_aes_exit,
+    .cra_cipher = {
+        .cia_min_keysize = AES_MIN_KEY_SIZE,
+        .cia_max_keysize = AES_MAX_KEY_SIZE,
+        .cia_setkey = p8_aes_setkey,
+        .cia_encrypt = p8_aes_encrypt,
+        .cia_decrypt = p8_aes_decrypt,
+    },
+};
+
diff --git a/drivers/crypto/vmx/aes_cbc.c b/drivers/crypto/vmx/aes_cbc.c
new file mode 100644
index 000000000000..1a559b7dddb5
--- /dev/null
+++ b/drivers/crypto/vmx/aes_cbc.c
@@ -0,0 +1,184 @@
+/**
+ * AES CBC routines supporting VMX instructions on the Power 8
+ *
+ * Copyright (C) 2015 International Business Machines Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 only.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ * Author: Marcelo Henrique Cerri <mhcerri@br.ibm.com>
+ */
+
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/crypto.h>
+#include <linux/delay.h>
+#include <linux/hardirq.h>
+#include <asm/switch_to.h>
+#include <crypto/aes.h>
+#include <crypto/scatterwalk.h>
+
+#include "aesp8-ppc.h"
+
+struct p8_aes_cbc_ctx {
+    struct crypto_blkcipher *fallback;
+    struct aes_key enc_key;
+    struct aes_key dec_key;
+};
+
+static int p8_aes_cbc_init(struct crypto_tfm *tfm)
+{
+    const char *alg;
+    struct crypto_blkcipher *fallback;
+    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (!(alg = crypto_tfm_alg_name(tfm))) {
+        printk(KERN_ERR "Failed to get algorithm name.\n");
+        return -ENOENT;
+    }
+
+    fallback = crypto_alloc_blkcipher(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
+    if (IS_ERR(fallback)) {
+        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
+                alg, PTR_ERR(fallback));
+        return PTR_ERR(fallback);
+    }
+    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+            crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
+
+    crypto_blkcipher_set_flags(fallback,
+            crypto_blkcipher_get_flags((struct crypto_blkcipher *) tfm));
+    ctx->fallback = fallback;
+
+    return 0;
+}
+
+static void p8_aes_cbc_exit(struct crypto_tfm *tfm)
+{
+    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (ctx->fallback) {
+        crypto_free_blkcipher(ctx->fallback);
+        ctx->fallback = NULL;
+    }
+}
+
+static int p8_aes_cbc_setkey(struct crypto_tfm *tfm, const u8 *key,
+    unsigned int keylen)
+{
+    int ret;
+    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    pagefault_disable();
+    enable_kernel_altivec();
+    ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
+    ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
+    pagefault_enable();
+
+    ret += crypto_blkcipher_setkey(ctx->fallback, key, keylen);
+    return ret;
+}
+
+static int p8_aes_cbc_encrypt(struct blkcipher_desc *desc,
+    struct scatterlist *dst, struct scatterlist *src,
+    unsigned int nbytes)
+{
+    int ret;
+    struct blkcipher_walk walk;
+    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(
+            crypto_blkcipher_tfm(desc->tfm));
+    struct blkcipher_desc fallback_desc = {
+        .tfm = ctx->fallback,
+        .info = desc->info,
+        .flags = desc->flags
+    };
+
+    if (in_interrupt()) {
+        ret = crypto_blkcipher_encrypt(&fallback_desc, dst, src, nbytes);
+    } else {
+        pagefault_disable();
+        enable_kernel_altivec();
+
+	blkcipher_walk_init(&walk, dst, src, nbytes);
+        ret = blkcipher_walk_virt(desc, &walk);
+        while ((nbytes = walk.nbytes)) {
+			aes_p8_cbc_encrypt(walk.src.virt.addr, walk.dst.virt.addr,
+				nbytes & AES_BLOCK_MASK, &ctx->enc_key, walk.iv, 1);
+			nbytes &= AES_BLOCK_SIZE - 1;
+			ret = blkcipher_walk_done(desc, &walk, nbytes);
+	}
+
+        pagefault_enable();
+    }
+
+    return ret;
+}
+
+static int p8_aes_cbc_decrypt(struct blkcipher_desc *desc,
+    struct scatterlist *dst, struct scatterlist *src,
+    unsigned int nbytes)
+{
+    int ret;
+    struct blkcipher_walk walk;
+    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(
+            crypto_blkcipher_tfm(desc->tfm));
+    struct blkcipher_desc fallback_desc = {
+        .tfm = ctx->fallback,
+        .info = desc->info,
+        .flags = desc->flags
+    };
+
+    if (in_interrupt()) {
+        ret = crypto_blkcipher_decrypt(&fallback_desc, dst, src, nbytes);
+    } else {
+        pagefault_disable();
+        enable_kernel_altivec();
+
+	blkcipher_walk_init(&walk, dst, src, nbytes);
+        ret = blkcipher_walk_virt(desc, &walk);
+        while ((nbytes = walk.nbytes)) {
+			aes_p8_cbc_encrypt(walk.src.virt.addr, walk.dst.virt.addr,
+				nbytes & AES_BLOCK_MASK, &ctx->dec_key, walk.iv, 0);
+			nbytes &= AES_BLOCK_SIZE - 1;
+			ret = blkcipher_walk_done(desc, &walk, nbytes);
+		}
+
+        pagefault_enable();
+    }
+
+    return ret;
+}
+
+
+struct crypto_alg p8_aes_cbc_alg = {
+    .cra_name = "cbc(aes)",
+    .cra_driver_name = "p8_aes_cbc",
+    .cra_module = THIS_MODULE,
+    .cra_priority = 1000,
+    .cra_type = &crypto_blkcipher_type,
+    .cra_flags = CRYPTO_ALG_TYPE_BLKCIPHER | CRYPTO_ALG_NEED_FALLBACK,
+    .cra_alignmask = 0,
+    .cra_blocksize = AES_BLOCK_SIZE,
+    .cra_ctxsize = sizeof(struct p8_aes_cbc_ctx),
+    .cra_init = p8_aes_cbc_init,
+    .cra_exit = p8_aes_cbc_exit,
+    .cra_blkcipher = {
+        .ivsize = 0,
+        .min_keysize = AES_MIN_KEY_SIZE,
+        .max_keysize = AES_MAX_KEY_SIZE,
+        .setkey = p8_aes_cbc_setkey,
+        .encrypt = p8_aes_cbc_encrypt,
+        .decrypt = p8_aes_cbc_decrypt,
+    },
+};
+
diff --git a/drivers/crypto/vmx/aes_ctr.c b/drivers/crypto/vmx/aes_ctr.c
new file mode 100644
index 000000000000..96dbee4bf4a6
--- /dev/null
+++ b/drivers/crypto/vmx/aes_ctr.c
@@ -0,0 +1,167 @@
+/**
+ * AES CTR routines supporting VMX instructions on the Power 8
+ *
+ * Copyright (C) 2015 International Business Machines Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 only.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ * Author: Marcelo Henrique Cerri <mhcerri@br.ibm.com>
+ */
+
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/crypto.h>
+#include <linux/delay.h>
+#include <linux/hardirq.h>
+#include <asm/switch_to.h>
+#include <crypto/aes.h>
+#include <crypto/scatterwalk.h>
+#include "aesp8-ppc.h"
+
+struct p8_aes_ctr_ctx {
+    struct crypto_blkcipher *fallback;
+    struct aes_key enc_key;
+};
+
+static int p8_aes_ctr_init(struct crypto_tfm *tfm)
+{
+    const char *alg;
+    struct crypto_blkcipher *fallback;
+    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (!(alg = crypto_tfm_alg_name(tfm))) {
+        printk(KERN_ERR "Failed to get algorithm name.\n");
+        return -ENOENT;
+    }
+
+    fallback = crypto_alloc_blkcipher(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
+    if (IS_ERR(fallback)) {
+        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
+                alg, PTR_ERR(fallback));
+        return PTR_ERR(fallback);
+    }
+    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+            crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
+
+    crypto_blkcipher_set_flags(fallback,
+            crypto_blkcipher_get_flags((struct crypto_blkcipher *) tfm));
+    ctx->fallback = fallback;
+
+    return 0;
+}
+
+static void p8_aes_ctr_exit(struct crypto_tfm *tfm)
+{
+    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (ctx->fallback) {
+        crypto_free_blkcipher(ctx->fallback);
+        ctx->fallback = NULL;
+    }
+}
+
+static int p8_aes_ctr_setkey(struct crypto_tfm *tfm, const u8 *key,
+    unsigned int keylen)
+{
+    int ret;
+    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    pagefault_disable();
+    enable_kernel_altivec();
+    ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
+    pagefault_enable();
+
+    ret += crypto_blkcipher_setkey(ctx->fallback, key, keylen);
+    return ret;
+}
+
+static void p8_aes_ctr_final(struct p8_aes_ctr_ctx *ctx,
+                struct blkcipher_walk *walk)
+{
+    u8 *ctrblk = walk->iv;
+    u8 keystream[AES_BLOCK_SIZE];
+    u8 *src = walk->src.virt.addr;
+    u8 *dst = walk->dst.virt.addr;
+    unsigned int nbytes = walk->nbytes;
+
+    pagefault_disable();
+    enable_kernel_altivec();
+    aes_p8_encrypt(ctrblk, keystream, &ctx->enc_key);
+    pagefault_enable();
+
+    crypto_xor(keystream, src, nbytes);
+    memcpy(dst, keystream, nbytes);
+    crypto_inc(ctrblk, AES_BLOCK_SIZE);
+}
+
+static int p8_aes_ctr_crypt(struct blkcipher_desc *desc,
+    struct scatterlist *dst, struct scatterlist *src,
+    unsigned int nbytes)
+{
+    int ret;
+    struct blkcipher_walk walk;
+    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(
+            crypto_blkcipher_tfm(desc->tfm));
+    struct blkcipher_desc fallback_desc = {
+        .tfm = ctx->fallback,
+        .info = desc->info,
+        .flags = desc->flags
+    };
+
+    if (in_interrupt()) {
+        ret = crypto_blkcipher_encrypt(&fallback_desc, dst, src, nbytes);
+    } else {
+        blkcipher_walk_init(&walk, dst, src, nbytes);
+        ret = blkcipher_walk_virt_block(desc, &walk, AES_BLOCK_SIZE);
+        while ((nbytes = walk.nbytes) >= AES_BLOCK_SIZE) {
+            pagefault_disable();
+            enable_kernel_altivec();
+            aes_p8_ctr32_encrypt_blocks(walk.src.virt.addr, walk.dst.virt.addr,
+                (nbytes & AES_BLOCK_MASK)/AES_BLOCK_SIZE, &ctx->enc_key, walk.iv);
+            pagefault_enable();
+
+            crypto_inc(walk.iv, AES_BLOCK_SIZE);
+            nbytes &= AES_BLOCK_SIZE - 1;
+            ret = blkcipher_walk_done(desc, &walk, nbytes);
+        }
+        if (walk.nbytes) {
+            p8_aes_ctr_final(ctx, &walk);
+            ret = blkcipher_walk_done(desc, &walk, 0);
+        }
+    }
+
+    return ret;
+}
+
+struct crypto_alg p8_aes_ctr_alg = {
+    .cra_name = "ctr(aes)",
+    .cra_driver_name = "p8_aes_ctr",
+    .cra_module = THIS_MODULE,
+    .cra_priority = 1000,
+    .cra_type = &crypto_blkcipher_type,
+    .cra_flags = CRYPTO_ALG_TYPE_BLKCIPHER | CRYPTO_ALG_NEED_FALLBACK,
+    .cra_alignmask = 0,
+    .cra_blocksize = 1,
+    .cra_ctxsize = sizeof(struct p8_aes_ctr_ctx),
+    .cra_init = p8_aes_ctr_init,
+    .cra_exit = p8_aes_ctr_exit,
+    .cra_blkcipher = {
+        .ivsize = 0,
+        .min_keysize = AES_MIN_KEY_SIZE,
+        .max_keysize = AES_MAX_KEY_SIZE,
+        .setkey = p8_aes_ctr_setkey,
+        .encrypt = p8_aes_ctr_crypt,
+        .decrypt = p8_aes_ctr_crypt,
+    },
+};
diff --git a/drivers/crypto/vmx/aesp8-ppc.h b/drivers/crypto/vmx/aesp8-ppc.h
new file mode 100644
index 000000000000..e963945a83e1
--- /dev/null
+++ b/drivers/crypto/vmx/aesp8-ppc.h
@@ -0,0 +1,20 @@
+#include <linux/types.h>
+#include <crypto/aes.h>
+
+#define AES_BLOCK_MASK  (~(AES_BLOCK_SIZE-1))
+
+struct aes_key {
+    u8 key[AES_MAX_KEYLENGTH];
+    int rounds;
+};
+
+int aes_p8_set_encrypt_key(const u8 *userKey, const int bits,
+        struct aes_key *key);
+int aes_p8_set_decrypt_key(const u8 *userKey, const int bits,
+        struct aes_key *key);
+void aes_p8_encrypt(const u8 *in, u8 *out, const struct aes_key *key);
+void aes_p8_decrypt(const u8 *in, u8 *out,const struct aes_key *key);
+void aes_p8_cbc_encrypt(const u8 *in, u8 *out, size_t len,
+		const struct aes_key *key, u8 *iv, const int enc);
+void aes_p8_ctr32_encrypt_blocks(const u8 *in, u8 *out,
+        size_t len, const struct aes_key *key, const u8 *iv);
diff --git a/drivers/crypto/vmx/aesp8-ppc.pl b/drivers/crypto/vmx/aesp8-ppc.pl
new file mode 100644
index 000000000000..6c5c20c6108e
--- /dev/null
+++ b/drivers/crypto/vmx/aesp8-ppc.pl
@@ -0,0 +1,1930 @@
+#!/usr/bin/env perl
+#
+# ====================================================================
+# Written by Andy Polyakov <appro@openssl.org> for the OpenSSL
+# project. The module is, however, dual licensed under OpenSSL and
+# CRYPTOGAMS licenses depending on where you obtain it. For further
+# details see http://www.openssl.org/~appro/cryptogams/.
+# ====================================================================
+#
+# This module implements support for AES instructions as per PowerISA
+# specification version 2.07, first implemented by POWER8 processor.
+# The module is endian-agnostic in sense that it supports both big-
+# and little-endian cases. Data alignment in parallelizable modes is
+# handled with VSX loads and stores, which implies MSR.VSX flag being
+# set. It should also be noted that ISA specification doesn't prohibit
+# alignment exceptions for these instructions on page boundaries.
+# Initially alignment was handled in pure AltiVec/VMX way [when data
+# is aligned programmatically, which in turn guarantees exception-
+# free execution], but it turned to hamper performance when vcipher
+# instructions are interleaved. It's reckoned that eventual
+# misalignment penalties at page boundaries are in average lower
+# than additional overhead in pure AltiVec approach.
+
+$flavour = shift;
+
+if ($flavour =~ /64/) {
+	$SIZE_T	=8;
+	$LRSAVE	=2*$SIZE_T;
+	$STU	="stdu";
+	$POP	="ld";
+	$PUSH	="std";
+	$UCMP	="cmpld";
+	$SHL	="sldi";
+} elsif ($flavour =~ /32/) {
+	$SIZE_T	=4;
+	$LRSAVE	=$SIZE_T;
+	$STU	="stwu";
+	$POP	="lwz";
+	$PUSH	="stw";
+	$UCMP	="cmplw";
+	$SHL	="slwi";
+} else { die "nonsense $flavour"; }
+
+$LITTLE_ENDIAN = ($flavour=~/le$/) ? $SIZE_T : 0;
+
+$0 =~ m/(.*[\/\\])[^\/\\]+$/; $dir=$1;
+( $xlate="${dir}ppc-xlate.pl" and -f $xlate ) or
+( $xlate="${dir}../../perlasm/ppc-xlate.pl" and -f $xlate) or
+die "can't locate ppc-xlate.pl";
+
+open STDOUT,"| $^X $xlate $flavour ".shift || die "can't call $xlate: $!";
+
+$FRAME=8*$SIZE_T;
+$prefix="aes_p8";
+
+$sp="r1";
+$vrsave="r12";
+
+#########################################################################
+{{{	# Key setup procedures						#
+my ($inp,$bits,$out,$ptr,$cnt,$rounds)=map("r$_",(3..8));
+my ($zero,$in0,$in1,$key,$rcon,$mask,$tmp)=map("v$_",(0..6));
+my ($stage,$outperm,$outmask,$outhead,$outtail)=map("v$_",(7..11));
+
+$code.=<<___;
+.machine	"any"
+
+.text
+
+.align	7
+rcon:
+.long	0x01000000, 0x01000000, 0x01000000, 0x01000000	?rev
+.long	0x1b000000, 0x1b000000, 0x1b000000, 0x1b000000	?rev
+.long	0x0d0e0f0c, 0x0d0e0f0c, 0x0d0e0f0c, 0x0d0e0f0c	?rev
+.long	0,0,0,0						?asis
+Lconsts:
+	mflr	r0
+	bcl	20,31,\$+4
+	mflr	$ptr	 #vvvvv "distance between . and rcon
+	addi	$ptr,$ptr,-0x48
+	mtlr	r0
+	blr
+	.long	0
+	.byte	0,12,0x14,0,0,0,0,0
+.asciz	"AES for PowerISA 2.07, CRYPTOGAMS by <appro\@openssl.org>"
+
+.globl	.${prefix}_set_encrypt_key
+Lset_encrypt_key:
+	mflr		r11
+	$PUSH		r11,$LRSAVE($sp)
+
+	li		$ptr,-1
+	${UCMP}i	$inp,0
+	beq-		Lenc_key_abort		# if ($inp==0) return -1;
+	${UCMP}i	$out,0
+	beq-		Lenc_key_abort		# if ($out==0) return -1;
+	li		$ptr,-2
+	cmpwi		$bits,128
+	blt-		Lenc_key_abort
+	cmpwi		$bits,256
+	bgt-		Lenc_key_abort
+	andi.		r0,$bits,0x3f
+	bne-		Lenc_key_abort
+
+	lis		r0,0xfff0
+	mfspr		$vrsave,256
+	mtspr		256,r0
+
+	bl		Lconsts
+	mtlr		r11
+
+	neg		r9,$inp
+	lvx		$in0,0,$inp
+	addi		$inp,$inp,15		# 15 is not typo
+	lvsr		$key,0,r9		# borrow $key
+	li		r8,0x20
+	cmpwi		$bits,192
+	lvx		$in1,0,$inp
+	le?vspltisb	$mask,0x0f		# borrow $mask
+	lvx		$rcon,0,$ptr
+	le?vxor		$key,$key,$mask		# adjust for byte swap
+	lvx		$mask,r8,$ptr
+	addi		$ptr,$ptr,0x10
+	vperm		$in0,$in0,$in1,$key	# align [and byte swap in LE]
+	li		$cnt,8
+	vxor		$zero,$zero,$zero
+	mtctr		$cnt
+
+	?lvsr		$outperm,0,$out
+	vspltisb	$outmask,-1
+	lvx		$outhead,0,$out
+	?vperm		$outmask,$zero,$outmask,$outperm
+
+	blt		Loop128
+	addi		$inp,$inp,8
+	beq		L192
+	addi		$inp,$inp,8
+	b		L256
+
+.align	4
+Loop128:
+	vperm		$key,$in0,$in0,$mask	# rotate-n-splat
+	vsldoi		$tmp,$zero,$in0,12	# >>32
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	vcipherlast	$key,$key,$rcon
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	 vadduwm	$rcon,$rcon,$rcon
+	vxor		$in0,$in0,$key
+	bdnz		Loop128
+
+	lvx		$rcon,0,$ptr		# last two round keys
+
+	vperm		$key,$in0,$in0,$mask	# rotate-n-splat
+	vsldoi		$tmp,$zero,$in0,12	# >>32
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	vcipherlast	$key,$key,$rcon
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	 vadduwm	$rcon,$rcon,$rcon
+	vxor		$in0,$in0,$key
+
+	vperm		$key,$in0,$in0,$mask	# rotate-n-splat
+	vsldoi		$tmp,$zero,$in0,12	# >>32
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	vcipherlast	$key,$key,$rcon
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	vxor		$in0,$in0,$key
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	 stvx		$stage,0,$out
+
+	addi		$inp,$out,15		# 15 is not typo
+	addi		$out,$out,0x50
+
+	li		$rounds,10
+	b		Ldone
+
+.align	4
+L192:
+	lvx		$tmp,0,$inp
+	li		$cnt,4
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+	vperm		$in1,$in1,$tmp,$key	# align [and byte swap in LE]
+	vspltisb	$key,8			# borrow $key
+	mtctr		$cnt
+	vsububm		$mask,$mask,$key	# adjust the mask
+
+Loop192:
+	vperm		$key,$in1,$in1,$mask	# roate-n-splat
+	vsldoi		$tmp,$zero,$in0,12	# >>32
+	vcipherlast	$key,$key,$rcon
+
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+
+	 vsldoi		$stage,$zero,$in1,8
+	vspltw		$tmp,$in0,3
+	vxor		$tmp,$tmp,$in1
+	vsldoi		$in1,$zero,$in1,12	# >>32
+	 vadduwm	$rcon,$rcon,$rcon
+	vxor		$in1,$in1,$tmp
+	vxor		$in0,$in0,$key
+	vxor		$in1,$in1,$key
+	 vsldoi		$stage,$stage,$in0,8
+
+	vperm		$key,$in1,$in1,$mask	# rotate-n-splat
+	vsldoi		$tmp,$zero,$in0,12	# >>32
+	 vperm		$outtail,$stage,$stage,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	vcipherlast	$key,$key,$rcon
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+
+	 vsldoi		$stage,$in0,$in1,8
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	 vperm		$outtail,$stage,$stage,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+
+	vspltw		$tmp,$in0,3
+	vxor		$tmp,$tmp,$in1
+	vsldoi		$in1,$zero,$in1,12	# >>32
+	 vadduwm	$rcon,$rcon,$rcon
+	vxor		$in1,$in1,$tmp
+	vxor		$in0,$in0,$key
+	vxor		$in1,$in1,$key
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	 stvx		$stage,0,$out
+	 addi		$inp,$out,15		# 15 is not typo
+	 addi		$out,$out,16
+	bdnz		Loop192
+
+	li		$rounds,12
+	addi		$out,$out,0x20
+	b		Ldone
+
+.align	4
+L256:
+	lvx		$tmp,0,$inp
+	li		$cnt,7
+	li		$rounds,14
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+	vperm		$in1,$in1,$tmp,$key	# align [and byte swap in LE]
+	mtctr		$cnt
+
+Loop256:
+	vperm		$key,$in1,$in1,$mask	# rotate-n-splat
+	vsldoi		$tmp,$zero,$in0,12	# >>32
+	 vperm		$outtail,$in1,$in1,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	vcipherlast	$key,$key,$rcon
+	 stvx		$stage,0,$out
+	 addi		$out,$out,16
+
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in0,$in0,$tmp
+	 vadduwm	$rcon,$rcon,$rcon
+	vxor		$in0,$in0,$key
+	 vperm		$outtail,$in0,$in0,$outperm	# rotate
+	 vsel		$stage,$outhead,$outtail,$outmask
+	 vmr		$outhead,$outtail
+	 stvx		$stage,0,$out
+	 addi		$inp,$out,15		# 15 is not typo
+	 addi		$out,$out,16
+	bdz		Ldone
+
+	vspltw		$key,$in0,3		# just splat
+	vsldoi		$tmp,$zero,$in1,12	# >>32
+	vsbox		$key,$key
+
+	vxor		$in1,$in1,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in1,$in1,$tmp
+	vsldoi		$tmp,$zero,$tmp,12	# >>32
+	vxor		$in1,$in1,$tmp
+
+	vxor		$in1,$in1,$key
+	b		Loop256
+
+.align	4
+Ldone:
+	lvx		$in1,0,$inp		# redundant in aligned case
+	vsel		$in1,$outhead,$in1,$outmask
+	stvx		$in1,0,$inp
+	li		$ptr,0
+	mtspr		256,$vrsave
+	stw		$rounds,0($out)
+
+Lenc_key_abort:
+	mr		r3,$ptr
+	blr
+	.long		0
+	.byte		0,12,0x14,1,0,0,3,0
+	.long		0
+.size	.${prefix}_set_encrypt_key,.-.${prefix}_set_encrypt_key
+
+.globl	.${prefix}_set_decrypt_key
+	$STU		$sp,-$FRAME($sp)
+	mflr		r10
+	$PUSH		r10,$FRAME+$LRSAVE($sp)
+	bl		Lset_encrypt_key
+	mtlr		r10
+
+	cmpwi		r3,0
+	bne-		Ldec_key_abort
+
+	slwi		$cnt,$rounds,4
+	subi		$inp,$out,240		# first round key
+	srwi		$rounds,$rounds,1
+	add		$out,$inp,$cnt		# last round key
+	mtctr		$rounds
+
+Ldeckey:
+	lwz		r0, 0($inp)
+	lwz		r6, 4($inp)
+	lwz		r7, 8($inp)
+	lwz		r8, 12($inp)
+	addi		$inp,$inp,16
+	lwz		r9, 0($out)
+	lwz		r10,4($out)
+	lwz		r11,8($out)
+	lwz		r12,12($out)
+	stw		r0, 0($out)
+	stw		r6, 4($out)
+	stw		r7, 8($out)
+	stw		r8, 12($out)
+	subi		$out,$out,16
+	stw		r9, -16($inp)
+	stw		r10,-12($inp)
+	stw		r11,-8($inp)
+	stw		r12,-4($inp)
+	bdnz		Ldeckey
+
+	xor		r3,r3,r3		# return value
+Ldec_key_abort:
+	addi		$sp,$sp,$FRAME
+	blr
+	.long		0
+	.byte		0,12,4,1,0x80,0,3,0
+	.long		0
+.size	.${prefix}_set_decrypt_key,.-.${prefix}_set_decrypt_key
+___
+}}}
+#########################################################################
+{{{	# Single block en- and decrypt procedures			#
+sub gen_block () {
+my $dir = shift;
+my $n   = $dir eq "de" ? "n" : "";
+my ($inp,$out,$key,$rounds,$idx)=map("r$_",(3..7));
+
+$code.=<<___;
+.globl	.${prefix}_${dir}crypt
+	lwz		$rounds,240($key)
+	lis		r0,0xfc00
+	mfspr		$vrsave,256
+	li		$idx,15			# 15 is not typo
+	mtspr		256,r0
+
+	lvx		v0,0,$inp
+	neg		r11,$out
+	lvx		v1,$idx,$inp
+	lvsl		v2,0,$inp		# inpperm
+	le?vspltisb	v4,0x0f
+	?lvsl		v3,0,r11		# outperm
+	le?vxor		v2,v2,v4
+	li		$idx,16
+	vperm		v0,v0,v1,v2		# align [and byte swap in LE]
+	lvx		v1,0,$key
+	?lvsl		v5,0,$key		# keyperm
+	srwi		$rounds,$rounds,1
+	lvx		v2,$idx,$key
+	addi		$idx,$idx,16
+	subi		$rounds,$rounds,1
+	?vperm		v1,v1,v2,v5		# align round key
+
+	vxor		v0,v0,v1
+	lvx		v1,$idx,$key
+	addi		$idx,$idx,16
+	mtctr		$rounds
+
+Loop_${dir}c:
+	?vperm		v2,v2,v1,v5
+	v${n}cipher	v0,v0,v2
+	lvx		v2,$idx,$key
+	addi		$idx,$idx,16
+	?vperm		v1,v1,v2,v5
+	v${n}cipher	v0,v0,v1
+	lvx		v1,$idx,$key
+	addi		$idx,$idx,16
+	bdnz		Loop_${dir}c
+
+	?vperm		v2,v2,v1,v5
+	v${n}cipher	v0,v0,v2
+	lvx		v2,$idx,$key
+	?vperm		v1,v1,v2,v5
+	v${n}cipherlast	v0,v0,v1
+
+	vspltisb	v2,-1
+	vxor		v1,v1,v1
+	li		$idx,15			# 15 is not typo
+	?vperm		v2,v1,v2,v3		# outmask
+	le?vxor		v3,v3,v4
+	lvx		v1,0,$out		# outhead
+	vperm		v0,v0,v0,v3		# rotate [and byte swap in LE]
+	vsel		v1,v1,v0,v2
+	lvx		v4,$idx,$out
+	stvx		v1,0,$out
+	vsel		v0,v0,v4,v2
+	stvx		v0,$idx,$out
+
+	mtspr		256,$vrsave
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0,0,3,0
+	.long		0
+.size	.${prefix}_${dir}crypt,.-.${prefix}_${dir}crypt
+___
+}
+&gen_block("en");
+&gen_block("de");
+}}}
+#########################################################################
+{{{	# CBC en- and decrypt procedures				#
+my ($inp,$out,$len,$key,$ivp,$enc,$rounds,$idx)=map("r$_",(3..10));
+my ($rndkey0,$rndkey1,$inout,$tmp)=		map("v$_",(0..3));
+my ($ivec,$inptail,$inpperm,$outhead,$outperm,$outmask,$keyperm)=
+						map("v$_",(4..10));
+$code.=<<___;
+.globl	.${prefix}_cbc_encrypt
+	${UCMP}i	$len,16
+	bltlr-
+
+	cmpwi		$enc,0			# test direction
+	lis		r0,0xffe0
+	mfspr		$vrsave,256
+	mtspr		256,r0
+
+	li		$idx,15
+	vxor		$rndkey0,$rndkey0,$rndkey0
+	le?vspltisb	$tmp,0x0f
+
+	lvx		$ivec,0,$ivp		# load [unaligned] iv
+	lvsl		$inpperm,0,$ivp
+	lvx		$inptail,$idx,$ivp
+	le?vxor		$inpperm,$inpperm,$tmp
+	vperm		$ivec,$ivec,$inptail,$inpperm
+
+	neg		r11,$inp
+	?lvsl		$keyperm,0,$key		# prepare for unaligned key
+	lwz		$rounds,240($key)
+
+	lvsr		$inpperm,0,r11		# prepare for unaligned load
+	lvx		$inptail,0,$inp
+	addi		$inp,$inp,15		# 15 is not typo
+	le?vxor		$inpperm,$inpperm,$tmp
+
+	?lvsr		$outperm,0,$out		# prepare for unaligned store
+	vspltisb	$outmask,-1
+	lvx		$outhead,0,$out
+	?vperm		$outmask,$rndkey0,$outmask,$outperm
+	le?vxor		$outperm,$outperm,$tmp
+
+	srwi		$rounds,$rounds,1
+	li		$idx,16
+	subi		$rounds,$rounds,1
+	beq		Lcbc_dec
+
+Lcbc_enc:
+	vmr		$inout,$inptail
+	lvx		$inptail,0,$inp
+	addi		$inp,$inp,16
+	mtctr		$rounds
+	subi		$len,$len,16		# len-=16
+
+	lvx		$rndkey0,0,$key
+	 vperm		$inout,$inout,$inptail,$inpperm
+	lvx		$rndkey1,$idx,$key
+	addi		$idx,$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vxor		$inout,$inout,$rndkey0
+	lvx		$rndkey0,$idx,$key
+	addi		$idx,$idx,16
+	vxor		$inout,$inout,$ivec
+
+Loop_cbc_enc:
+	?vperm		$rndkey1,$rndkey1,$rndkey0,$keyperm
+	vcipher		$inout,$inout,$rndkey1
+	lvx		$rndkey1,$idx,$key
+	addi		$idx,$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vcipher		$inout,$inout,$rndkey0
+	lvx		$rndkey0,$idx,$key
+	addi		$idx,$idx,16
+	bdnz		Loop_cbc_enc
+
+	?vperm		$rndkey1,$rndkey1,$rndkey0,$keyperm
+	vcipher		$inout,$inout,$rndkey1
+	lvx		$rndkey1,$idx,$key
+	li		$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vcipherlast	$ivec,$inout,$rndkey0
+	${UCMP}i	$len,16
+
+	vperm		$tmp,$ivec,$ivec,$outperm
+	vsel		$inout,$outhead,$tmp,$outmask
+	vmr		$outhead,$tmp
+	stvx		$inout,0,$out
+	addi		$out,$out,16
+	bge		Lcbc_enc
+
+	b		Lcbc_done
+
+.align	4
+Lcbc_dec:
+	${UCMP}i	$len,128
+	bge		_aesp8_cbc_decrypt8x
+	vmr		$tmp,$inptail
+	lvx		$inptail,0,$inp
+	addi		$inp,$inp,16
+	mtctr		$rounds
+	subi		$len,$len,16		# len-=16
+
+	lvx		$rndkey0,0,$key
+	 vperm		$tmp,$tmp,$inptail,$inpperm
+	lvx		$rndkey1,$idx,$key
+	addi		$idx,$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vxor		$inout,$tmp,$rndkey0
+	lvx		$rndkey0,$idx,$key
+	addi		$idx,$idx,16
+
+Loop_cbc_dec:
+	?vperm		$rndkey1,$rndkey1,$rndkey0,$keyperm
+	vncipher	$inout,$inout,$rndkey1
+	lvx		$rndkey1,$idx,$key
+	addi		$idx,$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vncipher	$inout,$inout,$rndkey0
+	lvx		$rndkey0,$idx,$key
+	addi		$idx,$idx,16
+	bdnz		Loop_cbc_dec
+
+	?vperm		$rndkey1,$rndkey1,$rndkey0,$keyperm
+	vncipher	$inout,$inout,$rndkey1
+	lvx		$rndkey1,$idx,$key
+	li		$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vncipherlast	$inout,$inout,$rndkey0
+	${UCMP}i	$len,16
+
+	vxor		$inout,$inout,$ivec
+	vmr		$ivec,$tmp
+	vperm		$tmp,$inout,$inout,$outperm
+	vsel		$inout,$outhead,$tmp,$outmask
+	vmr		$outhead,$tmp
+	stvx		$inout,0,$out
+	addi		$out,$out,16
+	bge		Lcbc_dec
+
+Lcbc_done:
+	addi		$out,$out,-1
+	lvx		$inout,0,$out		# redundant in aligned case
+	vsel		$inout,$outhead,$inout,$outmask
+	stvx		$inout,0,$out
+
+	neg		$enc,$ivp		# write [unaligned] iv
+	li		$idx,15			# 15 is not typo
+	vxor		$rndkey0,$rndkey0,$rndkey0
+	vspltisb	$outmask,-1
+	le?vspltisb	$tmp,0x0f
+	?lvsl		$outperm,0,$enc
+	?vperm		$outmask,$rndkey0,$outmask,$outperm
+	le?vxor		$outperm,$outperm,$tmp
+	lvx		$outhead,0,$ivp
+	vperm		$ivec,$ivec,$ivec,$outperm
+	vsel		$inout,$outhead,$ivec,$outmask
+	lvx		$inptail,$idx,$ivp
+	stvx		$inout,0,$ivp
+	vsel		$inout,$ivec,$inptail,$outmask
+	stvx		$inout,$idx,$ivp
+
+	mtspr		256,$vrsave
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0,0,6,0
+	.long		0
+___
+#########################################################################
+{{	# Optimized CBC decrypt procedure				#
+my $key_="r11";
+my ($x00,$x10,$x20,$x30,$x40,$x50,$x60,$x70)=map("r$_",(0,8,26..31));
+my ($in0, $in1, $in2, $in3, $in4, $in5, $in6, $in7 )=map("v$_",(0..3,10..13));
+my ($out0,$out1,$out2,$out3,$out4,$out5,$out6,$out7)=map("v$_",(14..21));
+my $rndkey0="v23";	# v24-v25 rotating buffer for first found keys
+			# v26-v31 last 6 round keys
+my ($tmp,$keyperm)=($in3,$in4);	# aliases with "caller", redundant assignment
+
+$code.=<<___;
+.align	5
+_aesp8_cbc_decrypt8x:
+	$STU		$sp,-`($FRAME+21*16+6*$SIZE_T)`($sp)
+	li		r10,`$FRAME+8*16+15`
+	li		r11,`$FRAME+8*16+31`
+	stvx		v20,r10,$sp		# ABI says so
+	addi		r10,r10,32
+	stvx		v21,r11,$sp
+	addi		r11,r11,32
+	stvx		v22,r10,$sp
+	addi		r10,r10,32
+	stvx		v23,r11,$sp
+	addi		r11,r11,32
+	stvx		v24,r10,$sp
+	addi		r10,r10,32
+	stvx		v25,r11,$sp
+	addi		r11,r11,32
+	stvx		v26,r10,$sp
+	addi		r10,r10,32
+	stvx		v27,r11,$sp
+	addi		r11,r11,32
+	stvx		v28,r10,$sp
+	addi		r10,r10,32
+	stvx		v29,r11,$sp
+	addi		r11,r11,32
+	stvx		v30,r10,$sp
+	stvx		v31,r11,$sp
+	li		r0,-1
+	stw		$vrsave,`$FRAME+21*16-4`($sp)	# save vrsave
+	li		$x10,0x10
+	$PUSH		r26,`$FRAME+21*16+0*$SIZE_T`($sp)
+	li		$x20,0x20
+	$PUSH		r27,`$FRAME+21*16+1*$SIZE_T`($sp)
+	li		$x30,0x30
+	$PUSH		r28,`$FRAME+21*16+2*$SIZE_T`($sp)
+	li		$x40,0x40
+	$PUSH		r29,`$FRAME+21*16+3*$SIZE_T`($sp)
+	li		$x50,0x50
+	$PUSH		r30,`$FRAME+21*16+4*$SIZE_T`($sp)
+	li		$x60,0x60
+	$PUSH		r31,`$FRAME+21*16+5*$SIZE_T`($sp)
+	li		$x70,0x70
+	mtspr		256,r0
+
+	subi		$rounds,$rounds,3	# -4 in total
+	subi		$len,$len,128		# bias
+
+	lvx		$rndkey0,$x00,$key	# load key schedule
+	lvx		v30,$x10,$key
+	addi		$key,$key,0x20
+	lvx		v31,$x00,$key
+	?vperm		$rndkey0,$rndkey0,v30,$keyperm
+	addi		$key_,$sp,$FRAME+15
+	mtctr		$rounds
+
+Load_cbc_dec_key:
+	?vperm		v24,v30,v31,$keyperm
+	lvx		v30,$x10,$key
+	addi		$key,$key,0x20
+	stvx		v24,$x00,$key_		# off-load round[1]
+	?vperm		v25,v31,v30,$keyperm
+	lvx		v31,$x00,$key
+	stvx		v25,$x10,$key_		# off-load round[2]
+	addi		$key_,$key_,0x20
+	bdnz		Load_cbc_dec_key
+
+	lvx		v26,$x10,$key
+	?vperm		v24,v30,v31,$keyperm
+	lvx		v27,$x20,$key
+	stvx		v24,$x00,$key_		# off-load round[3]
+	?vperm		v25,v31,v26,$keyperm
+	lvx		v28,$x30,$key
+	stvx		v25,$x10,$key_		# off-load round[4]
+	addi		$key_,$sp,$FRAME+15	# rewind $key_
+	?vperm		v26,v26,v27,$keyperm
+	lvx		v29,$x40,$key
+	?vperm		v27,v27,v28,$keyperm
+	lvx		v30,$x50,$key
+	?vperm		v28,v28,v29,$keyperm
+	lvx		v31,$x60,$key
+	?vperm		v29,v29,v30,$keyperm
+	lvx		$out0,$x70,$key		# borrow $out0
+	?vperm		v30,v30,v31,$keyperm
+	lvx		v24,$x00,$key_		# pre-load round[1]
+	?vperm		v31,v31,$out0,$keyperm
+	lvx		v25,$x10,$key_		# pre-load round[2]
+
+	#lvx		$inptail,0,$inp		# "caller" already did this
+	#addi		$inp,$inp,15		# 15 is not typo
+	subi		$inp,$inp,15		# undo "caller"
+
+	 le?li		$idx,8
+	lvx_u		$in0,$x00,$inp		# load first 8 "words"
+	 le?lvsl	$inpperm,0,$idx
+	 le?vspltisb	$tmp,0x0f
+	lvx_u		$in1,$x10,$inp
+	 le?vxor	$inpperm,$inpperm,$tmp	# transform for lvx_u/stvx_u
+	lvx_u		$in2,$x20,$inp
+	 le?vperm	$in0,$in0,$in0,$inpperm
+	lvx_u		$in3,$x30,$inp
+	 le?vperm	$in1,$in1,$in1,$inpperm
+	lvx_u		$in4,$x40,$inp
+	 le?vperm	$in2,$in2,$in2,$inpperm
+	vxor		$out0,$in0,$rndkey0
+	lvx_u		$in5,$x50,$inp
+	 le?vperm	$in3,$in3,$in3,$inpperm
+	vxor		$out1,$in1,$rndkey0
+	lvx_u		$in6,$x60,$inp
+	 le?vperm	$in4,$in4,$in4,$inpperm
+	vxor		$out2,$in2,$rndkey0
+	lvx_u		$in7,$x70,$inp
+	addi		$inp,$inp,0x80
+	 le?vperm	$in5,$in5,$in5,$inpperm
+	vxor		$out3,$in3,$rndkey0
+	 le?vperm	$in6,$in6,$in6,$inpperm
+	vxor		$out4,$in4,$rndkey0
+	 le?vperm	$in7,$in7,$in7,$inpperm
+	vxor		$out5,$in5,$rndkey0
+	vxor		$out6,$in6,$rndkey0
+	vxor		$out7,$in7,$rndkey0
+
+	mtctr		$rounds
+	b		Loop_cbc_dec8x
+.align	5
+Loop_cbc_dec8x:
+	vncipher	$out0,$out0,v24
+	vncipher	$out1,$out1,v24
+	vncipher	$out2,$out2,v24
+	vncipher	$out3,$out3,v24
+	vncipher	$out4,$out4,v24
+	vncipher	$out5,$out5,v24
+	vncipher	$out6,$out6,v24
+	vncipher	$out7,$out7,v24
+	lvx		v24,$x20,$key_		# round[3]
+	addi		$key_,$key_,0x20
+
+	vncipher	$out0,$out0,v25
+	vncipher	$out1,$out1,v25
+	vncipher	$out2,$out2,v25
+	vncipher	$out3,$out3,v25
+	vncipher	$out4,$out4,v25
+	vncipher	$out5,$out5,v25
+	vncipher	$out6,$out6,v25
+	vncipher	$out7,$out7,v25
+	lvx		v25,$x10,$key_		# round[4]
+	bdnz		Loop_cbc_dec8x
+
+	subic		$len,$len,128		# $len-=128
+	vncipher	$out0,$out0,v24
+	vncipher	$out1,$out1,v24
+	vncipher	$out2,$out2,v24
+	vncipher	$out3,$out3,v24
+	vncipher	$out4,$out4,v24
+	vncipher	$out5,$out5,v24
+	vncipher	$out6,$out6,v24
+	vncipher	$out7,$out7,v24
+
+	subfe.		r0,r0,r0		# borrow?-1:0
+	vncipher	$out0,$out0,v25
+	vncipher	$out1,$out1,v25
+	vncipher	$out2,$out2,v25
+	vncipher	$out3,$out3,v25
+	vncipher	$out4,$out4,v25
+	vncipher	$out5,$out5,v25
+	vncipher	$out6,$out6,v25
+	vncipher	$out7,$out7,v25
+
+	and		r0,r0,$len
+	vncipher	$out0,$out0,v26
+	vncipher	$out1,$out1,v26
+	vncipher	$out2,$out2,v26
+	vncipher	$out3,$out3,v26
+	vncipher	$out4,$out4,v26
+	vncipher	$out5,$out5,v26
+	vncipher	$out6,$out6,v26
+	vncipher	$out7,$out7,v26
+
+	add		$inp,$inp,r0		# $inp is adjusted in such
+						# way that at exit from the
+						# loop inX-in7 are loaded
+						# with last "words"
+	vncipher	$out0,$out0,v27
+	vncipher	$out1,$out1,v27
+	vncipher	$out2,$out2,v27
+	vncipher	$out3,$out3,v27
+	vncipher	$out4,$out4,v27
+	vncipher	$out5,$out5,v27
+	vncipher	$out6,$out6,v27
+	vncipher	$out7,$out7,v27
+
+	addi		$key_,$sp,$FRAME+15	# rewind $key_
+	vncipher	$out0,$out0,v28
+	vncipher	$out1,$out1,v28
+	vncipher	$out2,$out2,v28
+	vncipher	$out3,$out3,v28
+	vncipher	$out4,$out4,v28
+	vncipher	$out5,$out5,v28
+	vncipher	$out6,$out6,v28
+	vncipher	$out7,$out7,v28
+	lvx		v24,$x00,$key_		# re-pre-load round[1]
+
+	vncipher	$out0,$out0,v29
+	vncipher	$out1,$out1,v29
+	vncipher	$out2,$out2,v29
+	vncipher	$out3,$out3,v29
+	vncipher	$out4,$out4,v29
+	vncipher	$out5,$out5,v29
+	vncipher	$out6,$out6,v29
+	vncipher	$out7,$out7,v29
+	lvx		v25,$x10,$key_		# re-pre-load round[2]
+
+	vncipher	$out0,$out0,v30
+	 vxor		$ivec,$ivec,v31		# xor with last round key
+	vncipher	$out1,$out1,v30
+	 vxor		$in0,$in0,v31
+	vncipher	$out2,$out2,v30
+	 vxor		$in1,$in1,v31
+	vncipher	$out3,$out3,v30
+	 vxor		$in2,$in2,v31
+	vncipher	$out4,$out4,v30
+	 vxor		$in3,$in3,v31
+	vncipher	$out5,$out5,v30
+	 vxor		$in4,$in4,v31
+	vncipher	$out6,$out6,v30
+	 vxor		$in5,$in5,v31
+	vncipher	$out7,$out7,v30
+	 vxor		$in6,$in6,v31
+
+	vncipherlast	$out0,$out0,$ivec
+	vncipherlast	$out1,$out1,$in0
+	 lvx_u		$in0,$x00,$inp		# load next input block
+	vncipherlast	$out2,$out2,$in1
+	 lvx_u		$in1,$x10,$inp
+	vncipherlast	$out3,$out3,$in2
+	 le?vperm	$in0,$in0,$in0,$inpperm
+	 lvx_u		$in2,$x20,$inp
+	vncipherlast	$out4,$out4,$in3
+	 le?vperm	$in1,$in1,$in1,$inpperm
+	 lvx_u		$in3,$x30,$inp
+	vncipherlast	$out5,$out5,$in4
+	 le?vperm	$in2,$in2,$in2,$inpperm
+	 lvx_u		$in4,$x40,$inp
+	vncipherlast	$out6,$out6,$in5
+	 le?vperm	$in3,$in3,$in3,$inpperm
+	 lvx_u		$in5,$x50,$inp
+	vncipherlast	$out7,$out7,$in6
+	 le?vperm	$in4,$in4,$in4,$inpperm
+	 lvx_u		$in6,$x60,$inp
+	vmr		$ivec,$in7
+	 le?vperm	$in5,$in5,$in5,$inpperm
+	 lvx_u		$in7,$x70,$inp
+	 addi		$inp,$inp,0x80
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	 le?vperm	$in6,$in6,$in6,$inpperm
+	 vxor		$out0,$in0,$rndkey0
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x10,$out
+	 le?vperm	$in7,$in7,$in7,$inpperm
+	 vxor		$out1,$in1,$rndkey0
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x20,$out
+	 vxor		$out2,$in2,$rndkey0
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x30,$out
+	 vxor		$out3,$in3,$rndkey0
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x40,$out
+	 vxor		$out4,$in4,$rndkey0
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x50,$out
+	 vxor		$out5,$in5,$rndkey0
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x60,$out
+	 vxor		$out6,$in6,$rndkey0
+	stvx_u		$out7,$x70,$out
+	addi		$out,$out,0x80
+	 vxor		$out7,$in7,$rndkey0
+
+	mtctr		$rounds
+	beq		Loop_cbc_dec8x		# did $len-=128 borrow?
+
+	addic.		$len,$len,128
+	beq		Lcbc_dec8x_done
+	nop
+	nop
+
+Loop_cbc_dec8x_tail:				# up to 7 "words" tail...
+	vncipher	$out1,$out1,v24
+	vncipher	$out2,$out2,v24
+	vncipher	$out3,$out3,v24
+	vncipher	$out4,$out4,v24
+	vncipher	$out5,$out5,v24
+	vncipher	$out6,$out6,v24
+	vncipher	$out7,$out7,v24
+	lvx		v24,$x20,$key_		# round[3]
+	addi		$key_,$key_,0x20
+
+	vncipher	$out1,$out1,v25
+	vncipher	$out2,$out2,v25
+	vncipher	$out3,$out3,v25
+	vncipher	$out4,$out4,v25
+	vncipher	$out5,$out5,v25
+	vncipher	$out6,$out6,v25
+	vncipher	$out7,$out7,v25
+	lvx		v25,$x10,$key_		# round[4]
+	bdnz		Loop_cbc_dec8x_tail
+
+	vncipher	$out1,$out1,v24
+	vncipher	$out2,$out2,v24
+	vncipher	$out3,$out3,v24
+	vncipher	$out4,$out4,v24
+	vncipher	$out5,$out5,v24
+	vncipher	$out6,$out6,v24
+	vncipher	$out7,$out7,v24
+
+	vncipher	$out1,$out1,v25
+	vncipher	$out2,$out2,v25
+	vncipher	$out3,$out3,v25
+	vncipher	$out4,$out4,v25
+	vncipher	$out5,$out5,v25
+	vncipher	$out6,$out6,v25
+	vncipher	$out7,$out7,v25
+
+	vncipher	$out1,$out1,v26
+	vncipher	$out2,$out2,v26
+	vncipher	$out3,$out3,v26
+	vncipher	$out4,$out4,v26
+	vncipher	$out5,$out5,v26
+	vncipher	$out6,$out6,v26
+	vncipher	$out7,$out7,v26
+
+	vncipher	$out1,$out1,v27
+	vncipher	$out2,$out2,v27
+	vncipher	$out3,$out3,v27
+	vncipher	$out4,$out4,v27
+	vncipher	$out5,$out5,v27
+	vncipher	$out6,$out6,v27
+	vncipher	$out7,$out7,v27
+
+	vncipher	$out1,$out1,v28
+	vncipher	$out2,$out2,v28
+	vncipher	$out3,$out3,v28
+	vncipher	$out4,$out4,v28
+	vncipher	$out5,$out5,v28
+	vncipher	$out6,$out6,v28
+	vncipher	$out7,$out7,v28
+
+	vncipher	$out1,$out1,v29
+	vncipher	$out2,$out2,v29
+	vncipher	$out3,$out3,v29
+	vncipher	$out4,$out4,v29
+	vncipher	$out5,$out5,v29
+	vncipher	$out6,$out6,v29
+	vncipher	$out7,$out7,v29
+
+	vncipher	$out1,$out1,v30
+	 vxor		$ivec,$ivec,v31		# last round key
+	vncipher	$out2,$out2,v30
+	 vxor		$in1,$in1,v31
+	vncipher	$out3,$out3,v30
+	 vxor		$in2,$in2,v31
+	vncipher	$out4,$out4,v30
+	 vxor		$in3,$in3,v31
+	vncipher	$out5,$out5,v30
+	 vxor		$in4,$in4,v31
+	vncipher	$out6,$out6,v30
+	 vxor		$in5,$in5,v31
+	vncipher	$out7,$out7,v30
+	 vxor		$in6,$in6,v31
+
+	cmplwi		$len,32			# switch($len)
+	blt		Lcbc_dec8x_one
+	nop
+	beq		Lcbc_dec8x_two
+	cmplwi		$len,64
+	blt		Lcbc_dec8x_three
+	nop
+	beq		Lcbc_dec8x_four
+	cmplwi		$len,96
+	blt		Lcbc_dec8x_five
+	nop
+	beq		Lcbc_dec8x_six
+
+Lcbc_dec8x_seven:
+	vncipherlast	$out1,$out1,$ivec
+	vncipherlast	$out2,$out2,$in1
+	vncipherlast	$out3,$out3,$in2
+	vncipherlast	$out4,$out4,$in3
+	vncipherlast	$out5,$out5,$in4
+	vncipherlast	$out6,$out6,$in5
+	vncipherlast	$out7,$out7,$in6
+	vmr		$ivec,$in7
+
+	le?vperm	$out1,$out1,$out1,$inpperm
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x00,$out
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x10,$out
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x20,$out
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x30,$out
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x40,$out
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x50,$out
+	stvx_u		$out7,$x60,$out
+	addi		$out,$out,0x70
+	b		Lcbc_dec8x_done
+
+.align	5
+Lcbc_dec8x_six:
+	vncipherlast	$out2,$out2,$ivec
+	vncipherlast	$out3,$out3,$in2
+	vncipherlast	$out4,$out4,$in3
+	vncipherlast	$out5,$out5,$in4
+	vncipherlast	$out6,$out6,$in5
+	vncipherlast	$out7,$out7,$in6
+	vmr		$ivec,$in7
+
+	le?vperm	$out2,$out2,$out2,$inpperm
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x00,$out
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x10,$out
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x20,$out
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x30,$out
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x40,$out
+	stvx_u		$out7,$x50,$out
+	addi		$out,$out,0x60
+	b		Lcbc_dec8x_done
+
+.align	5
+Lcbc_dec8x_five:
+	vncipherlast	$out3,$out3,$ivec
+	vncipherlast	$out4,$out4,$in3
+	vncipherlast	$out5,$out5,$in4
+	vncipherlast	$out6,$out6,$in5
+	vncipherlast	$out7,$out7,$in6
+	vmr		$ivec,$in7
+
+	le?vperm	$out3,$out3,$out3,$inpperm
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x00,$out
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x10,$out
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x20,$out
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x30,$out
+	stvx_u		$out7,$x40,$out
+	addi		$out,$out,0x50
+	b		Lcbc_dec8x_done
+
+.align	5
+Lcbc_dec8x_four:
+	vncipherlast	$out4,$out4,$ivec
+	vncipherlast	$out5,$out5,$in4
+	vncipherlast	$out6,$out6,$in5
+	vncipherlast	$out7,$out7,$in6
+	vmr		$ivec,$in7
+
+	le?vperm	$out4,$out4,$out4,$inpperm
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x00,$out
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x10,$out
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x20,$out
+	stvx_u		$out7,$x30,$out
+	addi		$out,$out,0x40
+	b		Lcbc_dec8x_done
+
+.align	5
+Lcbc_dec8x_three:
+	vncipherlast	$out5,$out5,$ivec
+	vncipherlast	$out6,$out6,$in5
+	vncipherlast	$out7,$out7,$in6
+	vmr		$ivec,$in7
+
+	le?vperm	$out5,$out5,$out5,$inpperm
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x00,$out
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x10,$out
+	stvx_u		$out7,$x20,$out
+	addi		$out,$out,0x30
+	b		Lcbc_dec8x_done
+
+.align	5
+Lcbc_dec8x_two:
+	vncipherlast	$out6,$out6,$ivec
+	vncipherlast	$out7,$out7,$in6
+	vmr		$ivec,$in7
+
+	le?vperm	$out6,$out6,$out6,$inpperm
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x00,$out
+	stvx_u		$out7,$x10,$out
+	addi		$out,$out,0x20
+	b		Lcbc_dec8x_done
+
+.align	5
+Lcbc_dec8x_one:
+	vncipherlast	$out7,$out7,$ivec
+	vmr		$ivec,$in7
+
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out7,0,$out
+	addi		$out,$out,0x10
+
+Lcbc_dec8x_done:
+	le?vperm	$ivec,$ivec,$ivec,$inpperm
+	stvx_u		$ivec,0,$ivp		# write [unaligned] iv
+
+	li		r10,`$FRAME+15`
+	li		r11,`$FRAME+31`
+	stvx		$inpperm,r10,$sp	# wipe copies of round keys
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+	stvx		$inpperm,r10,$sp
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+	stvx		$inpperm,r10,$sp
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+	stvx		$inpperm,r10,$sp
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+
+	mtspr		256,$vrsave
+	lvx		v20,r10,$sp		# ABI says so
+	addi		r10,r10,32
+	lvx		v21,r11,$sp
+	addi		r11,r11,32
+	lvx		v22,r10,$sp
+	addi		r10,r10,32
+	lvx		v23,r11,$sp
+	addi		r11,r11,32
+	lvx		v24,r10,$sp
+	addi		r10,r10,32
+	lvx		v25,r11,$sp
+	addi		r11,r11,32
+	lvx		v26,r10,$sp
+	addi		r10,r10,32
+	lvx		v27,r11,$sp
+	addi		r11,r11,32
+	lvx		v28,r10,$sp
+	addi		r10,r10,32
+	lvx		v29,r11,$sp
+	addi		r11,r11,32
+	lvx		v30,r10,$sp
+	lvx		v31,r11,$sp
+	$POP		r26,`$FRAME+21*16+0*$SIZE_T`($sp)
+	$POP		r27,`$FRAME+21*16+1*$SIZE_T`($sp)
+	$POP		r28,`$FRAME+21*16+2*$SIZE_T`($sp)
+	$POP		r29,`$FRAME+21*16+3*$SIZE_T`($sp)
+	$POP		r30,`$FRAME+21*16+4*$SIZE_T`($sp)
+	$POP		r31,`$FRAME+21*16+5*$SIZE_T`($sp)
+	addi		$sp,$sp,`$FRAME+21*16+6*$SIZE_T`
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0x80,6,6,0
+	.long		0
+.size	.${prefix}_cbc_encrypt,.-.${prefix}_cbc_encrypt
+___
+}}	}}}
+
+#########################################################################
+{{{	# CTR procedure[s]						#
+my ($inp,$out,$len,$key,$ivp,$x10,$rounds,$idx)=map("r$_",(3..10));
+my ($rndkey0,$rndkey1,$inout,$tmp)=		map("v$_",(0..3));
+my ($ivec,$inptail,$inpperm,$outhead,$outperm,$outmask,$keyperm,$one)=
+						map("v$_",(4..11));
+my $dat=$tmp;
+
+$code.=<<___;
+.globl	.${prefix}_ctr32_encrypt_blocks
+	${UCMP}i	$len,1
+	bltlr-
+
+	lis		r0,0xfff0
+	mfspr		$vrsave,256
+	mtspr		256,r0
+
+	li		$idx,15
+	vxor		$rndkey0,$rndkey0,$rndkey0
+	le?vspltisb	$tmp,0x0f
+
+	lvx		$ivec,0,$ivp		# load [unaligned] iv
+	lvsl		$inpperm,0,$ivp
+	lvx		$inptail,$idx,$ivp
+	 vspltisb	$one,1
+	le?vxor		$inpperm,$inpperm,$tmp
+	vperm		$ivec,$ivec,$inptail,$inpperm
+	 vsldoi		$one,$rndkey0,$one,1
+
+	neg		r11,$inp
+	?lvsl		$keyperm,0,$key		# prepare for unaligned key
+	lwz		$rounds,240($key)
+
+	lvsr		$inpperm,0,r11		# prepare for unaligned load
+	lvx		$inptail,0,$inp
+	addi		$inp,$inp,15		# 15 is not typo
+	le?vxor		$inpperm,$inpperm,$tmp
+
+	srwi		$rounds,$rounds,1
+	li		$idx,16
+	subi		$rounds,$rounds,1
+
+	${UCMP}i	$len,8
+	bge		_aesp8_ctr32_encrypt8x
+
+	?lvsr		$outperm,0,$out		# prepare for unaligned store
+	vspltisb	$outmask,-1
+	lvx		$outhead,0,$out
+	?vperm		$outmask,$rndkey0,$outmask,$outperm
+	le?vxor		$outperm,$outperm,$tmp
+
+	lvx		$rndkey0,0,$key
+	mtctr		$rounds
+	lvx		$rndkey1,$idx,$key
+	addi		$idx,$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vxor		$inout,$ivec,$rndkey0
+	lvx		$rndkey0,$idx,$key
+	addi		$idx,$idx,16
+	b		Loop_ctr32_enc
+
+.align	5
+Loop_ctr32_enc:
+	?vperm		$rndkey1,$rndkey1,$rndkey0,$keyperm
+	vcipher		$inout,$inout,$rndkey1
+	lvx		$rndkey1,$idx,$key
+	addi		$idx,$idx,16
+	?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vcipher		$inout,$inout,$rndkey0
+	lvx		$rndkey0,$idx,$key
+	addi		$idx,$idx,16
+	bdnz		Loop_ctr32_enc
+
+	vadduwm		$ivec,$ivec,$one
+	 vmr		$dat,$inptail
+	 lvx		$inptail,0,$inp
+	 addi		$inp,$inp,16
+	 subic.		$len,$len,1		# blocks--
+
+	?vperm		$rndkey1,$rndkey1,$rndkey0,$keyperm
+	vcipher		$inout,$inout,$rndkey1
+	lvx		$rndkey1,$idx,$key
+	 vperm		$dat,$dat,$inptail,$inpperm
+	 li		$idx,16
+	?vperm		$rndkey1,$rndkey0,$rndkey1,$keyperm
+	 lvx		$rndkey0,0,$key
+	vxor		$dat,$dat,$rndkey1	# last round key
+	vcipherlast	$inout,$inout,$dat
+
+	 lvx		$rndkey1,$idx,$key
+	 addi		$idx,$idx,16
+	vperm		$inout,$inout,$inout,$outperm
+	vsel		$dat,$outhead,$inout,$outmask
+	 mtctr		$rounds
+	 ?vperm		$rndkey0,$rndkey0,$rndkey1,$keyperm
+	vmr		$outhead,$inout
+	 vxor		$inout,$ivec,$rndkey0
+	 lvx		$rndkey0,$idx,$key
+	 addi		$idx,$idx,16
+	stvx		$dat,0,$out
+	addi		$out,$out,16
+	bne		Loop_ctr32_enc
+
+	addi		$out,$out,-1
+	lvx		$inout,0,$out		# redundant in aligned case
+	vsel		$inout,$outhead,$inout,$outmask
+	stvx		$inout,0,$out
+
+	mtspr		256,$vrsave
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0,0,6,0
+	.long		0
+___
+#########################################################################
+{{	# Optimized CTR procedure					#
+my $key_="r11";
+my ($x00,$x10,$x20,$x30,$x40,$x50,$x60,$x70)=map("r$_",(0,8,26..31));
+my ($in0, $in1, $in2, $in3, $in4, $in5, $in6, $in7 )=map("v$_",(0..3,10,12..14));
+my ($out0,$out1,$out2,$out3,$out4,$out5,$out6,$out7)=map("v$_",(15..22));
+my $rndkey0="v23";	# v24-v25 rotating buffer for first found keys
+			# v26-v31 last 6 round keys
+my ($tmp,$keyperm)=($in3,$in4);	# aliases with "caller", redundant assignment
+my ($two,$three,$four)=($outhead,$outperm,$outmask);
+
+$code.=<<___;
+.align	5
+_aesp8_ctr32_encrypt8x:
+	$STU		$sp,-`($FRAME+21*16+6*$SIZE_T)`($sp)
+	li		r10,`$FRAME+8*16+15`
+	li		r11,`$FRAME+8*16+31`
+	stvx		v20,r10,$sp		# ABI says so
+	addi		r10,r10,32
+	stvx		v21,r11,$sp
+	addi		r11,r11,32
+	stvx		v22,r10,$sp
+	addi		r10,r10,32
+	stvx		v23,r11,$sp
+	addi		r11,r11,32
+	stvx		v24,r10,$sp
+	addi		r10,r10,32
+	stvx		v25,r11,$sp
+	addi		r11,r11,32
+	stvx		v26,r10,$sp
+	addi		r10,r10,32
+	stvx		v27,r11,$sp
+	addi		r11,r11,32
+	stvx		v28,r10,$sp
+	addi		r10,r10,32
+	stvx		v29,r11,$sp
+	addi		r11,r11,32
+	stvx		v30,r10,$sp
+	stvx		v31,r11,$sp
+	li		r0,-1
+	stw		$vrsave,`$FRAME+21*16-4`($sp)	# save vrsave
+	li		$x10,0x10
+	$PUSH		r26,`$FRAME+21*16+0*$SIZE_T`($sp)
+	li		$x20,0x20
+	$PUSH		r27,`$FRAME+21*16+1*$SIZE_T`($sp)
+	li		$x30,0x30
+	$PUSH		r28,`$FRAME+21*16+2*$SIZE_T`($sp)
+	li		$x40,0x40
+	$PUSH		r29,`$FRAME+21*16+3*$SIZE_T`($sp)
+	li		$x50,0x50
+	$PUSH		r30,`$FRAME+21*16+4*$SIZE_T`($sp)
+	li		$x60,0x60
+	$PUSH		r31,`$FRAME+21*16+5*$SIZE_T`($sp)
+	li		$x70,0x70
+	mtspr		256,r0
+
+	subi		$rounds,$rounds,3	# -4 in total
+
+	lvx		$rndkey0,$x00,$key	# load key schedule
+	lvx		v30,$x10,$key
+	addi		$key,$key,0x20
+	lvx		v31,$x00,$key
+	?vperm		$rndkey0,$rndkey0,v30,$keyperm
+	addi		$key_,$sp,$FRAME+15
+	mtctr		$rounds
+
+Load_ctr32_enc_key:
+	?vperm		v24,v30,v31,$keyperm
+	lvx		v30,$x10,$key
+	addi		$key,$key,0x20
+	stvx		v24,$x00,$key_		# off-load round[1]
+	?vperm		v25,v31,v30,$keyperm
+	lvx		v31,$x00,$key
+	stvx		v25,$x10,$key_		# off-load round[2]
+	addi		$key_,$key_,0x20
+	bdnz		Load_ctr32_enc_key
+
+	lvx		v26,$x10,$key
+	?vperm		v24,v30,v31,$keyperm
+	lvx		v27,$x20,$key
+	stvx		v24,$x00,$key_		# off-load round[3]
+	?vperm		v25,v31,v26,$keyperm
+	lvx		v28,$x30,$key
+	stvx		v25,$x10,$key_		# off-load round[4]
+	addi		$key_,$sp,$FRAME+15	# rewind $key_
+	?vperm		v26,v26,v27,$keyperm
+	lvx		v29,$x40,$key
+	?vperm		v27,v27,v28,$keyperm
+	lvx		v30,$x50,$key
+	?vperm		v28,v28,v29,$keyperm
+	lvx		v31,$x60,$key
+	?vperm		v29,v29,v30,$keyperm
+	lvx		$out0,$x70,$key		# borrow $out0
+	?vperm		v30,v30,v31,$keyperm
+	lvx		v24,$x00,$key_		# pre-load round[1]
+	?vperm		v31,v31,$out0,$keyperm
+	lvx		v25,$x10,$key_		# pre-load round[2]
+
+	vadduwm		$two,$one,$one
+	subi		$inp,$inp,15		# undo "caller"
+	$SHL		$len,$len,4
+
+	vadduwm		$out1,$ivec,$one	# counter values ...
+	vadduwm		$out2,$ivec,$two
+	vxor		$out0,$ivec,$rndkey0	# ... xored with rndkey[0]
+	 le?li		$idx,8
+	vadduwm		$out3,$out1,$two
+	vxor		$out1,$out1,$rndkey0
+	 le?lvsl	$inpperm,0,$idx
+	vadduwm		$out4,$out2,$two
+	vxor		$out2,$out2,$rndkey0
+	 le?vspltisb	$tmp,0x0f
+	vadduwm		$out5,$out3,$two
+	vxor		$out3,$out3,$rndkey0
+	 le?vxor	$inpperm,$inpperm,$tmp	# transform for lvx_u/stvx_u
+	vadduwm		$out6,$out4,$two
+	vxor		$out4,$out4,$rndkey0
+	vadduwm		$out7,$out5,$two
+	vxor		$out5,$out5,$rndkey0
+	vadduwm		$ivec,$out6,$two	# next counter value
+	vxor		$out6,$out6,$rndkey0
+	vxor		$out7,$out7,$rndkey0
+
+	mtctr		$rounds
+	b		Loop_ctr32_enc8x
+.align	5
+Loop_ctr32_enc8x:
+	vcipher 	$out0,$out0,v24
+	vcipher 	$out1,$out1,v24
+	vcipher 	$out2,$out2,v24
+	vcipher 	$out3,$out3,v24
+	vcipher 	$out4,$out4,v24
+	vcipher 	$out5,$out5,v24
+	vcipher 	$out6,$out6,v24
+	vcipher 	$out7,$out7,v24
+Loop_ctr32_enc8x_middle:
+	lvx		v24,$x20,$key_		# round[3]
+	addi		$key_,$key_,0x20
+
+	vcipher 	$out0,$out0,v25
+	vcipher 	$out1,$out1,v25
+	vcipher 	$out2,$out2,v25
+	vcipher 	$out3,$out3,v25
+	vcipher 	$out4,$out4,v25
+	vcipher 	$out5,$out5,v25
+	vcipher 	$out6,$out6,v25
+	vcipher 	$out7,$out7,v25
+	lvx		v25,$x10,$key_		# round[4]
+	bdnz		Loop_ctr32_enc8x
+
+	subic		r11,$len,256		# $len-256, borrow $key_
+	vcipher 	$out0,$out0,v24
+	vcipher 	$out1,$out1,v24
+	vcipher 	$out2,$out2,v24
+	vcipher 	$out3,$out3,v24
+	vcipher 	$out4,$out4,v24
+	vcipher 	$out5,$out5,v24
+	vcipher 	$out6,$out6,v24
+	vcipher 	$out7,$out7,v24
+
+	subfe		r0,r0,r0		# borrow?-1:0
+	vcipher 	$out0,$out0,v25
+	vcipher 	$out1,$out1,v25
+	vcipher 	$out2,$out2,v25
+	vcipher 	$out3,$out3,v25
+	vcipher 	$out4,$out4,v25
+	vcipher		$out5,$out5,v25
+	vcipher		$out6,$out6,v25
+	vcipher		$out7,$out7,v25
+
+	and		r0,r0,r11
+	addi		$key_,$sp,$FRAME+15	# rewind $key_
+	vcipher		$out0,$out0,v26
+	vcipher		$out1,$out1,v26
+	vcipher		$out2,$out2,v26
+	vcipher		$out3,$out3,v26
+	vcipher		$out4,$out4,v26
+	vcipher		$out5,$out5,v26
+	vcipher		$out6,$out6,v26
+	vcipher		$out7,$out7,v26
+	lvx		v24,$x00,$key_		# re-pre-load round[1]
+
+	subic		$len,$len,129		# $len-=129
+	vcipher		$out0,$out0,v27
+	addi		$len,$len,1		# $len-=128 really
+	vcipher		$out1,$out1,v27
+	vcipher		$out2,$out2,v27
+	vcipher		$out3,$out3,v27
+	vcipher		$out4,$out4,v27
+	vcipher		$out5,$out5,v27
+	vcipher		$out6,$out6,v27
+	vcipher		$out7,$out7,v27
+	lvx		v25,$x10,$key_		# re-pre-load round[2]
+
+	vcipher		$out0,$out0,v28
+	 lvx_u		$in0,$x00,$inp		# load input
+	vcipher		$out1,$out1,v28
+	 lvx_u		$in1,$x10,$inp
+	vcipher		$out2,$out2,v28
+	 lvx_u		$in2,$x20,$inp
+	vcipher		$out3,$out3,v28
+	 lvx_u		$in3,$x30,$inp
+	vcipher		$out4,$out4,v28
+	 lvx_u		$in4,$x40,$inp
+	vcipher		$out5,$out5,v28
+	 lvx_u		$in5,$x50,$inp
+	vcipher		$out6,$out6,v28
+	 lvx_u		$in6,$x60,$inp
+	vcipher		$out7,$out7,v28
+	 lvx_u		$in7,$x70,$inp
+	 addi		$inp,$inp,0x80
+
+	vcipher		$out0,$out0,v29
+	 le?vperm	$in0,$in0,$in0,$inpperm
+	vcipher		$out1,$out1,v29
+	 le?vperm	$in1,$in1,$in1,$inpperm
+	vcipher		$out2,$out2,v29
+	 le?vperm	$in2,$in2,$in2,$inpperm
+	vcipher		$out3,$out3,v29
+	 le?vperm	$in3,$in3,$in3,$inpperm
+	vcipher		$out4,$out4,v29
+	 le?vperm	$in4,$in4,$in4,$inpperm
+	vcipher		$out5,$out5,v29
+	 le?vperm	$in5,$in5,$in5,$inpperm
+	vcipher		$out6,$out6,v29
+	 le?vperm	$in6,$in6,$in6,$inpperm
+	vcipher		$out7,$out7,v29
+	 le?vperm	$in7,$in7,$in7,$inpperm
+
+	add		$inp,$inp,r0		# $inp is adjusted in such
+						# way that at exit from the
+						# loop inX-in7 are loaded
+						# with last "words"
+	subfe.		r0,r0,r0		# borrow?-1:0
+	vcipher		$out0,$out0,v30
+	 vxor		$in0,$in0,v31		# xor with last round key
+	vcipher		$out1,$out1,v30
+	 vxor		$in1,$in1,v31
+	vcipher		$out2,$out2,v30
+	 vxor		$in2,$in2,v31
+	vcipher		$out3,$out3,v30
+	 vxor		$in3,$in3,v31
+	vcipher		$out4,$out4,v30
+	 vxor		$in4,$in4,v31
+	vcipher		$out5,$out5,v30
+	 vxor		$in5,$in5,v31
+	vcipher		$out6,$out6,v30
+	 vxor		$in6,$in6,v31
+	vcipher		$out7,$out7,v30
+	 vxor		$in7,$in7,v31
+
+	bne		Lctr32_enc8x_break	# did $len-129 borrow?
+
+	vcipherlast	$in0,$out0,$in0
+	vcipherlast	$in1,$out1,$in1
+	 vadduwm	$out1,$ivec,$one	# counter values ...
+	vcipherlast	$in2,$out2,$in2
+	 vadduwm	$out2,$ivec,$two
+	 vxor		$out0,$ivec,$rndkey0	# ... xored with rndkey[0]
+	vcipherlast	$in3,$out3,$in3
+	 vadduwm	$out3,$out1,$two
+	 vxor		$out1,$out1,$rndkey0
+	vcipherlast	$in4,$out4,$in4
+	 vadduwm	$out4,$out2,$two
+	 vxor		$out2,$out2,$rndkey0
+	vcipherlast	$in5,$out5,$in5
+	 vadduwm	$out5,$out3,$two
+	 vxor		$out3,$out3,$rndkey0
+	vcipherlast	$in6,$out6,$in6
+	 vadduwm	$out6,$out4,$two
+	 vxor		$out4,$out4,$rndkey0
+	vcipherlast	$in7,$out7,$in7
+	 vadduwm	$out7,$out5,$two
+	 vxor		$out5,$out5,$rndkey0
+	le?vperm	$in0,$in0,$in0,$inpperm
+	 vadduwm	$ivec,$out6,$two	# next counter value
+	 vxor		$out6,$out6,$rndkey0
+	le?vperm	$in1,$in1,$in1,$inpperm
+	 vxor		$out7,$out7,$rndkey0
+	mtctr		$rounds
+
+	 vcipher	$out0,$out0,v24
+	stvx_u		$in0,$x00,$out
+	le?vperm	$in2,$in2,$in2,$inpperm
+	 vcipher	$out1,$out1,v24
+	stvx_u		$in1,$x10,$out
+	le?vperm	$in3,$in3,$in3,$inpperm
+	 vcipher	$out2,$out2,v24
+	stvx_u		$in2,$x20,$out
+	le?vperm	$in4,$in4,$in4,$inpperm
+	 vcipher	$out3,$out3,v24
+	stvx_u		$in3,$x30,$out
+	le?vperm	$in5,$in5,$in5,$inpperm
+	 vcipher	$out4,$out4,v24
+	stvx_u		$in4,$x40,$out
+	le?vperm	$in6,$in6,$in6,$inpperm
+	 vcipher	$out5,$out5,v24
+	stvx_u		$in5,$x50,$out
+	le?vperm	$in7,$in7,$in7,$inpperm
+	 vcipher	$out6,$out6,v24
+	stvx_u		$in6,$x60,$out
+	 vcipher	$out7,$out7,v24
+	stvx_u		$in7,$x70,$out
+	addi		$out,$out,0x80
+
+	b		Loop_ctr32_enc8x_middle
+
+.align	5
+Lctr32_enc8x_break:
+	cmpwi		$len,-0x60
+	blt		Lctr32_enc8x_one
+	nop
+	beq		Lctr32_enc8x_two
+	cmpwi		$len,-0x40
+	blt		Lctr32_enc8x_three
+	nop
+	beq		Lctr32_enc8x_four
+	cmpwi		$len,-0x20
+	blt		Lctr32_enc8x_five
+	nop
+	beq		Lctr32_enc8x_six
+	cmpwi		$len,0x00
+	blt		Lctr32_enc8x_seven
+
+Lctr32_enc8x_eight:
+	vcipherlast	$out0,$out0,$in0
+	vcipherlast	$out1,$out1,$in1
+	vcipherlast	$out2,$out2,$in2
+	vcipherlast	$out3,$out3,$in3
+	vcipherlast	$out4,$out4,$in4
+	vcipherlast	$out5,$out5,$in5
+	vcipherlast	$out6,$out6,$in6
+	vcipherlast	$out7,$out7,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x10,$out
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x20,$out
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x30,$out
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x40,$out
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x50,$out
+	le?vperm	$out7,$out7,$out7,$inpperm
+	stvx_u		$out6,$x60,$out
+	stvx_u		$out7,$x70,$out
+	addi		$out,$out,0x80
+	b		Lctr32_enc8x_done
+
+.align	5
+Lctr32_enc8x_seven:
+	vcipherlast	$out0,$out0,$in1
+	vcipherlast	$out1,$out1,$in2
+	vcipherlast	$out2,$out2,$in3
+	vcipherlast	$out3,$out3,$in4
+	vcipherlast	$out4,$out4,$in5
+	vcipherlast	$out5,$out5,$in6
+	vcipherlast	$out6,$out6,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x10,$out
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x20,$out
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x30,$out
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x40,$out
+	le?vperm	$out6,$out6,$out6,$inpperm
+	stvx_u		$out5,$x50,$out
+	stvx_u		$out6,$x60,$out
+	addi		$out,$out,0x70
+	b		Lctr32_enc8x_done
+
+.align	5
+Lctr32_enc8x_six:
+	vcipherlast	$out0,$out0,$in2
+	vcipherlast	$out1,$out1,$in3
+	vcipherlast	$out2,$out2,$in4
+	vcipherlast	$out3,$out3,$in5
+	vcipherlast	$out4,$out4,$in6
+	vcipherlast	$out5,$out5,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x10,$out
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x20,$out
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x30,$out
+	le?vperm	$out5,$out5,$out5,$inpperm
+	stvx_u		$out4,$x40,$out
+	stvx_u		$out5,$x50,$out
+	addi		$out,$out,0x60
+	b		Lctr32_enc8x_done
+
+.align	5
+Lctr32_enc8x_five:
+	vcipherlast	$out0,$out0,$in3
+	vcipherlast	$out1,$out1,$in4
+	vcipherlast	$out2,$out2,$in5
+	vcipherlast	$out3,$out3,$in6
+	vcipherlast	$out4,$out4,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x10,$out
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x20,$out
+	le?vperm	$out4,$out4,$out4,$inpperm
+	stvx_u		$out3,$x30,$out
+	stvx_u		$out4,$x40,$out
+	addi		$out,$out,0x50
+	b		Lctr32_enc8x_done
+
+.align	5
+Lctr32_enc8x_four:
+	vcipherlast	$out0,$out0,$in4
+	vcipherlast	$out1,$out1,$in5
+	vcipherlast	$out2,$out2,$in6
+	vcipherlast	$out3,$out3,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x10,$out
+	le?vperm	$out3,$out3,$out3,$inpperm
+	stvx_u		$out2,$x20,$out
+	stvx_u		$out3,$x30,$out
+	addi		$out,$out,0x40
+	b		Lctr32_enc8x_done
+
+.align	5
+Lctr32_enc8x_three:
+	vcipherlast	$out0,$out0,$in5
+	vcipherlast	$out1,$out1,$in6
+	vcipherlast	$out2,$out2,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	le?vperm	$out2,$out2,$out2,$inpperm
+	stvx_u		$out1,$x10,$out
+	stvx_u		$out2,$x20,$out
+	addi		$out,$out,0x30
+	b		Lcbc_dec8x_done
+
+.align	5
+Lctr32_enc8x_two:
+	vcipherlast	$out0,$out0,$in6
+	vcipherlast	$out1,$out1,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	le?vperm	$out1,$out1,$out1,$inpperm
+	stvx_u		$out0,$x00,$out
+	stvx_u		$out1,$x10,$out
+	addi		$out,$out,0x20
+	b		Lcbc_dec8x_done
+
+.align	5
+Lctr32_enc8x_one:
+	vcipherlast	$out0,$out0,$in7
+
+	le?vperm	$out0,$out0,$out0,$inpperm
+	stvx_u		$out0,0,$out
+	addi		$out,$out,0x10
+
+Lctr32_enc8x_done:
+	li		r10,`$FRAME+15`
+	li		r11,`$FRAME+31`
+	stvx		$inpperm,r10,$sp	# wipe copies of round keys
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+	stvx		$inpperm,r10,$sp
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+	stvx		$inpperm,r10,$sp
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+	stvx		$inpperm,r10,$sp
+	addi		r10,r10,32
+	stvx		$inpperm,r11,$sp
+	addi		r11,r11,32
+
+	mtspr		256,$vrsave
+	lvx		v20,r10,$sp		# ABI says so
+	addi		r10,r10,32
+	lvx		v21,r11,$sp
+	addi		r11,r11,32
+	lvx		v22,r10,$sp
+	addi		r10,r10,32
+	lvx		v23,r11,$sp
+	addi		r11,r11,32
+	lvx		v24,r10,$sp
+	addi		r10,r10,32
+	lvx		v25,r11,$sp
+	addi		r11,r11,32
+	lvx		v26,r10,$sp
+	addi		r10,r10,32
+	lvx		v27,r11,$sp
+	addi		r11,r11,32
+	lvx		v28,r10,$sp
+	addi		r10,r10,32
+	lvx		v29,r11,$sp
+	addi		r11,r11,32
+	lvx		v30,r10,$sp
+	lvx		v31,r11,$sp
+	$POP		r26,`$FRAME+21*16+0*$SIZE_T`($sp)
+	$POP		r27,`$FRAME+21*16+1*$SIZE_T`($sp)
+	$POP		r28,`$FRAME+21*16+2*$SIZE_T`($sp)
+	$POP		r29,`$FRAME+21*16+3*$SIZE_T`($sp)
+	$POP		r30,`$FRAME+21*16+4*$SIZE_T`($sp)
+	$POP		r31,`$FRAME+21*16+5*$SIZE_T`($sp)
+	addi		$sp,$sp,`$FRAME+21*16+6*$SIZE_T`
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0x80,6,6,0
+	.long		0
+.size	.${prefix}_ctr32_encrypt_blocks,.-.${prefix}_ctr32_encrypt_blocks
+___
+}}	}}}
+
+my $consts=1;
+foreach(split("\n",$code)) {
+        s/\`([^\`]*)\`/eval($1)/geo;
+
+	# constants table endian-specific conversion
+	if ($consts && m/\.(long|byte)\s+(.+)\s+(\?[a-z]*)$/o) {
+	    my $conv=$3;
+	    my @bytes=();
+
+	    # convert to endian-agnostic format
+	    if ($1 eq "long") {
+	      foreach (split(/,\s*/,$2)) {
+		my $l = /^0/?oct:int;
+		push @bytes,($l>>24)&0xff,($l>>16)&0xff,($l>>8)&0xff,$l&0xff;
+	      }
+	    } else {
+		@bytes = map(/^0/?oct:int,split(/,\s*/,$2));
+	    }
+
+	    # little-endian conversion
+	    if ($flavour =~ /le$/o) {
+		SWITCH: for($conv)  {
+		    /\?inv/ && do   { @bytes=map($_^0xf,@bytes); last; };
+		    /\?rev/ && do   { @bytes=reverse(@bytes);    last; }; 
+		}
+	    }
+
+	    #emit
+	    print ".byte\t",join(',',map (sprintf("0x%02x",$_),@bytes)),"\n";
+	    next;
+	}
+	$consts=0 if (m/Lconsts:/o);	# end of table
+
+	# instructions prefixed with '?' are endian-specific and need
+	# to be adjusted accordingly...
+	if ($flavour =~ /le$/o) {	# little-endian
+	    s/le\?//o		or
+	    s/be\?/#be#/o	or
+	    s/\?lvsr/lvsl/o	or
+	    s/\?lvsl/lvsr/o	or
+	    s/\?(vperm\s+v[0-9]+,\s*)(v[0-9]+,\s*)(v[0-9]+,\s*)(v[0-9]+)/$1$3$2$4/o or
+	    s/\?(vsldoi\s+v[0-9]+,\s*)(v[0-9]+,)\s*(v[0-9]+,\s*)([0-9]+)/$1$3$2 16-$4/o or
+	    s/\?(vspltw\s+v[0-9]+,\s*)(v[0-9]+,)\s*([0-9])/$1$2 3-$3/o;
+	} else {			# big-endian
+	    s/le\?/#le#/o	or
+	    s/be\?//o		or
+	    s/\?([a-z]+)/$1/o;
+	}
+
+        print $_,"\n";
+}
+
+close STDOUT;
diff --git a/drivers/crypto/vmx/ghash.c b/drivers/crypto/vmx/ghash.c
new file mode 100644
index 000000000000..d0ffe277af5c
--- /dev/null
+++ b/drivers/crypto/vmx/ghash.c
@@ -0,0 +1,214 @@
+/**
+ * GHASH routines supporting VMX instructions on the Power 8
+ *
+ * Copyright (C) 2015 International Business Machines Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 only.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ * Author: Marcelo Henrique Cerri <mhcerri@br.ibm.com>
+ */
+
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/crypto.h>
+#include <linux/delay.h>
+#include <linux/hardirq.h>
+#include <asm/switch_to.h>
+#include <crypto/aes.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/internal/hash.h>
+#include <crypto/b128ops.h>
+
+#define IN_INTERRUPT in_interrupt()
+
+#define GHASH_BLOCK_SIZE (16)
+#define GHASH_DIGEST_SIZE (16)
+#define GHASH_KEY_LEN (16)
+
+void gcm_init_p8(u128 htable[16], const u64 Xi[2]);
+void gcm_gmult_p8(u64 Xi[2], const u128 htable[16]);
+void gcm_ghash_p8(u64 Xi[2], const u128 htable[16],
+        const u8 *in,size_t len);
+
+struct p8_ghash_ctx {
+    u128 htable[16];
+    struct crypto_shash *fallback;
+};
+
+struct p8_ghash_desc_ctx {
+    u64 shash[2];
+    u8 buffer[GHASH_DIGEST_SIZE];
+    int bytes;
+    struct shash_desc fallback_desc;
+};
+
+static int p8_ghash_init_tfm(struct crypto_tfm *tfm)
+{
+    const char *alg;
+    struct crypto_shash *fallback;
+    struct crypto_shash *shash_tfm = __crypto_shash_cast(tfm);
+    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (!(alg = crypto_tfm_alg_name(tfm))) {
+        printk(KERN_ERR "Failed to get algorithm name.\n");
+        return -ENOENT;
+    }
+
+    fallback = crypto_alloc_shash(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
+    if (IS_ERR(fallback)) {
+        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
+                alg, PTR_ERR(fallback));
+        return PTR_ERR(fallback);
+    }
+    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+            crypto_tfm_alg_driver_name(crypto_shash_tfm(fallback)));
+
+    crypto_shash_set_flags(fallback,
+            crypto_shash_get_flags((struct crypto_shash *) tfm));
+    ctx->fallback = fallback;
+
+    shash_tfm->descsize = sizeof(struct p8_ghash_desc_ctx)
+        + crypto_shash_descsize(fallback);
+
+    return 0;
+}
+
+static void p8_ghash_exit_tfm(struct crypto_tfm *tfm)
+{
+    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(tfm);
+
+    if (ctx->fallback) {
+        crypto_free_shash(ctx->fallback);
+        ctx->fallback = NULL;
+    }
+}
+
+static int p8_ghash_init(struct shash_desc *desc)
+{
+    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
+    struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
+
+    dctx->bytes = 0;
+    memset(dctx->shash, 0, GHASH_DIGEST_SIZE);
+    dctx->fallback_desc.tfm = ctx->fallback;
+    dctx->fallback_desc.flags = desc->flags;
+    return crypto_shash_init(&dctx->fallback_desc);
+}
+
+static int p8_ghash_setkey(struct crypto_shash *tfm, const u8 *key,
+    unsigned int keylen)
+{
+    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(tfm));
+
+    if (keylen != GHASH_KEY_LEN)
+        return -EINVAL;
+
+    pagefault_disable();
+    enable_kernel_altivec();
+    enable_kernel_fp();
+    gcm_init_p8(ctx->htable, (const u64 *) key);
+    pagefault_enable();
+    return crypto_shash_setkey(ctx->fallback, key, keylen);
+}
+
+static int p8_ghash_update(struct shash_desc *desc,
+        const u8 *src, unsigned int srclen)
+{
+    unsigned int len;
+    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
+    struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
+
+    if (IN_INTERRUPT) {
+        return crypto_shash_update(&dctx->fallback_desc, src, srclen);
+    } else {
+        if (dctx->bytes) {
+            if (dctx->bytes + srclen < GHASH_DIGEST_SIZE) {
+                memcpy(dctx->buffer + dctx->bytes, src, srclen);
+                dctx->bytes += srclen;
+                return 0;
+            }
+            memcpy(dctx->buffer + dctx->bytes, src,
+                    GHASH_DIGEST_SIZE - dctx->bytes);
+            pagefault_disable();
+            enable_kernel_altivec();
+            enable_kernel_fp();
+            gcm_ghash_p8(dctx->shash, ctx->htable, dctx->buffer,
+                    GHASH_DIGEST_SIZE);
+            pagefault_enable();
+            src += GHASH_DIGEST_SIZE - dctx->bytes;
+            srclen -= GHASH_DIGEST_SIZE - dctx->bytes;
+            dctx->bytes = 0;
+        }
+        len = srclen & ~(GHASH_DIGEST_SIZE - 1);
+        if (len) {
+            pagefault_disable();
+            enable_kernel_altivec();
+            enable_kernel_fp();
+            gcm_ghash_p8(dctx->shash, ctx->htable, src, len);
+            pagefault_enable();
+            src += len;
+            srclen -= len;
+        }
+        if (srclen) {
+            memcpy(dctx->buffer, src, srclen);
+            dctx->bytes = srclen;
+        }
+        return 0;
+    }
+}
+
+static int p8_ghash_final(struct shash_desc *desc, u8 *out)
+{
+    int i;
+    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
+    struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
+
+    if (IN_INTERRUPT) {
+        return crypto_shash_final(&dctx->fallback_desc, out);
+    } else {
+        if (dctx->bytes) {
+            for (i = dctx->bytes; i < GHASH_DIGEST_SIZE; i++)
+                dctx->buffer[i] = 0;
+            pagefault_disable();
+            enable_kernel_altivec();
+            enable_kernel_fp();
+            gcm_ghash_p8(dctx->shash, ctx->htable, dctx->buffer,
+                    GHASH_DIGEST_SIZE);
+            pagefault_enable();
+            dctx->bytes = 0;
+        }
+        memcpy(out, dctx->shash, GHASH_DIGEST_SIZE);
+        return 0;
+    }
+}
+
+struct shash_alg p8_ghash_alg = {
+    .digestsize = GHASH_DIGEST_SIZE,
+    .init       = p8_ghash_init,
+    .update     = p8_ghash_update,
+    .final      = p8_ghash_final,
+    .setkey     = p8_ghash_setkey,
+    .descsize   = sizeof(struct p8_ghash_desc_ctx),
+    .base       = {
+        .cra_name = "ghash",
+        .cra_driver_name = "p8_ghash",
+        .cra_priority = 1000,
+        .cra_flags = CRYPTO_ALG_TYPE_SHASH | CRYPTO_ALG_NEED_FALLBACK,
+        .cra_blocksize = GHASH_BLOCK_SIZE,
+        .cra_ctxsize = sizeof(struct p8_ghash_ctx),
+        .cra_module = THIS_MODULE,
+        .cra_init = p8_ghash_init_tfm,
+        .cra_exit = p8_ghash_exit_tfm,
+    },
+};
diff --git a/drivers/crypto/vmx/ghashp8-ppc.pl b/drivers/crypto/vmx/ghashp8-ppc.pl
new file mode 100644
index 000000000000..0a6f899839dd
--- /dev/null
+++ b/drivers/crypto/vmx/ghashp8-ppc.pl
@@ -0,0 +1,228 @@
+#!/usr/bin/env perl
+#
+# ====================================================================
+# Written by Andy Polyakov <appro@openssl.org> for the OpenSSL
+# project. The module is, however, dual licensed under OpenSSL and
+# CRYPTOGAMS licenses depending on where you obtain it. For further
+# details see http://www.openssl.org/~appro/cryptogams/.
+# ====================================================================
+#
+# GHASH for for PowerISA v2.07.
+#
+# July 2014
+#
+# Accurate performance measurements are problematic, because it's
+# always virtualized setup with possibly throttled processor.
+# Relative comparison is therefore more informative. This initial
+# version is ~2.1x slower than hardware-assisted AES-128-CTR, ~12x
+# faster than "4-bit" integer-only compiler-generated 64-bit code.
+# "Initial version" means that there is room for futher improvement.
+
+$flavour=shift;
+$output =shift;
+
+if ($flavour =~ /64/) {
+	$SIZE_T=8;
+	$LRSAVE=2*$SIZE_T;
+	$STU="stdu";
+	$POP="ld";
+	$PUSH="std";
+} elsif ($flavour =~ /32/) {
+	$SIZE_T=4;
+	$LRSAVE=$SIZE_T;
+	$STU="stwu";
+	$POP="lwz";
+	$PUSH="stw";
+} else { die "nonsense $flavour"; }
+
+$0 =~ m/(.*[\/\\])[^\/\\]+$/; $dir=$1;
+( $xlate="${dir}ppc-xlate.pl" and -f $xlate ) or
+( $xlate="${dir}../../perlasm/ppc-xlate.pl" and -f $xlate) or
+die "can't locate ppc-xlate.pl";
+
+open STDOUT,"| $^X $xlate $flavour $output" || die "can't call $xlate: $!";
+
+my ($Xip,$Htbl,$inp,$len)=map("r$_",(3..6));	# argument block
+
+my ($Xl,$Xm,$Xh,$IN)=map("v$_",(0..3));
+my ($zero,$t0,$t1,$t2,$xC2,$H,$Hh,$Hl,$lemask)=map("v$_",(4..12));
+my $vrsave="r12";
+
+$code=<<___;
+.machine	"any"
+
+.text
+
+.globl	.gcm_init_p8
+	lis		r0,0xfff0
+	li		r8,0x10
+	mfspr		$vrsave,256
+	li		r9,0x20
+	mtspr		256,r0
+	li		r10,0x30
+	lvx_u		$H,0,r4			# load H
+
+	vspltisb	$xC2,-16		# 0xf0
+	vspltisb	$t0,1			# one
+	vaddubm		$xC2,$xC2,$xC2		# 0xe0
+	vxor		$zero,$zero,$zero
+	vor		$xC2,$xC2,$t0		# 0xe1
+	vsldoi		$xC2,$xC2,$zero,15	# 0xe1...
+	vsldoi		$t1,$zero,$t0,1		# ...1
+	vaddubm		$xC2,$xC2,$xC2		# 0xc2...
+	vspltisb	$t2,7
+	vor		$xC2,$xC2,$t1		# 0xc2....01
+	vspltb		$t1,$H,0		# most significant byte
+	vsl		$H,$H,$t0		# H<<=1
+	vsrab		$t1,$t1,$t2		# broadcast carry bit
+	vand		$t1,$t1,$xC2
+	vxor		$H,$H,$t1		# twisted H
+
+	vsldoi		$H,$H,$H,8		# twist even more ...
+	vsldoi		$xC2,$zero,$xC2,8	# 0xc2.0
+	vsldoi		$Hl,$zero,$H,8		# ... and split
+	vsldoi		$Hh,$H,$zero,8
+
+	stvx_u		$xC2,0,r3		# save pre-computed table
+	stvx_u		$Hl,r8,r3
+	stvx_u		$H, r9,r3
+	stvx_u		$Hh,r10,r3
+
+	mtspr		256,$vrsave
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0,0,2,0
+	.long		0
+.size	.gcm_init_p8,.-.gcm_init_p8
+
+.globl	.gcm_gmult_p8
+	lis		r0,0xfff8
+	li		r8,0x10
+	mfspr		$vrsave,256
+	li		r9,0x20
+	mtspr		256,r0
+	li		r10,0x30
+	lvx_u		$IN,0,$Xip		# load Xi
+
+	lvx_u		$Hl,r8,$Htbl		# load pre-computed table
+	 le?lvsl	$lemask,r0,r0
+	lvx_u		$H, r9,$Htbl
+	 le?vspltisb	$t0,0x07
+	lvx_u		$Hh,r10,$Htbl
+	 le?vxor	$lemask,$lemask,$t0
+	lvx_u		$xC2,0,$Htbl
+	 le?vperm	$IN,$IN,$IN,$lemask
+	vxor		$zero,$zero,$zero
+
+	vpmsumd		$Xl,$IN,$Hl		# H.lo·Xi.lo
+	vpmsumd		$Xm,$IN,$H		# H.hi·Xi.lo+H.lo·Xi.hi
+	vpmsumd		$Xh,$IN,$Hh		# H.hi·Xi.hi
+
+	vpmsumd		$t2,$Xl,$xC2		# 1st phase
+
+	vsldoi		$t0,$Xm,$zero,8
+	vsldoi		$t1,$zero,$Xm,8
+	vxor		$Xl,$Xl,$t0
+	vxor		$Xh,$Xh,$t1
+
+	vsldoi		$Xl,$Xl,$Xl,8
+	vxor		$Xl,$Xl,$t2
+
+	vsldoi		$t1,$Xl,$Xl,8		# 2nd phase
+	vpmsumd		$Xl,$Xl,$xC2
+	vxor		$t1,$t1,$Xh
+	vxor		$Xl,$Xl,$t1
+
+	le?vperm	$Xl,$Xl,$Xl,$lemask
+	stvx_u		$Xl,0,$Xip		# write out Xi
+
+	mtspr		256,$vrsave
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0,0,2,0
+	.long		0
+.size	.gcm_gmult_p8,.-.gcm_gmult_p8
+
+.globl	.gcm_ghash_p8
+	lis		r0,0xfff8
+	li		r8,0x10
+	mfspr		$vrsave,256
+	li		r9,0x20
+	mtspr		256,r0
+	li		r10,0x30
+	lvx_u		$Xl,0,$Xip		# load Xi
+
+	lvx_u		$Hl,r8,$Htbl		# load pre-computed table
+	 le?lvsl	$lemask,r0,r0
+	lvx_u		$H, r9,$Htbl
+	 le?vspltisb	$t0,0x07
+	lvx_u		$Hh,r10,$Htbl
+	 le?vxor	$lemask,$lemask,$t0
+	lvx_u		$xC2,0,$Htbl
+	 le?vperm	$Xl,$Xl,$Xl,$lemask
+	vxor		$zero,$zero,$zero
+
+	lvx_u		$IN,0,$inp
+	addi		$inp,$inp,16
+	subi		$len,$len,16
+	 le?vperm	$IN,$IN,$IN,$lemask
+	vxor		$IN,$IN,$Xl
+	b		Loop
+
+.align	5
+Loop:
+	 subic		$len,$len,16
+	vpmsumd		$Xl,$IN,$Hl		# H.lo·Xi.lo
+	 subfe.		r0,r0,r0		# borrow?-1:0
+	vpmsumd		$Xm,$IN,$H		# H.hi·Xi.lo+H.lo·Xi.hi
+	 and		r0,r0,$len
+	vpmsumd		$Xh,$IN,$Hh		# H.hi·Xi.hi
+	 add		$inp,$inp,r0
+
+	vpmsumd		$t2,$Xl,$xC2		# 1st phase
+
+	vsldoi		$t0,$Xm,$zero,8
+	vsldoi		$t1,$zero,$Xm,8
+	vxor		$Xl,$Xl,$t0
+	vxor		$Xh,$Xh,$t1
+
+	vsldoi		$Xl,$Xl,$Xl,8
+	vxor		$Xl,$Xl,$t2
+	 lvx_u		$IN,0,$inp
+	 addi		$inp,$inp,16
+
+	vsldoi		$t1,$Xl,$Xl,8		# 2nd phase
+	vpmsumd		$Xl,$Xl,$xC2
+	 le?vperm	$IN,$IN,$IN,$lemask
+	vxor		$t1,$t1,$Xh
+	vxor		$IN,$IN,$t1
+	vxor		$IN,$IN,$Xl
+	beq		Loop			# did $len-=16 borrow?
+
+	vxor		$Xl,$Xl,$t1
+	le?vperm	$Xl,$Xl,$Xl,$lemask
+	stvx_u		$Xl,0,$Xip		# write out Xi
+
+	mtspr		256,$vrsave
+	blr
+	.long		0
+	.byte		0,12,0x14,0,0,0,4,0
+	.long		0
+.size	.gcm_ghash_p8,.-.gcm_ghash_p8
+
+.asciz  "GHASH for PowerISA 2.07, CRYPTOGAMS by <appro\@openssl.org>"
+.align  2
+___
+
+foreach (split("\n",$code)) {
+	if ($flavour =~ /le$/o) {	# little-endian
+	    s/le\?//o		or
+	    s/be\?/#be#/o;
+	} else {
+	    s/le\?/#le#/o	or
+	    s/be\?//o;
+	}
+	print $_,"\n";
+}
+
+close STDOUT; # enforce flush
diff --git a/drivers/crypto/vmx/ppc-xlate.pl b/drivers/crypto/vmx/ppc-xlate.pl
new file mode 100644
index 000000000000..a59188494af8
--- /dev/null
+++ b/drivers/crypto/vmx/ppc-xlate.pl
@@ -0,0 +1,207 @@
+#!/usr/bin/env perl
+
+# PowerPC assembler distiller by <appro>.
+
+my $flavour = shift;
+my $output = shift;
+open STDOUT,">$output" || die "can't open $output: $!";
+
+my %GLOBALS;
+my $dotinlocallabels=($flavour=~/linux/)?1:0;
+
+################################################################
+# directives which need special treatment on different platforms
+################################################################
+my $globl = sub {
+    my $junk = shift;
+    my $name = shift;
+    my $global = \$GLOBALS{$name};
+    my $ret;
+
+    $name =~ s|^[\.\_]||;
+ 
+    SWITCH: for ($flavour) {
+	/aix/		&& do { $name = ".$name";
+				last;
+			      };
+	/osx/		&& do { $name = "_$name";
+				last;
+			      };
+	/linux/
+			&& do {	$ret = "_GLOBAL($name)";
+				last;
+			      };
+    }
+
+    $ret = ".globl	$name\nalign 5\n$name:" if (!$ret);
+    $$global = $name;
+    $ret;
+};
+my $text = sub {
+    my $ret = ($flavour =~ /aix/) ? ".csect\t.text[PR],7" : ".text";
+    $ret = ".abiversion	2\n".$ret	if ($flavour =~ /linux.*64le/);
+    $ret;
+};
+my $machine = sub {
+    my $junk = shift;
+    my $arch = shift;
+    if ($flavour =~ /osx/)
+    {	$arch =~ s/\"//g;
+	$arch = ($flavour=~/64/) ? "ppc970-64" : "ppc970" if ($arch eq "any");
+    }
+    ".machine	$arch";
+};
+my $size = sub {
+    if ($flavour =~ /linux/)
+    {	shift;
+	my $name = shift; $name =~ s|^[\.\_]||;
+	my $ret  = ".size	$name,.-".($flavour=~/64$/?".":"").$name;
+	$ret .= "\n.size	.$name,.-.$name" if ($flavour=~/64$/);
+	$ret;
+    }
+    else
+    {	"";	}
+};
+my $asciz = sub {
+    shift;
+    my $line = join(",",@_);
+    if ($line =~ /^"(.*)"$/)
+    {	".byte	" . join(",",unpack("C*",$1),0) . "\n.align	2";	}
+    else
+    {	"";	}
+};
+my $quad = sub {
+    shift;
+    my @ret;
+    my ($hi,$lo);
+    for (@_) {
+	if (/^0x([0-9a-f]*?)([0-9a-f]{1,8})$/io)
+	{  $hi=$1?"0x$1":"0"; $lo="0x$2";  }
+	elsif (/^([0-9]+)$/o)
+	{  $hi=$1>>32; $lo=$1&0xffffffff;  } # error-prone with 32-bit perl
+	else
+	{  $hi=undef; $lo=$_; }
+
+	if (defined($hi))
+	{  push(@ret,$flavour=~/le$/o?".long\t$lo,$hi":".long\t$hi,$lo");  }
+	else
+	{  push(@ret,".quad	$lo");  }
+    }
+    join("\n",@ret);
+};
+
+################################################################
+# simplified mnemonics not handled by at least one assembler
+################################################################
+my $cmplw = sub {
+    my $f = shift;
+    my $cr = 0; $cr = shift if ($#_>1);
+    # Some out-of-date 32-bit GNU assembler just can't handle cmplw...
+    ($flavour =~ /linux.*32/) ?
+	"	.long	".sprintf "0x%x",31<<26|$cr<<23|$_[0]<<16|$_[1]<<11|64 :
+	"	cmplw	".join(',',$cr,@_);
+};
+my $bdnz = sub {
+    my $f = shift;
+    my $bo = $f=~/[\+\-]/ ? 16+9 : 16;	# optional "to be taken" hint
+    "	bc	$bo,0,".shift;
+} if ($flavour!~/linux/);
+my $bltlr = sub {
+    my $f = shift;
+    my $bo = $f=~/\-/ ? 12+2 : 12;	# optional "not to be taken" hint
+    ($flavour =~ /linux/) ?		# GNU as doesn't allow most recent hints
+	"	.long	".sprintf "0x%x",19<<26|$bo<<21|16<<1 :
+	"	bclr	$bo,0";
+};
+my $bnelr = sub {
+    my $f = shift;
+    my $bo = $f=~/\-/ ? 4+2 : 4;	# optional "not to be taken" hint
+    ($flavour =~ /linux/) ?		# GNU as doesn't allow most recent hints
+	"	.long	".sprintf "0x%x",19<<26|$bo<<21|2<<16|16<<1 :
+	"	bclr	$bo,2";
+};
+my $beqlr = sub {
+    my $f = shift;
+    my $bo = $f=~/-/ ? 12+2 : 12;	# optional "not to be taken" hint
+    ($flavour =~ /linux/) ?		# GNU as doesn't allow most recent hints
+	"	.long	".sprintf "0x%X",19<<26|$bo<<21|2<<16|16<<1 :
+	"	bclr	$bo,2";
+};
+# GNU assembler can't handle extrdi rA,rS,16,48, or when sum of last two
+# arguments is 64, with "operand out of range" error.
+my $extrdi = sub {
+    my ($f,$ra,$rs,$n,$b) = @_;
+    $b = ($b+$n)&63; $n = 64-$n;
+    "	rldicl	$ra,$rs,$b,$n";
+};
+my $vmr = sub {
+    my ($f,$vx,$vy) = @_;
+    "	vor	$vx,$vy,$vy";
+};
+
+# PowerISA 2.06 stuff
+sub vsxmem_op {
+    my ($f, $vrt, $ra, $rb, $op) = @_;
+    "	.long	".sprintf "0x%X",(31<<26)|($vrt<<21)|($ra<<16)|($rb<<11)|($op*2+1);
+}
+# made-up unaligned memory reference AltiVec/VMX instructions
+my $lvx_u	= sub {	vsxmem_op(@_, 844); };	# lxvd2x
+my $stvx_u	= sub {	vsxmem_op(@_, 972); };	# stxvd2x
+my $lvdx_u	= sub {	vsxmem_op(@_, 588); };	# lxsdx
+my $stvdx_u	= sub {	vsxmem_op(@_, 716); };	# stxsdx
+my $lvx_4w	= sub { vsxmem_op(@_, 780); };	# lxvw4x
+my $stvx_4w	= sub { vsxmem_op(@_, 908); };	# stxvw4x
+
+# PowerISA 2.07 stuff
+sub vcrypto_op {
+    my ($f, $vrt, $vra, $vrb, $op) = @_;
+    "	.long	".sprintf "0x%X",(4<<26)|($vrt<<21)|($vra<<16)|($vrb<<11)|$op;
+}
+my $vcipher	= sub { vcrypto_op(@_, 1288); };
+my $vcipherlast	= sub { vcrypto_op(@_, 1289); };
+my $vncipher	= sub { vcrypto_op(@_, 1352); };
+my $vncipherlast= sub { vcrypto_op(@_, 1353); };
+my $vsbox	= sub { vcrypto_op(@_, 0, 1480); };
+my $vshasigmad	= sub { my ($st,$six)=splice(@_,-2); vcrypto_op(@_, $st<<4|$six, 1730); };
+my $vshasigmaw	= sub { my ($st,$six)=splice(@_,-2); vcrypto_op(@_, $st<<4|$six, 1666); };
+my $vpmsumb	= sub { vcrypto_op(@_, 1032); };
+my $vpmsumd	= sub { vcrypto_op(@_, 1224); };
+my $vpmsubh	= sub { vcrypto_op(@_, 1096); };
+my $vpmsumw	= sub { vcrypto_op(@_, 1160); };
+my $vaddudm	= sub { vcrypto_op(@_, 192);  };
+
+my $mtsle	= sub {
+    my ($f, $arg) = @_;
+    "	.long	".sprintf "0x%X",(31<<26)|($arg<<21)|(147*2);
+};
+
+print "#include <asm/ppc_asm.h>\n" if $flavour =~ /linux/;
+
+while($line=<>) {
+
+    $line =~ s|[#!;].*$||;	# get rid of asm-style comments...
+    $line =~ s|/\*.*\*/||;	# ... and C-style comments...
+    $line =~ s|^\s+||;		# ... and skip white spaces in beginning...
+    $line =~ s|\s+$||;		# ... and at the end
+
+    {
+	$line =~ s|\b\.L(\w+)|L$1|g;	# common denominator for Locallabel
+	$line =~ s|\bL(\w+)|\.L$1|g	if ($dotinlocallabels);
+    }
+
+    {
+	$line =~ s|^\s*(\.?)(\w+)([\.\+\-]?)\s*||;
+	my $c = $1; $c = "\t" if ($c eq "");
+	my $mnemonic = $2;
+	my $f = $3;
+	my $opcode = eval("\$$mnemonic");
+	$line =~ s/\b(c?[rf]|v|vs)([0-9]+)\b/$2/g if ($c ne "." and $flavour !~ /osx/);
+	if (ref($opcode) eq 'CODE') { $line = &$opcode($f,split(',',$line)); }
+	elsif ($mnemonic)           { $line = $c.$mnemonic.$f."\t".$line; }
+    }
+
+    print $line if ($line);
+    print "\n";
+}
+
+close STDOUT;
diff --git a/drivers/crypto/vmx/vmx.c b/drivers/crypto/vmx/vmx.c
new file mode 100644
index 000000000000..44d8d5cfe40d
--- /dev/null
+++ b/drivers/crypto/vmx/vmx.c
@@ -0,0 +1,88 @@
+/**
+ * Routines supporting VMX instructions on the Power 8
+ *
+ * Copyright (C) 2015 International Business Machines Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 only.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ * Author: Marcelo Henrique Cerri <mhcerri@br.ibm.com>
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/crypto.h>
+#include <asm/cputable.h>
+#include <crypto/internal/hash.h>
+
+extern struct shash_alg p8_ghash_alg;
+extern struct crypto_alg p8_aes_alg;
+extern struct crypto_alg p8_aes_cbc_alg;
+extern struct crypto_alg p8_aes_ctr_alg;
+static struct crypto_alg *algs[] = {
+    &p8_aes_alg,
+    &p8_aes_cbc_alg,
+    &p8_aes_ctr_alg,
+    NULL,
+};
+
+int __init p8_init(void)
+{
+    int ret = 0;
+    struct crypto_alg **alg_it;
+
+    if (!(cur_cpu_spec->cpu_user_features2 & PPC_FEATURE2_VEC_CRYPTO))
+        return -ENODEV;
+
+    for (alg_it = algs; *alg_it; alg_it++) {
+        ret = crypto_register_alg(*alg_it);
+        printk(KERN_INFO "crypto_register_alg '%s' = %d\n",
+                (*alg_it)->cra_name, ret);
+        if (ret) {
+            for (alg_it--; alg_it >= algs; alg_it--)
+                crypto_unregister_alg(*alg_it);
+            break;
+        }
+    }
+    if (ret)
+        return ret;
+
+    ret = crypto_register_shash(&p8_ghash_alg);
+    if (ret) {
+        for (alg_it = algs; *alg_it; alg_it++)
+            crypto_unregister_alg(*alg_it);
+    }
+    return ret;
+}
+
+void __exit p8_exit(void)
+{
+    struct crypto_alg **alg_it;
+
+    for (alg_it = algs; *alg_it; alg_it++) {
+        printk(KERN_INFO "Removing '%s'\n", (*alg_it)->cra_name);
+        crypto_unregister_alg(*alg_it);
+    }
+    crypto_unregister_shash(&p8_ghash_alg);
+}
+
+module_init(p8_init);
+module_exit(p8_exit);
+
+MODULE_AUTHOR("Marcelo Cerri<mhcerri@br.ibm.com>");
+MODULE_DESCRIPTION("IBM VMX cryptogaphic acceleration instructions support on Power 8");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0.0");
+
diff --git a/drivers/devfreq/devfreq.c b/drivers/devfreq/devfreq.c
index 30b538d8cc90..ca1b362d77e2 100644
--- a/drivers/devfreq/devfreq.c
+++ b/drivers/devfreq/devfreq.c
@@ -392,7 +392,6 @@ static int devfreq_notifier_call(struct notifier_block *nb, unsigned long type,
 /**
  * _remove_devfreq() - Remove devfreq from the list and release its resources.
  * @devfreq:	the devfreq struct
- * @skip:	skip calling device_unregister().
  */
 static void _remove_devfreq(struct devfreq *devfreq)
 {
diff --git a/drivers/devfreq/event/exynos-ppmu.c b/drivers/devfreq/event/exynos-ppmu.c
index ad8347385f53..7d99d13bacd8 100644
--- a/drivers/devfreq/event/exynos-ppmu.c
+++ b/drivers/devfreq/event/exynos-ppmu.c
@@ -194,7 +194,7 @@ static int exynos_ppmu_get_event(struct devfreq_event_dev *edev,
 	return 0;
 }
 
-static struct devfreq_event_ops exynos_ppmu_ops = {
+static const struct devfreq_event_ops exynos_ppmu_ops = {
 	.disable = exynos_ppmu_disable,
 	.set_event = exynos_ppmu_set_event,
 	.get_event = exynos_ppmu_get_event,
diff --git a/drivers/devfreq/tegra-devfreq.c b/drivers/devfreq/tegra-devfreq.c
index 34790961af5a..13a1a6e8108c 100644
--- a/drivers/devfreq/tegra-devfreq.c
+++ b/drivers/devfreq/tegra-devfreq.c
@@ -62,7 +62,8 @@
 #define ACTMON_BELOW_WMARK_WINDOW				3
 #define ACTMON_BOOST_FREQ_STEP					16000
 
-/* activity counter is incremented every 256 memory transactions, and each
+/*
+ * Activity counter is incremented every 256 memory transactions, and each
  * transaction takes 4 EMC clocks for Tegra124; So the COUNT_WEIGHT is
  * 4 * 256 = 1024.
  */
@@ -85,16 +86,25 @@
  * struct tegra_devfreq_device_config - configuration specific to an ACTMON
  * device
  *
- * Coefficients and thresholds are in %
+ * Coefficients and thresholds are percentages unless otherwise noted
  */
 struct tegra_devfreq_device_config {
 	u32		offset;
 	u32		irq_mask;
 
+	/* Factors applied to boost_freq every consecutive watermark breach */
 	unsigned int	boost_up_coeff;
 	unsigned int	boost_down_coeff;
+
+	/* Define the watermark bounds when applied to the current avg */
 	unsigned int	boost_up_threshold;
 	unsigned int	boost_down_threshold;
+
+	/*
+	 * Threshold of activity (cycles) below which the CPU frequency isn't
+	 * to be taken into account. This is to avoid increasing the EMC
+	 * frequency when the CPU is very busy but not accessing the bus often.
+	 */
 	u32		avg_dependency_threshold;
 };
 
@@ -105,7 +115,7 @@ enum tegra_actmon_device {
 
 static struct tegra_devfreq_device_config actmon_device_configs[] = {
 	{
-		/* MCALL */
+		/* MCALL: All memory accesses (including from the CPUs) */
 		.offset = 0x1c0,
 		.irq_mask = 1 << 26,
 		.boost_up_coeff = 200,
@@ -114,7 +124,7 @@ static struct tegra_devfreq_device_config actmon_device_configs[] = {
 		.boost_down_threshold = 40,
 	},
 	{
-		/* MCCPU */
+		/* MCCPU: memory accesses from the CPUs */
 		.offset = 0x200,
 		.irq_mask = 1 << 25,
 		.boost_up_coeff = 800,
@@ -132,25 +142,29 @@ static struct tegra_devfreq_device_config actmon_device_configs[] = {
  */
 struct tegra_devfreq_device {
 	const struct tegra_devfreq_device_config *config;
+	void __iomem *regs;
+	spinlock_t lock;
+
+	/* Average event count sampled in the last interrupt */
+	u32 avg_count;
 
-	void __iomem	*regs;
-	u32		avg_band_freq;
-	u32		avg_count;
+	/*
+	 * Extra frequency to increase the target by due to consecutive
+	 * watermark breaches.
+	 */
+	unsigned long boost_freq;
 
-	unsigned long	target_freq;
-	unsigned long	boost_freq;
+	/* Optimal frequency calculated from the stats for this device */
+	unsigned long target_freq;
 };
 
 struct tegra_devfreq {
 	struct devfreq		*devfreq;
 
-	struct platform_device	*pdev;
 	struct reset_control	*reset;
 	struct clk		*clock;
 	void __iomem		*regs;
 
-	spinlock_t		lock;
-
 	struct clk		*emc_clock;
 	unsigned long		max_freq;
 	unsigned long		cur_freq;
@@ -174,19 +188,43 @@ static struct tegra_actmon_emc_ratio actmon_emc_ratios[] = {
 	{  250000,    100000 },
 };
 
+static u32 actmon_readl(struct tegra_devfreq *tegra, u32 offset)
+{
+	return readl(tegra->regs + offset);
+}
+
+static void actmon_writel(struct tegra_devfreq *tegra, u32 val, u32 offset)
+{
+	writel(val, tegra->regs + offset);
+}
+
+static u32 device_readl(struct tegra_devfreq_device *dev, u32 offset)
+{
+	return readl(dev->regs + offset);
+}
+
+static void device_writel(struct tegra_devfreq_device *dev, u32 val,
+			  u32 offset)
+{
+	writel(val, dev->regs + offset);
+}
+
 static unsigned long do_percent(unsigned long val, unsigned int pct)
 {
 	return val * pct / 100;
 }
 
-static void tegra_devfreq_update_avg_wmark(struct tegra_devfreq_device *dev)
+static void tegra_devfreq_update_avg_wmark(struct tegra_devfreq *tegra,
+					   struct tegra_devfreq_device *dev)
 {
 	u32 avg = dev->avg_count;
-	u32 band = dev->avg_band_freq * ACTMON_SAMPLING_PERIOD;
+	u32 avg_band_freq = tegra->max_freq * ACTMON_DEFAULT_AVG_BAND / KHZ;
+	u32 band = avg_band_freq * ACTMON_SAMPLING_PERIOD;
+
+	device_writel(dev, avg + band, ACTMON_DEV_AVG_UPPER_WMARK);
 
-	writel(avg + band, dev->regs + ACTMON_DEV_AVG_UPPER_WMARK);
-	avg = max(avg, band);
-	writel(avg - band, dev->regs + ACTMON_DEV_AVG_LOWER_WMARK);
+	avg = max(dev->avg_count, band);
+	device_writel(dev, avg - band, ACTMON_DEV_AVG_LOWER_WMARK);
 }
 
 static void tegra_devfreq_update_wmark(struct tegra_devfreq *tegra,
@@ -194,96 +232,96 @@ static void tegra_devfreq_update_wmark(struct tegra_devfreq *tegra,
 {
 	u32 val = tegra->cur_freq * ACTMON_SAMPLING_PERIOD;
 
-	writel(do_percent(val, dev->config->boost_up_threshold),
-	       dev->regs + ACTMON_DEV_UPPER_WMARK);
+	device_writel(dev, do_percent(val, dev->config->boost_up_threshold),
+		      ACTMON_DEV_UPPER_WMARK);
 
-	writel(do_percent(val, dev->config->boost_down_threshold),
-	       dev->regs + ACTMON_DEV_LOWER_WMARK);
+	device_writel(dev, do_percent(val, dev->config->boost_down_threshold),
+		      ACTMON_DEV_LOWER_WMARK);
 }
 
 static void actmon_write_barrier(struct tegra_devfreq *tegra)
 {
 	/* ensure the update has reached the ACTMON */
 	wmb();
-	readl(tegra->regs + ACTMON_GLB_STATUS);
+	actmon_readl(tegra, ACTMON_GLB_STATUS);
 }
 
-static irqreturn_t actmon_isr(int irq, void *data)
+static void actmon_isr_device(struct tegra_devfreq *tegra,
+			      struct tegra_devfreq_device *dev)
 {
-	struct tegra_devfreq *tegra = data;
-	struct tegra_devfreq_device *dev = NULL;
 	unsigned long flags;
-	u32 val;
-	unsigned int i;
-
-	val = readl(tegra->regs + ACTMON_GLB_STATUS);
-
-	for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
-		if (val & tegra->devices[i].config->irq_mask) {
-			dev = tegra->devices + i;
-			break;
-		}
-	}
-
-	if (!dev)
-		return IRQ_NONE;
+	u32 intr_status, dev_ctrl;
 
-	spin_lock_irqsave(&tegra->lock, flags);
+	spin_lock_irqsave(&dev->lock, flags);
 
-	dev->avg_count = readl(dev->regs + ACTMON_DEV_AVG_COUNT);
-	tegra_devfreq_update_avg_wmark(dev);
+	dev->avg_count = device_readl(dev, ACTMON_DEV_AVG_COUNT);
+	tegra_devfreq_update_avg_wmark(tegra, dev);
 
-	val = readl(dev->regs + ACTMON_DEV_INTR_STATUS);
-	if (val & ACTMON_DEV_INTR_CONSECUTIVE_UPPER) {
-		val = readl(dev->regs + ACTMON_DEV_CTRL) |
-			ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN |
-			ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
+	intr_status = device_readl(dev, ACTMON_DEV_INTR_STATUS);
+	dev_ctrl = device_readl(dev, ACTMON_DEV_CTRL);
 
+	if (intr_status & ACTMON_DEV_INTR_CONSECUTIVE_UPPER) {
 		/*
 		 * new_boost = min(old_boost * up_coef + step, max_freq)
 		 */
 		dev->boost_freq = do_percent(dev->boost_freq,
 					     dev->config->boost_up_coeff);
 		dev->boost_freq += ACTMON_BOOST_FREQ_STEP;
-		if (dev->boost_freq >= tegra->max_freq) {
-			dev->boost_freq = tegra->max_freq;
-			val &= ~ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
-		}
-		writel(val, dev->regs + ACTMON_DEV_CTRL);
-	} else if (val & ACTMON_DEV_INTR_CONSECUTIVE_LOWER) {
-		val = readl(dev->regs + ACTMON_DEV_CTRL) |
-			ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN |
-			ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
 
+		dev_ctrl |= ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
+
+		if (dev->boost_freq >= tegra->max_freq)
+			dev->boost_freq = tegra->max_freq;
+		else
+			dev_ctrl |= ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
+	} else if (intr_status & ACTMON_DEV_INTR_CONSECUTIVE_LOWER) {
 		/*
 		 * new_boost = old_boost * down_coef
 		 * or 0 if (old_boost * down_coef < step / 2)
 		 */
 		dev->boost_freq = do_percent(dev->boost_freq,
 					     dev->config->boost_down_coeff);
-		if (dev->boost_freq < (ACTMON_BOOST_FREQ_STEP >> 1)) {
+
+		dev_ctrl |= ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
+
+		if (dev->boost_freq < (ACTMON_BOOST_FREQ_STEP >> 1))
 			dev->boost_freq = 0;
-			val &= ~ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
-		}
-		writel(val, dev->regs + ACTMON_DEV_CTRL);
+		else
+			dev_ctrl |= ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
 	}
 
 	if (dev->config->avg_dependency_threshold) {
-		val = readl(dev->regs + ACTMON_DEV_CTRL);
 		if (dev->avg_count >= dev->config->avg_dependency_threshold)
-			val |= ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
+			dev_ctrl |= ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
 		else if (dev->boost_freq == 0)
-			val &= ~ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
-		writel(val, dev->regs + ACTMON_DEV_CTRL);
+			dev_ctrl &= ~ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
 	}
 
-	writel(ACTMON_INTR_STATUS_CLEAR, dev->regs + ACTMON_DEV_INTR_STATUS);
+	device_writel(dev, dev_ctrl, ACTMON_DEV_CTRL);
+
+	device_writel(dev, ACTMON_INTR_STATUS_CLEAR, ACTMON_DEV_INTR_STATUS);
 
 	actmon_write_barrier(tegra);
 
-	spin_unlock_irqrestore(&tegra->lock, flags);
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
 
-	return IRQ_WAKE_THREAD;
+static irqreturn_t actmon_isr(int irq, void *data)
+{
+	struct tegra_devfreq *tegra = data;
+	bool handled = false;
+	unsigned int i;
+	u32 val;
+
+	val = actmon_readl(tegra, ACTMON_GLB_STATUS);
+	for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
+		if (val & tegra->devices[i].config->irq_mask) {
+			actmon_isr_device(tegra, tegra->devices + i);
+			handled = true;
+		}
+	}
+
+	return handled ? IRQ_WAKE_THREAD : IRQ_NONE;
 }
 
 static unsigned long actmon_cpu_to_emc_rate(struct tegra_devfreq *tegra,
@@ -317,7 +355,7 @@ static void actmon_update_target(struct tegra_devfreq *tegra,
 		static_cpu_emc_freq = actmon_cpu_to_emc_rate(tegra, cpu_freq);
 	}
 
-	spin_lock_irqsave(&tegra->lock, flags);
+	spin_lock_irqsave(&dev->lock, flags);
 
 	dev->target_freq = dev->avg_count / ACTMON_SAMPLING_PERIOD;
 	avg_sustain_coef = 100 * 100 / dev->config->boost_up_threshold;
@@ -327,7 +365,7 @@ static void actmon_update_target(struct tegra_devfreq *tegra,
 	if (dev->avg_count >= dev->config->avg_dependency_threshold)
 		dev->target_freq = max(dev->target_freq, static_cpu_emc_freq);
 
-	spin_unlock_irqrestore(&tegra->lock, flags);
+	spin_unlock_irqrestore(&dev->lock, flags);
 }
 
 static irqreturn_t actmon_thread_isr(int irq, void *data)
@@ -345,131 +383,110 @@ static int tegra_actmon_rate_notify_cb(struct notifier_block *nb,
 				       unsigned long action, void *ptr)
 {
 	struct clk_notifier_data *data = ptr;
-	struct tegra_devfreq *tegra = container_of(nb, struct tegra_devfreq,
-						   rate_change_nb);
+	struct tegra_devfreq *tegra;
+	struct tegra_devfreq_device *dev;
 	unsigned int i;
 	unsigned long flags;
 
-	spin_lock_irqsave(&tegra->lock, flags);
+	if (action != POST_RATE_CHANGE)
+		return NOTIFY_OK;
 
-	switch (action) {
-	case POST_RATE_CHANGE:
-		tegra->cur_freq = data->new_rate / KHZ;
+	tegra = container_of(nb, struct tegra_devfreq, rate_change_nb);
 
-		for (i = 0; i < ARRAY_SIZE(tegra->devices); i++)
-			tegra_devfreq_update_wmark(tegra, tegra->devices + i);
+	tegra->cur_freq = data->new_rate / KHZ;
 
-		actmon_write_barrier(tegra);
-		break;
-	case PRE_RATE_CHANGE:
-		/* fall through */
-	case ABORT_RATE_CHANGE:
-		break;
-	};
+	for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
+		dev = &tegra->devices[i];
 
-	spin_unlock_irqrestore(&tegra->lock, flags);
+		spin_lock_irqsave(&dev->lock, flags);
+		tegra_devfreq_update_wmark(tegra, dev);
+		spin_unlock_irqrestore(&dev->lock, flags);
+	}
+
+	actmon_write_barrier(tegra);
 
 	return NOTIFY_OK;
 }
 
-static void tegra_actmon_configure_device(struct tegra_devfreq *tegra,
-					  struct tegra_devfreq_device *dev)
+static void tegra_actmon_enable_interrupts(struct tegra_devfreq *tegra)
 {
+	struct tegra_devfreq_device *dev;
 	u32 val;
+	unsigned int i;
 
-	dev->avg_band_freq = tegra->max_freq * ACTMON_DEFAULT_AVG_BAND / KHZ;
-	dev->target_freq = tegra->cur_freq;
-
-	dev->avg_count = tegra->cur_freq * ACTMON_SAMPLING_PERIOD;
-	writel(dev->avg_count, dev->regs + ACTMON_DEV_INIT_AVG);
-
-	tegra_devfreq_update_avg_wmark(dev);
-	tegra_devfreq_update_wmark(tegra, dev);
-
-	writel(ACTMON_COUNT_WEIGHT, dev->regs + ACTMON_DEV_COUNT_WEIGHT);
-	writel(ACTMON_INTR_STATUS_CLEAR, dev->regs + ACTMON_DEV_INTR_STATUS);
-
-	val = 0;
-	val |= ACTMON_DEV_CTRL_ENB_PERIODIC |
-	       ACTMON_DEV_CTRL_AVG_ABOVE_WMARK_EN |
-	       ACTMON_DEV_CTRL_AVG_BELOW_WMARK_EN;
-	val |= (ACTMON_AVERAGE_WINDOW_LOG2 - 1)
-		<< ACTMON_DEV_CTRL_K_VAL_SHIFT;
-	val |= (ACTMON_BELOW_WMARK_WINDOW - 1)
-		<< ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_NUM_SHIFT;
-	val |= (ACTMON_ABOVE_WMARK_WINDOW - 1)
-		<< ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_NUM_SHIFT;
-	val |= ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN |
-	       ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
-
-	writel(val, dev->regs + ACTMON_DEV_CTRL);
+	for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
+		dev = &tegra->devices[i];
 
-	actmon_write_barrier(tegra);
+		val = device_readl(dev, ACTMON_DEV_CTRL);
+		val |= ACTMON_DEV_CTRL_AVG_ABOVE_WMARK_EN;
+		val |= ACTMON_DEV_CTRL_AVG_BELOW_WMARK_EN;
+		val |= ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
+		val |= ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
 
-	val = readl(dev->regs + ACTMON_DEV_CTRL);
-	val |= ACTMON_DEV_CTRL_ENB;
-	writel(val, dev->regs + ACTMON_DEV_CTRL);
+		device_writel(dev, val, ACTMON_DEV_CTRL);
+	}
 
 	actmon_write_barrier(tegra);
 }
 
-static int tegra_devfreq_suspend(struct device *dev)
+static void tegra_actmon_disable_interrupts(struct tegra_devfreq *tegra)
 {
-	struct platform_device *pdev;
-	struct tegra_devfreq *tegra;
-	struct tegra_devfreq_device *actmon_dev;
-	unsigned int i;
+	struct tegra_devfreq_device *dev;
 	u32 val;
-
-	pdev = container_of(dev, struct platform_device, dev);
-	tegra = platform_get_drvdata(pdev);
+	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
-		actmon_dev = &tegra->devices[i];
-
-		val = readl(actmon_dev->regs + ACTMON_DEV_CTRL);
-		val &= ~ACTMON_DEV_CTRL_ENB;
-		writel(val, actmon_dev->regs + ACTMON_DEV_CTRL);
+		dev = &tegra->devices[i];
 
-		writel(ACTMON_INTR_STATUS_CLEAR,
-		       actmon_dev->regs + ACTMON_DEV_INTR_STATUS);
+		val = device_readl(dev, ACTMON_DEV_CTRL);
+		val &= ~ACTMON_DEV_CTRL_AVG_ABOVE_WMARK_EN;
+		val &= ~ACTMON_DEV_CTRL_AVG_BELOW_WMARK_EN;
+		val &= ~ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_EN;
+		val &= ~ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_EN;
 
-		actmon_write_barrier(tegra);
+		device_writel(dev, val, ACTMON_DEV_CTRL);
 	}
 
-	return 0;
+	actmon_write_barrier(tegra);
 }
 
-static int tegra_devfreq_resume(struct device *dev)
+static void tegra_actmon_configure_device(struct tegra_devfreq *tegra,
+					  struct tegra_devfreq_device *dev)
 {
-	struct platform_device *pdev;
-	struct tegra_devfreq *tegra;
-	struct tegra_devfreq_device *actmon_dev;
-	unsigned int i;
+	u32 val = 0;
 
-	pdev = container_of(dev, struct platform_device, dev);
-	tegra = platform_get_drvdata(pdev);
+	dev->target_freq = tegra->cur_freq;
 
-	for (i = 0; i < ARRAY_SIZE(tegra->devices); i++) {
-		actmon_dev = &tegra->devices[i];
+	dev->avg_count = tegra->cur_freq * ACTMON_SAMPLING_PERIOD;
+	device_writel(dev, dev->avg_count, ACTMON_DEV_INIT_AVG);
 
-		tegra_actmon_configure_device(tegra, actmon_dev);
-	}
+	tegra_devfreq_update_avg_wmark(tegra, dev);
+	tegra_devfreq_update_wmark(tegra, dev);
 
-	return 0;
+	device_writel(dev, ACTMON_COUNT_WEIGHT, ACTMON_DEV_COUNT_WEIGHT);
+	device_writel(dev, ACTMON_INTR_STATUS_CLEAR, ACTMON_DEV_INTR_STATUS);
+
+	val |= ACTMON_DEV_CTRL_ENB_PERIODIC;
+	val |= (ACTMON_AVERAGE_WINDOW_LOG2 - 1)
+		<< ACTMON_DEV_CTRL_K_VAL_SHIFT;
+	val |= (ACTMON_BELOW_WMARK_WINDOW - 1)
+		<< ACTMON_DEV_CTRL_CONSECUTIVE_BELOW_WMARK_NUM_SHIFT;
+	val |= (ACTMON_ABOVE_WMARK_WINDOW - 1)
+		<< ACTMON_DEV_CTRL_CONSECUTIVE_ABOVE_WMARK_NUM_SHIFT;
+	val |= ACTMON_DEV_CTRL_ENB;
+
+	device_writel(dev, val, ACTMON_DEV_CTRL);
+
+	actmon_write_barrier(tegra);
 }
 
 static int tegra_devfreq_target(struct device *dev, unsigned long *freq,
 				u32 flags)
 {
-	struct platform_device *pdev;
-	struct tegra_devfreq *tegra;
+	struct tegra_devfreq *tegra = dev_get_drvdata(dev);
 	struct dev_pm_opp *opp;
 	unsigned long rate = *freq * KHZ;
 
-	pdev = container_of(dev, struct platform_device, dev);
-	tegra = platform_get_drvdata(pdev);
-
 	rcu_read_lock();
 	opp = devfreq_recommended_opp(dev, &rate, flags);
 	if (IS_ERR(opp)) {
@@ -480,10 +497,8 @@ static int tegra_devfreq_target(struct device *dev, unsigned long *freq,
 	rate = dev_pm_opp_get_freq(opp);
 	rcu_read_unlock();
 
-	/* TODO: Once we have per-user clk constraints, set a floor */
-	clk_set_rate(tegra->emc_clock, rate);
-
-	/* TODO: Set voltage as well */
+	clk_set_min_rate(tegra->emc_clock, rate);
+	clk_set_rate(tegra->emc_clock, 0);
 
 	return 0;
 }
@@ -491,13 +506,9 @@ static int tegra_devfreq_target(struct device *dev, unsigned long *freq,
 static int tegra_devfreq_get_dev_status(struct device *dev,
 					struct devfreq_dev_status *stat)
 {
-	struct platform_device *pdev;
-	struct tegra_devfreq *tegra;
+	struct tegra_devfreq *tegra = dev_get_drvdata(dev);
 	struct tegra_devfreq_device *actmon_dev;
 
-	pdev = container_of(dev, struct platform_device, dev);
-	tegra = platform_get_drvdata(pdev);
-
 	stat->current_frequency = tegra->cur_freq;
 
 	/* To be used by the tegra governor */
@@ -508,7 +519,7 @@ static int tegra_devfreq_get_dev_status(struct device *dev,
 	actmon_dev = &tegra->devices[MCALL];
 
 	/* Number of cycles spent on memory access */
-	stat->busy_time = actmon_dev->avg_count;
+	stat->busy_time = device_readl(actmon_dev, ACTMON_DEV_AVG_COUNT);
 
 	/* The bus can be considered to be saturated way before 100% */
 	stat->busy_time *= 100 / BUS_SATURATION_RATIO;
@@ -516,11 +527,19 @@ static int tegra_devfreq_get_dev_status(struct device *dev,
 	/* Number of cycles in a sampling period */
 	stat->total_time = ACTMON_SAMPLING_PERIOD * tegra->cur_freq;
 
+	stat->busy_time = min(stat->busy_time, stat->total_time);
+
 	return 0;
 }
 
-static int tegra_devfreq_get_target(struct devfreq *devfreq,
-				    unsigned long *freq)
+static struct devfreq_dev_profile tegra_devfreq_profile = {
+	.polling_ms	= 0,
+	.target		= tegra_devfreq_target,
+	.get_dev_status	= tegra_devfreq_get_dev_status,
+};
+
+static int tegra_governor_get_target(struct devfreq *devfreq,
+				     unsigned long *freq)
 {
 	struct devfreq_dev_status stat;
 	struct tegra_devfreq *tegra;
@@ -548,22 +567,43 @@ static int tegra_devfreq_get_target(struct devfreq *devfreq,
 	return 0;
 }
 
-static int tegra_devfreq_event_handler(struct devfreq *devfreq,
-				       unsigned int event, void *data)
+static int tegra_governor_event_handler(struct devfreq *devfreq,
+					unsigned int event, void *data)
 {
-	return 0;
+	struct tegra_devfreq *tegra;
+	int ret = 0;
+
+	tegra = dev_get_drvdata(devfreq->dev.parent);
+
+	switch (event) {
+	case DEVFREQ_GOV_START:
+		devfreq_monitor_start(devfreq);
+		tegra_actmon_enable_interrupts(tegra);
+		break;
+
+	case DEVFREQ_GOV_STOP:
+		tegra_actmon_disable_interrupts(tegra);
+		devfreq_monitor_stop(devfreq);
+		break;
+
+	case DEVFREQ_GOV_SUSPEND:
+		tegra_actmon_disable_interrupts(tegra);
+		devfreq_monitor_suspend(devfreq);
+		break;
+
+	case DEVFREQ_GOV_RESUME:
+		devfreq_monitor_resume(devfreq);
+		tegra_actmon_enable_interrupts(tegra);
+		break;
+	}
+
+	return ret;
 }
 
 static struct devfreq_governor tegra_devfreq_governor = {
-	.name = "tegra",
-	.get_target_freq = tegra_devfreq_get_target,
-	.event_handler = tegra_devfreq_event_handler,
-};
-
-static struct devfreq_dev_profile tegra_devfreq_profile = {
-	.polling_ms	= 0,
-	.target		= tegra_devfreq_target,
-	.get_dev_status	= tegra_devfreq_get_dev_status,
+	.name = "tegra_actmon",
+	.get_target_freq = tegra_governor_get_target,
+	.event_handler = tegra_governor_event_handler,
 };
 
 static int tegra_devfreq_probe(struct platform_device *pdev)
@@ -571,8 +611,8 @@ static int tegra_devfreq_probe(struct platform_device *pdev)
 	struct tegra_devfreq *tegra;
 	struct tegra_devfreq_device *dev;
 	struct resource *res;
-	unsigned long max_freq;
 	unsigned int i;
+	unsigned long rate;
 	int irq;
 	int err;
 
@@ -580,19 +620,11 @@ static int tegra_devfreq_probe(struct platform_device *pdev)
 	if (!tegra)
 		return -ENOMEM;
 
-	spin_lock_init(&tegra->lock);
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "Failed to get regs resource\n");
-		return -ENODEV;
-	}
 
 	tegra->regs = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(tegra->regs)) {
-		dev_err(&pdev->dev, "Failed to get IO memory\n");
+	if (IS_ERR(tegra->regs))
 		return PTR_ERR(tegra->regs);
-	}
 
 	tegra->reset = devm_reset_control_get(&pdev->dev, "actmon");
 	if (IS_ERR(tegra->reset)) {
@@ -612,11 +644,7 @@ static int tegra_devfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(tegra->emc_clock);
 	}
 
-	err = of_init_opp_table(&pdev->dev);
-	if (err) {
-		dev_err(&pdev->dev, "Failed to init operating point table\n");
-		return err;
-	}
+	clk_set_rate(tegra->emc_clock, ULONG_MAX);
 
 	tegra->rate_change_nb.notifier_call = tegra_actmon_rate_notify_cb;
 	err = clk_notifier_register(tegra->emc_clock, &tegra->rate_change_nb);
@@ -630,43 +658,41 @@ static int tegra_devfreq_probe(struct platform_device *pdev)
 
 	err = clk_prepare_enable(tegra->clock);
 	if (err) {
-		reset_control_deassert(tegra->reset);
+		dev_err(&pdev->dev,
+			"Failed to prepare and enable ACTMON clock\n");
 		return err;
 	}
 
 	reset_control_deassert(tegra->reset);
 
-	max_freq = clk_round_rate(tegra->emc_clock, ULONG_MAX);
-	tegra->max_freq = max_freq / KHZ;
-
-	clk_set_rate(tegra->emc_clock, max_freq);
-
+	tegra->max_freq = clk_round_rate(tegra->emc_clock, ULONG_MAX) / KHZ;
 	tegra->cur_freq = clk_get_rate(tegra->emc_clock) / KHZ;
 
-	writel(ACTMON_SAMPLING_PERIOD - 1,
-	       tegra->regs + ACTMON_GLB_PERIOD_CTRL);
+	actmon_writel(tegra, ACTMON_SAMPLING_PERIOD - 1,
+		      ACTMON_GLB_PERIOD_CTRL);
 
 	for (i = 0; i < ARRAY_SIZE(actmon_device_configs); i++) {
 		dev = tegra->devices + i;
 		dev->config = actmon_device_configs + i;
 		dev->regs = tegra->regs + dev->config->offset;
+		spin_lock_init(&dev->lock);
 
-		tegra_actmon_configure_device(tegra, tegra->devices + i);
+		tegra_actmon_configure_device(tegra, dev);
 	}
 
-	err = devfreq_add_governor(&tegra_devfreq_governor);
-	if (err) {
-		dev_err(&pdev->dev, "Failed to add governor\n");
-		return err;
+	for (rate = 0; rate <= tegra->max_freq * KHZ; rate++) {
+		rate = clk_round_rate(tegra->emc_clock, rate);
+		dev_pm_opp_add(&pdev->dev, rate, 0);
 	}
 
-	tegra_devfreq_profile.initial_freq = clk_get_rate(tegra->emc_clock);
-	tegra->devfreq = devm_devfreq_add_device(&pdev->dev,
-						 &tegra_devfreq_profile,
-						 "tegra",
-						 NULL);
-
 	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		dev_err(&pdev->dev, "Failed to get IRQ\n");
+		return -ENODEV;
+	}
+
+	platform_set_drvdata(pdev, tegra);
+
 	err = devm_request_threaded_irq(&pdev->dev, irq, actmon_isr,
 					actmon_thread_isr, IRQF_SHARED,
 					"tegra-devfreq", tegra);
@@ -675,7 +701,11 @@ static int tegra_devfreq_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	platform_set_drvdata(pdev, tegra);
+	tegra_devfreq_profile.initial_freq = clk_get_rate(tegra->emc_clock);
+	tegra->devfreq = devm_devfreq_add_device(&pdev->dev,
+						 &tegra_devfreq_profile,
+						 "tegra_actmon",
+						 NULL);
 
 	return 0;
 }
@@ -683,6 +713,19 @@ static int tegra_devfreq_probe(struct platform_device *pdev)
 static int tegra_devfreq_remove(struct platform_device *pdev)
 {
 	struct tegra_devfreq *tegra = platform_get_drvdata(pdev);
+	int irq = platform_get_irq(pdev, 0);
+	u32 val;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(actmon_device_configs); i++) {
+		val = device_readl(&tegra->devices[i], ACTMON_DEV_CTRL);
+		val &= ~ACTMON_DEV_CTRL_ENB;
+		device_writel(&tegra->devices[i], val, ACTMON_DEV_CTRL);
+	}
+
+	actmon_write_barrier(tegra);
+
+	devm_free_irq(&pdev->dev, irq, tegra);
 
 	clk_notifier_unregister(tegra->emc_clock, &tegra->rate_change_nb);
 
@@ -691,28 +734,52 @@ static int tegra_devfreq_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static SIMPLE_DEV_PM_OPS(tegra_devfreq_pm_ops,
-			 tegra_devfreq_suspend,
-			 tegra_devfreq_resume);
-
-static struct of_device_id tegra_devfreq_of_match[] = {
+static const struct of_device_id tegra_devfreq_of_match[] = {
 	{ .compatible = "nvidia,tegra124-actmon" },
 	{ },
 };
 
+MODULE_DEVICE_TABLE(of, tegra_devfreq_of_match);
+
 static struct platform_driver tegra_devfreq_driver = {
 	.probe	= tegra_devfreq_probe,
 	.remove	= tegra_devfreq_remove,
 	.driver = {
-		.name		= "tegra-devfreq",
-		.owner		= THIS_MODULE,
+		.name = "tegra-devfreq",
 		.of_match_table = tegra_devfreq_of_match,
-		.pm		= &tegra_devfreq_pm_ops,
 	},
 };
-module_platform_driver(tegra_devfreq_driver);
 
-MODULE_LICENSE("GPL");
+static int __init tegra_devfreq_init(void)
+{
+	int ret = 0;
+
+	ret = devfreq_add_governor(&tegra_devfreq_governor);
+	if (ret) {
+		pr_err("%s: failed to add governor: %d\n", __func__, ret);
+		return ret;
+	}
+
+	ret = platform_driver_register(&tegra_devfreq_driver);
+	if (ret)
+		devfreq_remove_governor(&tegra_devfreq_governor);
+
+	return ret;
+}
+module_init(tegra_devfreq_init)
+
+static void __exit tegra_devfreq_exit(void)
+{
+	int ret = 0;
+
+	platform_driver_unregister(&tegra_devfreq_driver);
+
+	ret = devfreq_remove_governor(&tegra_devfreq_governor);
+	if (ret)
+		pr_err("%s: failed to remove governor: %d\n", __func__, ret);
+}
+module_exit(tegra_devfreq_exit)
+
+MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("Tegra devfreq driver");
 MODULE_AUTHOR("Tomeu Vizoso <tomeu.vizoso@collabora.com>");
-MODULE_DEVICE_TABLE(of, tegra_devfreq_of_match);
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/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/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/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/of-dma.c b/drivers/dma/of-dma.c
index ca31f1b45366..cbd4a8aff120 100644
--- a/drivers/dma/of-dma.c
+++ b/drivers/dma/of-dma.c
@@ -194,6 +194,7 @@ struct dma_chan *of_dma_request_slave_channel(struct device_node *np,
 
 	return ERR_PTR(ret_no_channel);
 }
+EXPORT_SYMBOL_GPL(of_dma_request_slave_channel);
 
 /**
  * of_dma_simple_xlate - Simple DMA engine translation function
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/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/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index 63f01c42aed4..6b5e795f3fe2 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -1149,6 +1149,7 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 		}
 		break;
 	case WM5110:
+	case WM8280:
 		switch (arizona->rev) {
 		case 0 ... 2:
 			break;
diff --git a/drivers/firewire/net.c b/drivers/firewire/net.c
index 2c68da1ceeee..f4ea80d602f7 100644
--- a/drivers/firewire/net.c
+++ b/drivers/firewire/net.c
@@ -237,18 +237,6 @@ static int fwnet_header_create(struct sk_buff *skb, struct net_device *net,
 	return -net->hard_header_len;
 }
 
-static int fwnet_header_rebuild(struct sk_buff *skb)
-{
-	struct fwnet_header *h = (struct fwnet_header *)skb->data;
-
-	if (get_unaligned_be16(&h->h_proto) == ETH_P_IP)
-		return arp_find((unsigned char *)&h->h_dest, skb);
-
-	dev_notice(&skb->dev->dev, "unable to resolve type %04x addresses\n",
-		   be16_to_cpu(h->h_proto));
-	return 0;
-}
-
 static int fwnet_header_cache(const struct neighbour *neigh,
 			      struct hh_cache *hh, __be16 type)
 {
@@ -282,7 +270,6 @@ static int fwnet_header_parse(const struct sk_buff *skb, unsigned char *haddr)
 
 static const struct header_ops fwnet_header_ops = {
 	.create         = fwnet_header_create,
-	.rebuild        = fwnet_header_rebuild,
 	.cache		= fwnet_header_cache,
 	.cache_update	= fwnet_header_cache_update,
 	.parse          = fwnet_header_parse,
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 69fac068669f..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, u32 len, int num,
+static void dmi_table(u8 *buf,
 		      void (*decode)(const struct dmi_header *, void *),
 		      void *private_data)
 {
@@ -86,10 +88,13 @@ static void dmi_table(u8 *buf, u32 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;
 
 		/*
@@ -98,9 +103,9 @@ static void dmi_table(u8 *buf, u32 len, int num,
 		 *  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);
 
 		/*
@@ -115,8 +120,6 @@ static void dmi_table(u8 *buf, u32 len, int num,
 }
 
 static phys_addr_t dmi_base;
-static u32 dmi_len;
-static u16 dmi_num;
 
 static int __init dmi_walk_early(void (*decode)(const struct dmi_header *,
 		void *))
@@ -127,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);
 
@@ -198,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);
@@ -470,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])) {
@@ -503,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;
@@ -528,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;
@@ -901,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/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/Kconfig b/drivers/gpio/Kconfig
index c1e2ca3d9a51..dc1aaa83a347 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -669,6 +669,7 @@ config GPIO_STP_XWAY
 config GPIO_TC3589X
 	bool "TC3589X GPIOs"
 	depends on MFD_TC3589X
+	depends on OF_GPIO
 	select GPIOLIB_IRQCHIP
 	help
 	  This enables support for the GPIOs found on the TC3589X
diff --git a/drivers/gpio/gpio-arizona.c b/drivers/gpio/gpio-arizona.c
index fe369f5c7fa6..9665d0aa4ebb 100644
--- a/drivers/gpio/gpio-arizona.c
+++ b/drivers/gpio/gpio-arizona.c
@@ -116,6 +116,7 @@ static int arizona_gpio_probe(struct platform_device *pdev)
 	switch (arizona->type) {
 	case WM5102:
 	case WM5110:
+	case WM8280:
 	case WM8997:
 		arizona_gpio->gpio_chip.ngpio = 5;
 		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-tc3589x.c b/drivers/gpio/gpio-tc3589x.c
index 11aed2671065..31b244cffabb 100644
--- a/drivers/gpio/gpio-tc3589x.c
+++ b/drivers/gpio/gpio-tc3589x.c
@@ -260,10 +260,7 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	tc3589x_gpio->chip.ngpio = tc3589x->num_gpio;
 	tc3589x_gpio->chip.dev = &pdev->dev;
 	tc3589x_gpio->chip.base = -1;
-
-#ifdef CONFIG_OF_GPIO
 	tc3589x_gpio->chip.of_node = np;
-#endif
 
 	/* Bring the GPIO module out of reset */
 	ret = tc3589x_set_bits(tc3589x, TC3589x_RSTCTRL,
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.h b/drivers/gpio/gpiolib.h
index 550a5eafbd38..ab892be26dc2 100644
--- a/drivers/gpio/gpiolib.h
+++ b/drivers/gpio/gpiolib.h
@@ -17,6 +17,8 @@
 
 enum of_gpio_flags;
 
+struct acpi_device;
+
 /**
  * struct acpi_gpio_info - ACPI GPIO specific information
  * @gpioint: if %true this GPIO is of type GpioInt otherwise type is GpioIo
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 910ff8ab9c9c..d8135adb2238 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_device_queue_manager.c
@@ -645,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) {
@@ -652,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_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/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index f6d04c7b5115..b6f076b213bc 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -525,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
@@ -1320,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;
@@ -2132,7 +2121,7 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	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++)
@@ -2212,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;
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_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/drm_trace.h b/drivers/gpu/drm/drm_trace.h
index 27cc95f36381..ce3c42813fbb 100644
--- a/drivers/gpu/drm/drm_trace.h
+++ b/drivers/gpu/drm/drm_trace.h
@@ -7,7 +7,6 @@
 
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM drm
-#define TRACE_SYSTEM_STRING __stringify(TRACE_SYSTEM)
 #define TRACE_INCLUDE_FILE drm_trace
 
 TRACE_EVENT(drm_vblank_event,
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_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index cc6ea53d2b81..5c66b568bb81 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -1095,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);
 
 	/*
@@ -1189,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);
 }
 
@@ -1197,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;
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 8727086cf48c..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;
 };
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 5b205863b659..27ea6bdebce7 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -2737,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;
@@ -2789,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);
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_trace.h b/drivers/gpu/drm/i915/i915_trace.h
index 6058a01b4443..d776621c8521 100644
--- a/drivers/gpu/drm/i915/i915_trace.h
+++ b/drivers/gpu/drm/i915/i915_trace.h
@@ -12,7 +12,6 @@
 
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM i915
-#define TRACE_SYSTEM_STRING __stringify(TRACE_SYSTEM)
 #define TRACE_INCLUDE_FILE i915_trace
 
 /* pipe updates */
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 9943c20a741d..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>
@@ -2416,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)
@@ -2429,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;
@@ -2453,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,
@@ -6602,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");
@@ -6610,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;
@@ -7643,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;
 
@@ -7730,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");
@@ -7738,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;
@@ -9816,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;
 
@@ -9884,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);
@@ -13718,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_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/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/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/cikd.h b/drivers/gpu/drm/radeon/cikd.h
index c648e1996dab..243a36c93b8f 100644
--- a/drivers/gpu/drm/radeon/cikd.h
+++ b/drivers/gpu/drm/radeon/cikd.h
@@ -2129,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/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_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_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 33cf4108386d..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;
 
@@ -858,6 +854,17 @@ static struct radeon_ps *radeon_dpm_pick_power_state(struct radeon_device *rdev,
 	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
 	 */
@@ -983,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)
@@ -1007,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.
@@ -1069,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_trace.h b/drivers/gpu/drm/radeon/radeon_trace.h
index ce075cb08cb2..fdce4062901f 100644
--- a/drivers/gpu/drm/radeon/radeon_trace.h
+++ b/drivers/gpu/drm/radeon/radeon_trace.h
@@ -9,7 +9,6 @@
 
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM radeon
-#define TRACE_SYSTEM_STRING __stringify(TRACE_SYSTEM)
 #define TRACE_INCLUDE_FILE radeon_trace
 
 TRACE_EVENT(radeon_bo_create,
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/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/hid/Kconfig b/drivers/hid/Kconfig
index 152b006833cd..15338afdf7f9 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -92,7 +92,7 @@ menu "Special HID drivers"
 	depends on HID
 
 config HID_A4TECH
-	tristate "A4 tech mice" if EXPERT
+	tristate "A4 tech mice"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -113,7 +113,7 @@ config HID_ACRUX_FF
 	game controllers.
 
 config HID_APPLE
-	tristate "Apple {i,Power,Mac}Books" if EXPERT
+	tristate "Apple {i,Power,Mac}Books"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -141,7 +141,7 @@ config HID_AUREAL
 	Support for Aureal Cy se W-01RN Remote Controller and other Aureal derived remotes.
 
 config HID_BELKIN
-	tristate "Belkin Flip KVM and Wireless keyboard" if EXPERT
+	tristate "Belkin Flip KVM and Wireless keyboard"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -158,14 +158,14 @@ config HID_BETOP_FF
 	 - BETOP 2185 PC & BFM MODE
 
 config HID_CHERRY
-	tristate "Cherry Cymotion keyboard" if EXPERT
+	tristate "Cherry Cymotion keyboard"
 	depends on HID
 	default !EXPERT
 	---help---
 	Support for Cherry Cymotion keyboard.
 
 config HID_CHICONY
-	tristate "Chicony Tactical pad" if EXPERT
+	tristate "Chicony Tactical pad"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -196,7 +196,7 @@ config HID_CP2112
 	customizable USB descriptor fields are exposed as sysfs attributes.
 
 config HID_CYPRESS
-	tristate "Cypress mouse and barcode readers" if EXPERT
+	tristate "Cypress mouse and barcode readers"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -245,7 +245,7 @@ config HID_ELO
 	different devices than those handled by CONFIG_TOUCHSCREEN_USB_ELO.
 
 config HID_EZKEY
-	tristate "Ezkey BTC 8193 keyboard" if EXPERT
+	tristate "Ezkey BTC 8193 keyboard"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -286,12 +286,6 @@ config HID_GT683R
 	Currently the following devices are know to be supported:
 	  - MSI GT683R
 
-config HID_HUION
-	tristate "Huion tablets"
-	depends on USB_HID
-	---help---
-	Support for Huion 580 tablet.
-
 config HID_KEYTOUCH
 	tristate "Keytouch HID devices"
 	depends on HID
@@ -312,9 +306,9 @@ config HID_KYE
 
 config HID_UCLOGIC
 	tristate "UC-Logic"
-	depends on HID
+	depends on USB_HID
 	---help---
-	Support for UC-Logic tablets.
+	Support for UC-Logic and Huion tablets.
 
 config HID_WALTOP
 	tristate "Waltop"
@@ -344,7 +338,7 @@ config HID_TWINHAN
 	Support for Twinhan IR remote control.
 
 config HID_KENSINGTON
-	tristate "Kensington Slimblade Trackball" if EXPERT
+	tristate "Kensington Slimblade Trackball"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -372,7 +366,7 @@ config HID_LENOVO
 	- ThinkPad Compact USB Keyboard with TrackPoint (supports Fn keys)
 
 config HID_LOGITECH
-	tristate "Logitech devices" if EXPERT
+	tristate "Logitech devices"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -461,14 +455,14 @@ config HID_MAGICMOUSE
 	Apple Wireless "Magic" Mouse and the Apple Wireless "Magic" Trackpad.
 
 config HID_MICROSOFT
-	tristate "Microsoft non-fully HID-compliant devices" if EXPERT
+	tristate "Microsoft non-fully HID-compliant devices"
 	depends on HID
 	default !EXPERT
 	---help---
 	Support for Microsoft devices that are not fully compliant with HID standard.
 
 config HID_MONTEREY
-	tristate "Monterey Genius KB29E keyboard" if EXPERT
+	tristate "Monterey Genius KB29E keyboard"
 	depends on HID
 	default !EXPERT
 	---help---
@@ -638,7 +632,6 @@ config HID_PICOLCD_CIR
 
 config HID_PLANTRONICS
 	tristate "Plantronics USB HID Driver"
-	default !EXPERT
 	depends on HID
 	---help---
 	Provides HID support for Plantronics telephony devices.
@@ -885,6 +878,21 @@ config HID_SENSOR_HUB
 	  for events and handle data streams. Each sensor driver can format
 	  data and present to user mode using input or IIO interface.
 
+config HID_SENSOR_CUSTOM_SENSOR
+	tristate "HID Sensors hub custom sensor support"
+	depends on HID_SENSOR_HUB
+	default n
+	---help---
+	  HID Sensor hub specification allows definition of some custom and
+	  generic sensors. Unlike other HID sensors, they can't be exported
+	  via Linux IIO because of custom fields. This is up to the manufacturer
+	  to decide how to interpret these special sensor ids and process in
+	  the user space. Currently some manufacturers are using these ids for
+	  sensor calibration and debugging other sensors. Manufacturers
+	  should't use these special custom sensor ids to export any of the
+	  standard sensors.
+	  Select this config option for custom/generic sensor support.
+
 endmenu
 
 endif # HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index 6f19958dfc38..e4a21dfd7ef3 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -41,7 +41,6 @@ obj-$(CONFIG_HID_GYRATION)	+= hid-gyration.o
 obj-$(CONFIG_HID_HOLTEK)	+= hid-holtek-kbd.o
 obj-$(CONFIG_HID_HOLTEK)	+= hid-holtek-mouse.o
 obj-$(CONFIG_HID_HOLTEK)	+= hid-holtekff.o
-obj-$(CONFIG_HID_HUION)		+= hid-huion.o
 obj-$(CONFIG_HID_HYPERV_MOUSE)	+= hid-hyperv.o
 obj-$(CONFIG_HID_ICADE)		+= hid-icade.o
 obj-$(CONFIG_HID_KENSINGTON)	+= hid-kensington.o
@@ -101,6 +100,7 @@ obj-$(CONFIG_HID_WACOM)		+= wacom.o
 obj-$(CONFIG_HID_WALTOP)	+= hid-waltop.o
 obj-$(CONFIG_HID_WIIMOTE)	+= hid-wiimote.o
 obj-$(CONFIG_HID_SENSOR_HUB)	+= hid-sensor-hub.o
+obj-$(CONFIG_HID_SENSOR_CUSTOM_SENSOR)	+= hid-sensor-custom.o
 
 obj-$(CONFIG_USB_HID)		+= usbhid/
 obj-$(CONFIG_USB_MOUSE)		+= usbhid/
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 7c669c328c4c..722a925795a2 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -1562,12 +1562,26 @@ read_report_descriptor(struct file *filp, struct kobject *kobj,
 	return count;
 }
 
+static ssize_t
+show_country(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+
+	return sprintf(buf, "%02x\n", hdev->country & 0xff);
+}
+
 static struct bin_attribute dev_bin_attr_report_desc = {
 	.attr = { .name = "report_descriptor", .mode = 0444 },
 	.read = read_report_descriptor,
 	.size = HID_MAX_DESCRIPTOR_SIZE,
 };
 
+static struct device_attribute dev_attr_country = {
+	.attr = { .name = "country", .mode = 0444 },
+	.show = show_country,
+};
+
 int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 {
 	static const char *types[] = { "Device", "Pointer", "Mouse", "Device",
@@ -1646,6 +1660,11 @@ int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 		bus = "<UNKNOWN>";
 	}
 
+	ret = device_create_file(&hdev->dev, &dev_attr_country);
+	if (ret)
+		hid_warn(hdev,
+			 "can't create sysfs country code attribute err: %d\n", ret);
+
 	ret = device_create_bin_file(&hdev->dev, &dev_bin_attr_report_desc);
 	if (ret)
 		hid_warn(hdev,
@@ -1661,6 +1680,7 @@ EXPORT_SYMBOL_GPL(hid_connect);
 
 void hid_disconnect(struct hid_device *hdev)
 {
+	device_remove_file(&hdev->dev, &dev_attr_country);
 	device_remove_bin_file(&hdev->dev, &dev_bin_attr_report_desc);
 	if (hdev->claimed & HID_CLAIMED_INPUT)
 		hidinput_disconnect(hdev);
@@ -1824,6 +1844,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_PENSKETCH_M912) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LCPOWER, USB_DEVICE_ID_LCPOWER_LC1000 ) },
 #if IS_ENABLED(CONFIG_HID_LENOVO)
@@ -1959,6 +1980,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb65a) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE_BT) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE_PRO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TOPSEED, USB_DEVICE_ID_TOPSEED_CYBERLINK) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TOPSEED2, USB_DEVICE_ID_TOPSEED2_RF_COMBO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TWINHAN, USB_DEVICE_ID_TWINHAN_IR_REMOTE) },
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index 8bf61d295ffd..2886b645ced7 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -165,6 +165,7 @@ static const struct hid_usage_entry hid_usage_table[] = {
     {0, 0x53, "DeviceIndex"},
     {0, 0x54, "ContactCount"},
     {0, 0x55, "ContactMaximumNumber"},
+    {0, 0x59, "ButtonType"},
     {0, 0x5A, "SecondaryBarrelSwitch"},
     {0, 0x5B, "TransducerSerialNumber"},
   { 15, 0, "PhysicalInterfaceDevice" },
@@ -814,7 +815,7 @@ static const char *keys[KEY_MAX + 1] = {
 	[KEY_DELETEFILE] = "DeleteFile",	[KEY_XFER] = "X-fer",
 	[KEY_PROG1] = "Prog1",			[KEY_PROG2] = "Prog2",
 	[KEY_WWW] = "WWW",			[KEY_MSDOS] = "MSDOS",
-	[KEY_COFFEE] = "Coffee",		[KEY_DIRECTION] = "Direction",
+	[KEY_COFFEE] = "Coffee",		[KEY_ROTATE_DISPLAY] = "RotateDisplay",
 	[KEY_CYCLEWINDOWS] = "CycleWindows",	[KEY_MAIL] = "Mail",
 	[KEY_BOOKMARKS] = "Bookmarks",		[KEY_COMPUTER] = "Computer",
 	[KEY_BACK] = "Back",			[KEY_FORWARD] = "Forward",
@@ -1127,7 +1128,8 @@ static ssize_t hid_debug_events_read(struct file *file, char __user *buffer,
 
 				if (!list->hdev || !list->hdev->debug) {
 					ret = -EIO;
-					break;
+					set_current_state(TASK_RUNNING);
+					goto out;
 				}
 
 				/* allow O_NONBLOCK from other threads */
diff --git a/drivers/hid/hid-huion.c b/drivers/hid/hid-huion.c
deleted file mode 100644
index 61b68ca27790..000000000000
--- a/drivers/hid/hid-huion.c
+++ /dev/null
@@ -1,290 +0,0 @@
-/*
- *  HID driver for Huion devices not fully compliant with HID standard
- *
- *  Copyright (c) 2013 Martin Rusko
- *  Copyright (c) 2014 Nikolai Kondrashov
- */
-
-/*
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the Free
- * Software Foundation; either version 2 of the License, or (at your option)
- * any later version.
- */
-
-#include <linux/device.h>
-#include <linux/hid.h>
-#include <linux/module.h>
-#include <linux/usb.h>
-#include <asm/unaligned.h>
-#include "usbhid/usbhid.h"
-
-#include "hid-ids.h"
-
-/* Report descriptor template placeholder head */
-#define HUION_PH_HEAD	0xFE, 0xED, 0x1D
-
-/* Report descriptor template placeholder IDs */
-enum huion_ph_id {
-	HUION_PH_ID_X_LM,
-	HUION_PH_ID_X_PM,
-	HUION_PH_ID_Y_LM,
-	HUION_PH_ID_Y_PM,
-	HUION_PH_ID_PRESSURE_LM,
-	HUION_PH_ID_NUM
-};
-
-/* Report descriptor template placeholder */
-#define HUION_PH(_ID) HUION_PH_HEAD, HUION_PH_ID_##_ID
-
-/* Fixed report descriptor template */
-static const __u8 huion_tablet_rdesc_template[] = {
-	0x05, 0x0D,             /*  Usage Page (Digitizer),                 */
-	0x09, 0x02,             /*  Usage (Pen),                            */
-	0xA1, 0x01,             /*  Collection (Application),               */
-	0x85, 0x07,             /*      Report ID (7),                      */
-	0x09, 0x20,             /*      Usage (Stylus),                     */
-	0xA0,                   /*      Collection (Physical),              */
-	0x14,                   /*          Logical Minimum (0),            */
-	0x25, 0x01,             /*          Logical Maximum (1),            */
-	0x75, 0x01,             /*          Report Size (1),                */
-	0x09, 0x42,             /*          Usage (Tip Switch),             */
-	0x09, 0x44,             /*          Usage (Barrel Switch),          */
-	0x09, 0x46,             /*          Usage (Tablet Pick),            */
-	0x95, 0x03,             /*          Report Count (3),               */
-	0x81, 0x02,             /*          Input (Variable),               */
-	0x95, 0x03,             /*          Report Count (3),               */
-	0x81, 0x03,             /*          Input (Constant, Variable),     */
-	0x09, 0x32,             /*          Usage (In Range),               */
-	0x95, 0x01,             /*          Report Count (1),               */
-	0x81, 0x02,             /*          Input (Variable),               */
-	0x95, 0x01,             /*          Report Count (1),               */
-	0x81, 0x03,             /*          Input (Constant, Variable),     */
-	0x75, 0x10,             /*          Report Size (16),               */
-	0x95, 0x01,             /*          Report Count (1),               */
-	0xA4,                   /*          Push,                           */
-	0x05, 0x01,             /*          Usage Page (Desktop),           */
-	0x65, 0x13,             /*          Unit (Inch),                    */
-	0x55, 0xFD,             /*          Unit Exponent (-3),             */
-	0x34,                   /*          Physical Minimum (0),           */
-	0x09, 0x30,             /*          Usage (X),                      */
-	0x27, HUION_PH(X_LM),   /*          Logical Maximum (PLACEHOLDER),  */
-	0x47, HUION_PH(X_PM),   /*          Physical Maximum (PLACEHOLDER), */
-	0x81, 0x02,             /*          Input (Variable),               */
-	0x09, 0x31,             /*          Usage (Y),                      */
-	0x27, HUION_PH(Y_LM),   /*          Logical Maximum (PLACEHOLDER),  */
-	0x47, HUION_PH(Y_PM),   /*          Physical Maximum (PLACEHOLDER), */
-	0x81, 0x02,             /*          Input (Variable),               */
-	0xB4,                   /*          Pop,                            */
-	0x09, 0x30,             /*          Usage (Tip Pressure),           */
-	0x27,
-	HUION_PH(PRESSURE_LM),  /*          Logical Maximum (PLACEHOLDER),  */
-	0x81, 0x02,             /*          Input (Variable),               */
-	0xC0,                   /*      End Collection,                     */
-	0xC0                    /*  End Collection                          */
-};
-
-/* Parameter indices */
-enum huion_prm {
-	HUION_PRM_X_LM		= 1,
-	HUION_PRM_Y_LM		= 2,
-	HUION_PRM_PRESSURE_LM	= 4,
-	HUION_PRM_RESOLUTION	= 5,
-	HUION_PRM_NUM
-};
-
-/* Driver data */
-struct huion_drvdata {
-	__u8 *rdesc;
-	unsigned int rsize;
-};
-
-static __u8 *huion_report_fixup(struct hid_device *hdev, __u8 *rdesc,
-		unsigned int *rsize)
-{
-	struct huion_drvdata *drvdata = hid_get_drvdata(hdev);
-	switch (hdev->product) {
-	case USB_DEVICE_ID_HUION_TABLET:
-		if (drvdata->rdesc != NULL) {
-			rdesc = drvdata->rdesc;
-			*rsize = drvdata->rsize;
-		}
-		break;
-	}
-	return rdesc;
-}
-
-/**
- * Enable fully-functional tablet mode and determine device parameters.
- *
- * @hdev:	HID device
- */
-static int huion_tablet_enable(struct hid_device *hdev)
-{
-	int rc;
-	struct usb_device *usb_dev = hid_to_usb_dev(hdev);
-	struct huion_drvdata *drvdata = hid_get_drvdata(hdev);
-	__le16 *buf = NULL;
-	size_t len;
-	s32 params[HUION_PH_ID_NUM];
-	s32 resolution;
-	__u8 *p;
-	s32 v;
-
-	/*
-	 * Read string descriptor containing tablet parameters. The specific
-	 * string descriptor and data were discovered by sniffing the Windows
-	 * driver traffic.
-	 * NOTE: This enables fully-functional tablet mode.
-	 */
-	len = HUION_PRM_NUM * sizeof(*buf);
-	buf = kmalloc(len, GFP_KERNEL);
-	if (buf == NULL) {
-		hid_err(hdev, "failed to allocate parameter buffer\n");
-		rc = -ENOMEM;
-		goto cleanup;
-	}
-	rc = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-				USB_REQ_GET_DESCRIPTOR, USB_DIR_IN,
-				(USB_DT_STRING << 8) + 0x64,
-				0x0409, buf, len,
-				USB_CTRL_GET_TIMEOUT);
-	if (rc == -EPIPE) {
-		hid_err(hdev, "device parameters not found\n");
-		rc = -ENODEV;
-		goto cleanup;
-	} else if (rc < 0) {
-		hid_err(hdev, "failed to get device parameters: %d\n", rc);
-		rc = -ENODEV;
-		goto cleanup;
-	} else if (rc != len) {
-		hid_err(hdev, "invalid device parameters\n");
-		rc = -ENODEV;
-		goto cleanup;
-	}
-
-	/* Extract device parameters */
-	params[HUION_PH_ID_X_LM] = le16_to_cpu(buf[HUION_PRM_X_LM]);
-	params[HUION_PH_ID_Y_LM] = le16_to_cpu(buf[HUION_PRM_Y_LM]);
-	params[HUION_PH_ID_PRESSURE_LM] =
-		le16_to_cpu(buf[HUION_PRM_PRESSURE_LM]);
-	resolution = le16_to_cpu(buf[HUION_PRM_RESOLUTION]);
-	if (resolution == 0) {
-		params[HUION_PH_ID_X_PM] = 0;
-		params[HUION_PH_ID_Y_PM] = 0;
-	} else {
-		params[HUION_PH_ID_X_PM] = params[HUION_PH_ID_X_LM] *
-						1000 / resolution;
-		params[HUION_PH_ID_Y_PM] = params[HUION_PH_ID_Y_LM] *
-						1000 / resolution;
-	}
-
-	/* Allocate fixed report descriptor */
-	drvdata->rdesc = devm_kmalloc(&hdev->dev,
-				sizeof(huion_tablet_rdesc_template),
-				GFP_KERNEL);
-	if (drvdata->rdesc == NULL) {
-		hid_err(hdev, "failed to allocate fixed rdesc\n");
-		rc = -ENOMEM;
-		goto cleanup;
-	}
-	drvdata->rsize = sizeof(huion_tablet_rdesc_template);
-
-	/* Format fixed report descriptor */
-	memcpy(drvdata->rdesc, huion_tablet_rdesc_template,
-		drvdata->rsize);
-	for (p = drvdata->rdesc;
-	     p <= drvdata->rdesc + drvdata->rsize - 4;) {
-		if (p[0] == 0xFE && p[1] == 0xED && p[2] == 0x1D &&
-		    p[3] < sizeof(params)) {
-			v = params[p[3]];
-			put_unaligned(cpu_to_le32(v), (s32 *)p);
-			p += 4;
-		} else {
-			p++;
-		}
-	}
-
-	rc = 0;
-
-cleanup:
-	kfree(buf);
-	return rc;
-}
-
-static int huion_probe(struct hid_device *hdev, const struct hid_device_id *id)
-{
-	int rc;
-	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
-	struct huion_drvdata *drvdata;
-
-	/* Allocate and assign driver data */
-	drvdata = devm_kzalloc(&hdev->dev, sizeof(*drvdata), GFP_KERNEL);
-	if (drvdata == NULL) {
-		hid_err(hdev, "failed to allocate driver data\n");
-		return -ENOMEM;
-	}
-	hid_set_drvdata(hdev, drvdata);
-
-	switch (id->product) {
-	case USB_DEVICE_ID_HUION_TABLET:
-		/* If this is the pen interface */
-		if (intf->cur_altsetting->desc.bInterfaceNumber == 0) {
-			rc = huion_tablet_enable(hdev);
-			if (rc) {
-				hid_err(hdev, "tablet enabling failed\n");
-				return rc;
-			}
-		}
-		break;
-	}
-
-	rc = hid_parse(hdev);
-	if (rc) {
-		hid_err(hdev, "parse failed\n");
-		return rc;
-	}
-
-	rc = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
-	if (rc) {
-		hid_err(hdev, "hw start failed\n");
-		return rc;
-	}
-
-	return 0;
-}
-
-static int huion_raw_event(struct hid_device *hdev, struct hid_report *report,
-			u8 *data, int size)
-{
-	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
-
-	/* If this is a pen input report */
-	if (intf->cur_altsetting->desc.bInterfaceNumber == 0 &&
-	    report->type == HID_INPUT_REPORT &&
-	    report->id == 0x07 && size >= 2)
-		/* Invert the in-range bit */
-		data[1] ^= 0x40;
-
-	return 0;
-}
-
-static const struct hid_device_id huion_devices[] = {
-	{ HID_USB_DEVICE(USB_VENDOR_ID_HUION, USB_DEVICE_ID_HUION_TABLET) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_HUION_TABLET) },
-	{ }
-};
-MODULE_DEVICE_TABLE(hid, huion_devices);
-
-static struct hid_driver huion_driver = {
-	.name = "huion",
-	.id_table = huion_devices,
-	.probe = huion_probe,
-	.report_fixup = huion_report_fixup,
-	.raw_event = huion_raw_event,
-};
-module_hid_driver(huion_driver);
-
-MODULE_AUTHOR("Martin Rusko");
-MODULE_DESCRIPTION("Huion HID driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 204312bfab2c..41f167e4d75f 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -459,6 +459,11 @@
 #define USB_DEVICE_ID_UGCI_FLYING	0x0020
 #define USB_DEVICE_ID_UGCI_FIGHTING	0x0030
 
+#define USB_VENDOR_ID_HP		0x03f0
+#define USB_PRODUCT_ID_HP_LOGITECH_OEM_USB_OPTICAL_MOUSE_0A4A	0x0a4a
+#define USB_PRODUCT_ID_HP_LOGITECH_OEM_USB_OPTICAL_MOUSE_0B4A	0x0b4a
+#define USB_PRODUCT_ID_HP_PIXART_OEM_USB_OPTICAL_MOUSE		0x134a
+
 #define USB_VENDOR_ID_HUION		0x256c
 #define USB_DEVICE_ID_HUION_TABLET	0x006e
 
@@ -533,6 +538,7 @@
 #define USB_DEVICE_ID_KYE_MOUSEPEN_I608X	0x5011
 #define USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2	0x501a
 #define USB_DEVICE_ID_KYE_EASYPEN_M610X	0x5013
+#define USB_DEVICE_ID_KYE_PENSKETCH_M912	0x5015
 
 #define USB_VENDOR_ID_LABTEC		0x1020
 #define USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD	0x0006
@@ -586,10 +592,14 @@
 #define USB_VENDOR_ID_LOGITECH		0x046d
 #define USB_DEVICE_ID_LOGITECH_AUDIOHUB 0x0a0e
 #define USB_DEVICE_ID_LOGITECH_T651	0xb00c
+#define USB_DEVICE_ID_LOGITECH_C077	0xc007
 #define USB_DEVICE_ID_LOGITECH_RECEIVER	0xc101
 #define USB_DEVICE_ID_LOGITECH_HARMONY_FIRST  0xc110
 #define USB_DEVICE_ID_LOGITECH_HARMONY_LAST 0xc14f
 #define USB_DEVICE_ID_LOGITECH_HARMONY_PS3 0x0306
+#define USB_DEVICE_ID_LOGITECH_MOUSE_C01A	0xc01a
+#define USB_DEVICE_ID_LOGITECH_MOUSE_C05A	0xc05a
+#define USB_DEVICE_ID_LOGITECH_MOUSE_C06A	0xc06a
 #define USB_DEVICE_ID_LOGITECH_RUMBLEPAD_CORD	0xc20a
 #define USB_DEVICE_ID_LOGITECH_RUMBLEPAD	0xc211
 #define USB_DEVICE_ID_LOGITECH_EXTREME_3D	0xc215
@@ -898,6 +908,7 @@
 #define USB_VENDOR_ID_TIVO		0x150a
 #define USB_DEVICE_ID_TIVO_SLIDE_BT	0x1200
 #define USB_DEVICE_ID_TIVO_SLIDE	0x1201
+#define USB_DEVICE_ID_TIVO_SLIDE_PRO	0x1203
 
 #define USB_VENDOR_ID_TOPSEED		0x0766
 #define USB_DEVICE_ID_TOPSEED_CYBERLINK	0x0204
@@ -1020,6 +1031,7 @@
 #define USB_DEVICE_ID_ZYTRONIC_ZXY100	0x0005
 
 #define USB_VENDOR_ID_PRIMAX	0x0461
+#define USB_DEVICE_ID_PRIMAX_MOUSE_4D22	0x4d22
 #define USB_DEVICE_ID_PRIMAX_KEYBOARD	0x4e05
 
 
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 052869d0ab78..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,
@@ -711,6 +720,29 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		}
 		break;
 
+	case HID_UP_TELEPHONY:
+		switch (usage->hid & HID_USAGE) {
+		case 0x2f: map_key_clear(KEY_MICMUTE);		break;
+		case 0xb0: map_key_clear(KEY_NUMERIC_0);	break;
+		case 0xb1: map_key_clear(KEY_NUMERIC_1);	break;
+		case 0xb2: map_key_clear(KEY_NUMERIC_2);	break;
+		case 0xb3: map_key_clear(KEY_NUMERIC_3);	break;
+		case 0xb4: map_key_clear(KEY_NUMERIC_4);	break;
+		case 0xb5: map_key_clear(KEY_NUMERIC_5);	break;
+		case 0xb6: map_key_clear(KEY_NUMERIC_6);	break;
+		case 0xb7: map_key_clear(KEY_NUMERIC_7);	break;
+		case 0xb8: map_key_clear(KEY_NUMERIC_8);	break;
+		case 0xb9: map_key_clear(KEY_NUMERIC_9);	break;
+		case 0xba: map_key_clear(KEY_NUMERIC_STAR);	break;
+		case 0xbb: map_key_clear(KEY_NUMERIC_POUND);	break;
+		case 0xbc: map_key_clear(KEY_NUMERIC_A);	break;
+		case 0xbd: map_key_clear(KEY_NUMERIC_B);	break;
+		case 0xbe: map_key_clear(KEY_NUMERIC_C);	break;
+		case 0xbf: map_key_clear(KEY_NUMERIC_D);	break;
+		default: goto ignore;
+		}
+		break;
+
 	case HID_UP_CONSUMER:	/* USB HUT v1.12, pages 75-84 */
 		switch (usage->hid & HID_USAGE) {
 		case 0x000: goto ignore;
diff --git a/drivers/hid/hid-kye.c b/drivers/hid/hid-kye.c
index 158fcf577fae..32e6d8d9ded0 100644
--- a/drivers/hid/hid-kye.c
+++ b/drivers/hid/hid-kye.c
@@ -268,6 +268,137 @@ static __u8 easypen_m610x_rdesc_fixed[] = {
 	0xC0                          /*  End Collection                  */
 };
 
+
+/* Original PenSketch M912 report descriptor size */
+#define PENSKETCH_M912_RDESC_ORIG_SIZE	482
+
+/* Fixed PenSketch M912 report descriptor */
+static __u8 pensketch_m912_rdesc_fixed[] = {
+	0x05, 0x01,                   /*  Usage Page (Desktop),           */
+	0x08,                         /*  Usage (00h),                    */
+	0xA1, 0x01,                   /*  Collection (Application),       */
+	0x85, 0x05,                   /*    Report ID (5),                */
+	0x06, 0x00, 0xFF,             /*    Usage Page (FF00h),           */
+	0x09, 0x01,                   /*    Usage (01h),                  */
+	0x15, 0x81,                   /*    Logical Minimum (-127),       */
+	0x25, 0x7F,                   /*    Logical Maximum (127),        */
+	0x75, 0x08,                   /*    Report Size (8),              */
+	0x95, 0x07,                   /*    Report Count (7),             */
+	0xB1, 0x02,                   /*    Feature (Variable),           */
+	0xC0,                         /*  End Collection,                 */
+	0x05, 0x0D,                   /*  Usage Page (Digitizer),         */
+	0x09, 0x02,                   /*  Usage (Pen),                    */
+	0xA1, 0x01,                   /*  Collection (Application),       */
+	0x85, 0x10,                   /*    Report ID (16),               */
+	0x09, 0x20,                   /*    Usage (Stylus),               */
+	0xA0,                         /*    Collection (Physical),        */
+	0x09, 0x42,                   /*      Usage (Tip Switch),         */
+	0x09, 0x44,                   /*      Usage (Barrel Switch),      */
+	0x09, 0x46,                   /*      Usage (Tablet Pick),        */
+	0x14,                         /*      Logical Minimum (0),        */
+	0x25, 0x01,                   /*      Logical Maximum (1),        */
+	0x75, 0x01,                   /*      Report Size (1),            */
+	0x95, 0x03,                   /*      Report Count (3),           */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x95, 0x04,                   /*      Report Count (4),           */
+	0x81, 0x03,                   /*      Input (Constant, Variable), */
+	0x09, 0x32,                   /*      Usage (In Range),           */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x75, 0x10,                   /*      Report Size (16),           */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0xA4,                         /*      Push,                       */
+	0x05, 0x01,                   /*      Usage Page (Desktop),       */
+	0x55, 0xFD,                   /*      Unit Exponent (-3),         */
+	0x65, 0x13,                   /*      Unit (Inch),                */
+	0x14,                         /*      Logical Minimum (0),        */
+	0x34,                         /*      Physical Minimum (0),       */
+	0x09, 0x30,                   /*      Usage (X),                  */
+	0x27, 0x00, 0xF0, 0x00, 0x00, /*      Logical Maximum (61440),    */
+	0x46, 0xE0, 0x2E,             /*      Physical Maximum (12000),   */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x09, 0x31,                   /*      Usage (Y),                  */
+	0x27, 0x00, 0xB4, 0x00, 0x00, /*      Logical Maximum (46080),    */
+	0x46, 0x28, 0x23,             /*      Physical Maximum (9000),    */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0xB4,                         /*      Pop,                        */
+	0x09, 0x30,                   /*      Usage (Tip Pressure),       */
+	0x14,                         /*      Logical Minimum (0),        */
+	0x26, 0xFF, 0x07,             /*      Logical Maximum (2047),     */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0xC0,                         /*    End Collection,               */
+	0xC0,                         /*  End Collection,                 */
+	0x05, 0x0D,                   /*  Usage Page (Digitizer),         */
+	0x09, 0x21,                   /*  Usage (Puck),                   */
+	0xA1, 0x01,                   /*  Collection (Application),       */
+	0x85, 0x11,                   /*    Report ID (17),               */
+	0x09, 0x21,                   /*    Usage (Puck),                 */
+	0xA0,                         /*    Collection (Physical),        */
+	0x05, 0x09,                   /*      Usage Page (Button),        */
+	0x75, 0x01,                   /*      Report Size (1),            */
+	0x19, 0x01,                   /*      Usage Minimum (01h),        */
+	0x29, 0x03,                   /*      Usage Maximum (03h),        */
+	0x14,                         /*      Logical Minimum (0),        */
+	0x25, 0x01,                   /*      Logical Maximum (1),        */
+	0x95, 0x03,                   /*      Report Count (3),           */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x95, 0x04,                   /*      Report Count (4),           */
+	0x81, 0x01,                   /*      Input (Constant),           */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0x0B, 0x32, 0x00, 0x0D, 0x00, /*      Usage (Digitizer In Range), */
+	0x14,                         /*      Logical Minimum (0),        */
+	0x25, 0x01,                   /*      Logical Maximum (1),        */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0xA4,                         /*      Push,                       */
+	0x05, 0x01,                   /*      Usage Page (Desktop),       */
+	0x75, 0x10,                   /*      Report Size (16),           */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0x55, 0xFD,                   /*      Unit Exponent (-3),         */
+	0x65, 0x13,                   /*      Unit (Inch),                */
+	0x14,                         /*      Logical Minimum (0),        */
+	0x34,                         /*      Physical Minimum (0),       */
+	0x09, 0x30,                   /*      Usage (X),                  */
+	0x27, 0x00, 0xF0, 0x00, 0x00, /*      Logical Maximum (61440),    */
+	0x46, 0xE0, 0x2E,             /*      Physical Maximum (12000),   */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x09, 0x31,                   /*      Usage (Y),                  */
+	0x27, 0x00, 0xB4, 0x00, 0x00, /*      Logical Maximum (46080),    */
+	0x46, 0x28, 0x23,             /*      Physical Maximum (9000),    */
+	0x81, 0x02,                   /*      Input (Variable),           */
+	0x09, 0x38,                   /*      Usage (Wheel),              */
+	0x75, 0x08,                   /*      Report Size (8),            */
+	0x95, 0x01,                   /*      Report Count (1),           */
+	0x15, 0xFF,                   /*      Logical Minimum (-1),       */
+	0x25, 0x01,                   /*      Logical Maximum (1),        */
+	0x34,                         /*      Physical Minimum (0),       */
+	0x44,                         /*      Physical Maximum (0),       */
+	0x81, 0x06,                   /*      Input (Variable, Relative), */
+	0xB4,                         /*      Pop,                        */
+	0xC0,                         /*    End Collection,               */
+	0xC0,                         /*  End Collection,                 */
+	0x05, 0x0C,                   /*  Usage Page (Consumer),          */
+	0x09, 0x01,                   /*  Usage (Consumer Control),       */
+	0xA1, 0x01,                   /*  Collection (Application),       */
+	0x85, 0x12,                   /*    Report ID (18),               */
+	0x14,                         /*    Logical Minimum (0),          */
+	0x25, 0x01,                   /*    Logical Maximum (1),          */
+	0x75, 0x01,                   /*    Report Size (1),              */
+	0x95, 0x08,                   /*    Report Count (8),             */
+	0x05, 0x0C,                   /*    Usage Page (Consumer),        */
+	0x0A, 0x6A, 0x02,             /*    Usage (AC Delete),            */
+	0x0A, 0x1A, 0x02,             /*    Usage (AC Undo),              */
+	0x0A, 0x01, 0x02,             /*    Usage (AC New),               */
+	0x0A, 0x2F, 0x02,             /*    Usage (AC Zoom),              */
+	0x0A, 0x25, 0x02,             /*    Usage (AC Forward),           */
+	0x0A, 0x24, 0x02,             /*    Usage (AC Back),              */
+	0x0A, 0x2D, 0x02,             /*    Usage (AC Zoom In),           */
+	0x0A, 0x2E, 0x02,             /*    Usage (AC Zoom Out),          */
+	0x81, 0x02,                   /*    Input (Variable),             */
+	0x95, 0x30,                   /*    Report Count (48),            */
+	0x81, 0x03,                   /*    Input (Constant, Variable),   */
+	0xC0                          /*  End Collection                  */
+};
+
 static __u8 *kye_consumer_control_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize, int offset, const char *device_name) {
 	/*
@@ -335,6 +466,12 @@ static __u8 *kye_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 			*rsize = sizeof(easypen_m610x_rdesc_fixed);
 		}
 		break;
+	case USB_DEVICE_ID_KYE_PENSKETCH_M912:
+		if (*rsize == PENSKETCH_M912_RDESC_ORIG_SIZE) {
+			rdesc = pensketch_m912_rdesc_fixed;
+			*rsize = sizeof(pensketch_m912_rdesc_fixed);
+		}
+		break;
 	case USB_DEVICE_ID_GENIUS_GILA_GAMING_MOUSE:
 		rdesc = kye_consumer_control_fixup(hdev, rdesc, rsize, 104,
 					"Genius Gila Gaming Mouse");
@@ -418,6 +555,7 @@ static int kye_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X:
 	case USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2:
 	case USB_DEVICE_ID_KYE_EASYPEN_M610X:
+	case USB_DEVICE_ID_KYE_PENSKETCH_M912:
 		ret = kye_tablet_enable(hdev);
 		if (ret) {
 			hid_err(hdev, "tablet enabling failed\n");
@@ -457,6 +595,8 @@ static const struct hid_device_id kye_devices[] = {
 				USB_DEVICE_ID_GENIUS_GX_IMPERATOR) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE,
 				USB_DEVICE_ID_GENIUS_MANTICORE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE,
+				USB_DEVICE_ID_KYE_PENSKETCH_M912) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, kye_devices);
diff --git a/drivers/hid/hid-lg.c b/drivers/hid/hid-lg.c
index f91ff145db9a..b86c18e651ed 100644
--- a/drivers/hid/hid-lg.c
+++ b/drivers/hid/hid-lg.c
@@ -27,6 +27,7 @@
 #include "usbhid/usbhid.h"
 #include "hid-ids.h"
 #include "hid-lg.h"
+#include "hid-lg4ff.h"
 
 #define LG_RDESC		0x001
 #define LG_BAD_RELATIVE_KEYS	0x002
@@ -818,4 +819,10 @@ static struct hid_driver lg_driver = {
 };
 module_hid_driver(lg_driver);
 
+#ifdef CONFIG_LOGIWHEELS_FF
+int lg4ff_no_autoswitch = 0;
+module_param_named(lg4ff_no_autoswitch, lg4ff_no_autoswitch, int, S_IRUGO);
+MODULE_PARM_DESC(lg4ff_no_autoswitch, "Do not switch multimode wheels to their native mode automatically");
+#endif
+
 MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-lg.h b/drivers/hid/hid-lg.h
index 142ce3f5f055..10dd8f024135 100644
--- a/drivers/hid/hid-lg.h
+++ b/drivers/hid/hid-lg.h
@@ -24,16 +24,4 @@ int lg3ff_init(struct hid_device *hdev);
 static inline int lg3ff_init(struct hid_device *hdev) { return -1; }
 #endif
 
-#ifdef CONFIG_LOGIWHEELS_FF
-int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
-			     struct hid_usage *usage, __s32 value, struct lg_drv_data *drv_data);
-int lg4ff_init(struct hid_device *hdev);
-int lg4ff_deinit(struct hid_device *hdev);
-#else
-static inline int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
-					   struct hid_usage *usage, __s32 value, struct lg_drv_data *drv_data) { return 0; }
-static inline int lg4ff_init(struct hid_device *hdev) { return -1; }
-static inline int lg4ff_deinit(struct hid_device *hdev) { return -1; }
-#endif
-
 #endif
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index db0dd9b17e53..1232210b1cc5 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -30,23 +30,44 @@
 
 #include "usbhid/usbhid.h"
 #include "hid-lg.h"
+#include "hid-lg4ff.h"
 #include "hid-ids.h"
 
-#define DFGT_REV_MAJ 0x13
-#define DFGT_REV_MIN 0x22
-#define DFGT2_REV_MIN 0x26
-#define DFP_REV_MAJ 0x11
-#define DFP_REV_MIN 0x06
-#define FFEX_REV_MAJ 0x21
-#define FFEX_REV_MIN 0x00
-#define G25_REV_MAJ 0x12
-#define G25_REV_MIN 0x22
-#define G27_REV_MAJ 0x12
-#define G27_REV_MIN 0x38
-#define G27_2_REV_MIN 0x39
-
 #define to_hid_device(pdev) container_of(pdev, struct hid_device, dev)
 
+#define LG4FF_MMODE_IS_MULTIMODE 0
+#define LG4FF_MMODE_SWITCHED 1
+#define LG4FF_MMODE_NOT_MULTIMODE 2
+
+#define LG4FF_MODE_NATIVE_IDX 0
+#define LG4FF_MODE_DFEX_IDX 1
+#define LG4FF_MODE_DFP_IDX 2
+#define LG4FF_MODE_G25_IDX 3
+#define LG4FF_MODE_DFGT_IDX 4
+#define LG4FF_MODE_G27_IDX 5
+#define LG4FF_MODE_MAX_IDX 6
+
+#define LG4FF_MODE_NATIVE BIT(LG4FF_MODE_NATIVE_IDX)
+#define LG4FF_MODE_DFEX BIT(LG4FF_MODE_DFEX_IDX)
+#define LG4FF_MODE_DFP BIT(LG4FF_MODE_DFP_IDX)
+#define LG4FF_MODE_G25 BIT(LG4FF_MODE_G25_IDX)
+#define LG4FF_MODE_DFGT BIT(LG4FF_MODE_DFGT_IDX)
+#define LG4FF_MODE_G27 BIT(LG4FF_MODE_G27_IDX)
+
+#define LG4FF_DFEX_TAG "DF-EX"
+#define LG4FF_DFEX_NAME "Driving Force / Formula EX"
+#define LG4FF_DFP_TAG "DFP"
+#define LG4FF_DFP_NAME "Driving Force Pro"
+#define LG4FF_G25_TAG "G25"
+#define LG4FF_G25_NAME "G25 Racing Wheel"
+#define LG4FF_G27_TAG "G27"
+#define LG4FF_G27_NAME "G27 Racing Wheel"
+#define LG4FF_DFGT_TAG "DFGT"
+#define LG4FF_DFGT_NAME "Driving Force GT"
+
+#define LG4FF_FFEX_REV_MAJ 0x21
+#define LG4FF_FFEX_REV_MIN 0x00
+
 static void hid_lg4ff_set_range_dfp(struct hid_device *hid, u16 range);
 static void hid_lg4ff_set_range_g25(struct hid_device *hid, u16 range);
 
@@ -59,6 +80,10 @@ struct lg4ff_device_entry {
 	__u8  led_state;
 	struct led_classdev *led[5];
 #endif
+	u32 alternate_modes;
+	const char *real_tag;
+	const char *real_name;
+	u16 real_product_id;
 	struct list_head list;
 	void (*set_range)(struct hid_device *hid, u16 range);
 };
@@ -77,6 +102,35 @@ struct lg4ff_wheel {
 	void (*set_range)(struct hid_device *hid, u16 range);
 };
 
+struct lg4ff_compat_mode_switch {
+	const __u8 cmd_count;	/* Number of commands to send */
+	const __u8 cmd[];
+};
+
+struct lg4ff_wheel_ident_info {
+	const u16 mask;
+	const u16 result;
+	const u16 real_product_id;
+};
+
+struct lg4ff_wheel_ident_checklist {
+	const u32 count;
+	const struct lg4ff_wheel_ident_info *models[];
+};
+
+struct lg4ff_multimode_wheel {
+	const u16 product_id;
+	const u32 alternate_modes;
+	const char *real_tag;
+	const char *real_name;
+};
+
+struct lg4ff_alternate_mode {
+	const u16 product_id;
+	const char *tag;
+	const char *name;
+};
+
 static const struct lg4ff_wheel lg4ff_devices[] = {
 	{USB_DEVICE_ID_LOGITECH_WHEEL,       lg4ff_wheel_effects, 40, 270, NULL},
 	{USB_DEVICE_ID_LOGITECH_MOMO_WHEEL,  lg4ff_wheel_effects, 40, 270, NULL},
@@ -88,46 +142,106 @@ static const struct lg4ff_wheel lg4ff_devices[] = {
 	{USB_DEVICE_ID_LOGITECH_WII_WHEEL,   lg4ff_wheel_effects, 40, 270, NULL}
 };
 
-struct lg4ff_native_cmd {
-	const __u8 cmd_num;	/* Number of commands to send */
-	const __u8 cmd[];
+static const struct lg4ff_multimode_wheel lg4ff_multimode_wheels[] = {
+	{USB_DEVICE_ID_LOGITECH_DFP_WHEEL,
+	 LG4FF_MODE_NATIVE | LG4FF_MODE_DFP | LG4FF_MODE_DFEX,
+	 LG4FF_DFP_TAG, LG4FF_DFP_NAME},
+	{USB_DEVICE_ID_LOGITECH_G25_WHEEL,
+	 LG4FF_MODE_NATIVE | LG4FF_MODE_G25 | LG4FF_MODE_DFP | LG4FF_MODE_DFEX,
+	 LG4FF_G25_TAG, LG4FF_G25_NAME},
+	{USB_DEVICE_ID_LOGITECH_DFGT_WHEEL,
+	 LG4FF_MODE_NATIVE | LG4FF_MODE_DFGT | LG4FF_MODE_DFP | LG4FF_MODE_DFEX,
+	 LG4FF_DFGT_TAG, LG4FF_DFGT_NAME},
+	{USB_DEVICE_ID_LOGITECH_G27_WHEEL,
+	 LG4FF_MODE_NATIVE | LG4FF_MODE_G27 | LG4FF_MODE_G25 | LG4FF_MODE_DFP | LG4FF_MODE_DFEX,
+	 LG4FF_G27_TAG, LG4FF_G27_NAME},
 };
 
-struct lg4ff_usb_revision {
-	const __u16 rev_maj;
-	const __u16 rev_min;
-	const struct lg4ff_native_cmd *command;
+static const struct lg4ff_alternate_mode lg4ff_alternate_modes[] = {
+	[LG4FF_MODE_NATIVE_IDX] = {0, "native", ""},
+	[LG4FF_MODE_DFEX_IDX] = {USB_DEVICE_ID_LOGITECH_WHEEL, LG4FF_DFEX_TAG, LG4FF_DFEX_NAME},
+	[LG4FF_MODE_DFP_IDX] = {USB_DEVICE_ID_LOGITECH_DFP_WHEEL, LG4FF_DFP_TAG, LG4FF_DFP_NAME},
+	[LG4FF_MODE_G25_IDX] = {USB_DEVICE_ID_LOGITECH_G25_WHEEL, LG4FF_G25_TAG, LG4FF_G25_NAME},
+	[LG4FF_MODE_DFGT_IDX] = {USB_DEVICE_ID_LOGITECH_DFGT_WHEEL, LG4FF_DFGT_TAG, LG4FF_DFGT_NAME},
+	[LG4FF_MODE_G27_IDX] = {USB_DEVICE_ID_LOGITECH_G27_WHEEL, LG4FF_G27_TAG, LG4FF_G27_NAME}
 };
 
-static const struct lg4ff_native_cmd native_dfp = {
-	1,
-	{0xf8, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00}
+/* Multimode wheel identificators */
+static const struct lg4ff_wheel_ident_info lg4ff_dfp_ident_info = {
+	0xf000,
+	0x1000,
+	USB_DEVICE_ID_LOGITECH_DFP_WHEEL
+};
+
+static const struct lg4ff_wheel_ident_info lg4ff_g25_ident_info = {
+	0xff00,
+	0x1200,
+	USB_DEVICE_ID_LOGITECH_G25_WHEEL
+};
+
+static const struct lg4ff_wheel_ident_info lg4ff_g27_ident_info = {
+	0xfff0,
+	0x1230,
+	USB_DEVICE_ID_LOGITECH_G27_WHEEL
 };
 
-static const struct lg4ff_native_cmd native_dfgt = {
+static const struct lg4ff_wheel_ident_info lg4ff_dfgt_ident_info = {
+	0xff00,
+	0x1300,
+	USB_DEVICE_ID_LOGITECH_DFGT_WHEEL
+};
+
+/* Multimode wheel identification checklists */
+static const struct lg4ff_wheel_ident_checklist lg4ff_main_checklist = {
+	4,
+	{&lg4ff_dfgt_ident_info,
+	 &lg4ff_g27_ident_info,
+	 &lg4ff_g25_ident_info,
+	 &lg4ff_dfp_ident_info}
+};
+
+/* Compatibility mode switching commands */
+/* EXT_CMD9 - Understood by G27 and DFGT */
+static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext09_dfex = {
 	2,
-	{0xf8, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 1st command */
-	 0xf8, 0x09, 0x03, 0x01, 0x00, 0x00, 0x00}	/* 2nd command */
+	{0xf8, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,	/* Revert mode upon USB reset */
+	 0xf8, 0x09, 0x00, 0x01, 0x00, 0x00, 0x00}	/* Switch mode to DF-EX with detach */
 };
 
-static const struct lg4ff_native_cmd native_g25 = {
-	1,
-	{0xf8, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}
+static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext09_dfp = {
+	2,
+	{0xf8, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,	/* Revert mode upon USB reset */
+	 0xf8, 0x09, 0x01, 0x01, 0x00, 0x00, 0x00}	/* Switch mode to DFP with detach */
+};
+
+static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext09_g25 = {
+	2,
+	{0xf8, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,	/* Revert mode upon USB reset */
+	 0xf8, 0x09, 0x02, 0x01, 0x00, 0x00, 0x00}	/* Switch mode to G25 with detach */
+};
+
+static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext09_dfgt = {
+	2,
+	{0xf8, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,	/* Revert mode upon USB reset */
+	 0xf8, 0x09, 0x03, 0x01, 0x00, 0x00, 0x00}	/* Switch mode to DFGT with detach */
 };
 
-static const struct lg4ff_native_cmd native_g27 = {
+static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext09_g27 = {
 	2,
-	{0xf8, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 1st command */
-	 0xf8, 0x09, 0x04, 0x01, 0x00, 0x00, 0x00}	/* 2nd command */
+	{0xf8, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,	/* Revert mode upon USB reset */
+	 0xf8, 0x09, 0x04, 0x01, 0x00, 0x00, 0x00}	/* Switch mode to G27 with detach */
 };
 
-static const struct lg4ff_usb_revision lg4ff_revs[] = {
-	{DFGT_REV_MAJ, DFGT_REV_MIN, &native_dfgt},	/* Driving Force GT */
-	{DFGT_REV_MAJ, DFGT2_REV_MIN, &native_dfgt},	/* Driving Force GT v2 */
-	{DFP_REV_MAJ,  DFP_REV_MIN,  &native_dfp},	/* Driving Force Pro */
-	{G25_REV_MAJ,  G25_REV_MIN,  &native_g25},	/* G25 */
-	{G27_REV_MAJ,  G27_REV_MIN,  &native_g27},	/* G27 */
-	{G27_REV_MAJ,  G27_2_REV_MIN,  &native_g27},	/* G27 v2 */
+/* EXT_CMD1 - Understood by DFP, G25, G27 and DFGT */
+static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext01_dfp = {
+	1,
+	{0xf8, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00}
+};
+
+/* EXT_CMD16 - Understood by G25 and G27 */
+static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext16_g25 = {
+	1,
+	{0xf8, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
 /* Recalculates X axis value accordingly to currently selected range */
@@ -396,20 +510,216 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 }
 
-static void hid_lg4ff_switch_native(struct hid_device *hid, const struct lg4ff_native_cmd *cmd)
+static const struct lg4ff_compat_mode_switch *lg4ff_get_mode_switch_command(const u16 real_product_id, const u16 target_product_id)
+{
+	switch (real_product_id) {
+	case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+		switch (target_product_id) {
+		case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+			return &lg4ff_mode_switch_ext01_dfp;
+		/* DFP can only be switched to its native mode */
+		default:
+			return NULL;
+		}
+		break;
+	case USB_DEVICE_ID_LOGITECH_G25_WHEEL:
+		switch (target_product_id) {
+		case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+			return &lg4ff_mode_switch_ext01_dfp;
+		case USB_DEVICE_ID_LOGITECH_G25_WHEEL:
+			return &lg4ff_mode_switch_ext16_g25;
+		/* G25 can only be switched to DFP mode or its native mode */
+		default:
+			return NULL;
+		}
+		break;
+	case USB_DEVICE_ID_LOGITECH_G27_WHEEL:
+		switch (target_product_id) {
+		case USB_DEVICE_ID_LOGITECH_WHEEL:
+			return &lg4ff_mode_switch_ext09_dfex;
+		case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+			return &lg4ff_mode_switch_ext09_dfp;
+		case USB_DEVICE_ID_LOGITECH_G25_WHEEL:
+			return &lg4ff_mode_switch_ext09_g25;
+		case USB_DEVICE_ID_LOGITECH_G27_WHEEL:
+			return &lg4ff_mode_switch_ext09_g27;
+		/* G27 can only be switched to DF-EX, DFP, G25 or its native mode */
+		default:
+			return NULL;
+		}
+		break;
+	case USB_DEVICE_ID_LOGITECH_DFGT_WHEEL:
+		switch (target_product_id) {
+		case USB_DEVICE_ID_LOGITECH_WHEEL:
+			return &lg4ff_mode_switch_ext09_dfex;
+		case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+			return &lg4ff_mode_switch_ext09_dfp;
+		case USB_DEVICE_ID_LOGITECH_DFGT_WHEEL:
+			return &lg4ff_mode_switch_ext09_dfgt;
+		/* DFGT can only be switched to DF-EX, DFP or its native mode */
+		default:
+			return NULL;
+		}
+		break;
+	/* No other wheels have multiple modes */
+	default:
+		return NULL;
+	}
+}
+
+static int lg4ff_switch_compatibility_mode(struct hid_device *hid, const struct lg4ff_compat_mode_switch *s)
 {
 	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
 	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
-	__u8 i, j;
+	__s32 *value = report->field[0]->value;
+	u8 i;
 
-	j = 0;
-	while (j < 7*cmd->cmd_num) {
-		for (i = 0; i < 7; i++)
-			report->field[0]->value[i] = cmd->cmd[j++];
+	for (i = 0; i < s->cmd_count; i++) {
+		u8 j;
+
+		for (j = 0; j < 7; j++)
+			value[j] = s->cmd[j + (7*i)];
 
 		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
 	}
+	hid_hw_wait(hid);
+	return 0;
+}
+
+static ssize_t lg4ff_alternate_modes_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct hid_device *hid = to_hid_device(dev);
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	ssize_t count = 0;
+	int i;
+
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return 0;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return 0;
+	}
+
+	if (!entry->real_name) {
+		hid_err(hid, "NULL pointer to string\n");
+		return 0;
+	}
+
+	for (i = 0; i < LG4FF_MODE_MAX_IDX; i++) {
+		if (entry->alternate_modes & BIT(i)) {
+			/* Print tag and full name */
+			count += scnprintf(buf + count, PAGE_SIZE - count, "%s: %s",
+					   lg4ff_alternate_modes[i].tag,
+					   !lg4ff_alternate_modes[i].product_id ? entry->real_name : lg4ff_alternate_modes[i].name);
+			if (count >= PAGE_SIZE - 1)
+				return count;
+
+			/* Mark the currently active mode with an asterisk */
+			if (lg4ff_alternate_modes[i].product_id == entry->product_id ||
+			    (lg4ff_alternate_modes[i].product_id == 0 && entry->product_id == entry->real_product_id))
+				count += scnprintf(buf + count, PAGE_SIZE - count, " *\n");
+			else
+				count += scnprintf(buf + count, PAGE_SIZE - count, "\n");
+
+			if (count >= PAGE_SIZE - 1)
+				return count;
+		}
+	}
+
+	return count;
+}
+
+static ssize_t lg4ff_alternate_modes_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hid_device *hid = to_hid_device(dev);
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	const struct lg4ff_compat_mode_switch *s;
+	u16 target_product_id = 0;
+	int i, ret;
+	char *lbuf;
+
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return -EINVAL;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return -EINVAL;
+	}
+
+	/* Allow \n at the end of the input parameter */
+	lbuf = kasprintf(GFP_KERNEL, "%s", buf);
+	if (!lbuf)
+		return -ENOMEM;
+
+	i = strlen(lbuf);
+	if (lbuf[i-1] == '\n') {
+		if (i == 1) {
+			kfree(lbuf);
+			return -EINVAL;
+		}
+		lbuf[i-1] = '\0';
+	}
+
+	for (i = 0; i < LG4FF_MODE_MAX_IDX; i++) {
+		const u16 mode_product_id = lg4ff_alternate_modes[i].product_id;
+		const char *tag = lg4ff_alternate_modes[i].tag;
+
+		if (entry->alternate_modes & BIT(i)) {
+			if (!strcmp(tag, lbuf)) {
+				if (!mode_product_id)
+					target_product_id = entry->real_product_id;
+				else
+					target_product_id = mode_product_id;
+				break;
+			}
+		}
+	}
+
+	if (i == LG4FF_MODE_MAX_IDX) {
+		hid_info(hid, "Requested mode \"%s\" is not supported by the device\n", lbuf);
+		kfree(lbuf);
+		return -EINVAL;
+	}
+	kfree(lbuf); /* Not needed anymore */
+
+	if (target_product_id == entry->product_id) /* Nothing to do */
+		return count;
+
+	/* Automatic switching has to be disabled for the switch to DF-EX mode to work correctly */
+	if (target_product_id == USB_DEVICE_ID_LOGITECH_WHEEL && !lg4ff_no_autoswitch) {
+		hid_info(hid, "\"%s\" cannot be switched to \"DF-EX\" mode. Load the \"hid_logitech\" module with \"lg4ff_no_autoswitch=1\" parameter set and try again\n",
+			 entry->real_name);
+		return -EINVAL;
+	}
+
+	/* Take care of hardware limitations */
+	if ((entry->real_product_id == USB_DEVICE_ID_LOGITECH_DFP_WHEEL || entry->real_product_id == USB_DEVICE_ID_LOGITECH_G25_WHEEL) &&
+	    entry->product_id > target_product_id) {
+		hid_info(hid, "\"%s\" cannot be switched back into \"%s\" mode\n", entry->real_name, lg4ff_alternate_modes[i].name);
+		return -EINVAL;
+	}
+
+	s = lg4ff_get_mode_switch_command(entry->real_product_id, target_product_id);
+	if (!s) {
+		hid_err(hid, "Invalid target product ID %X\n", target_product_id);
+		return -EINVAL;
+	}
+
+	ret = lg4ff_switch_compatibility_mode(hid, s);
+	return (ret == 0 ? count : ret);
 }
+static DEVICE_ATTR(alternate_modes, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH, lg4ff_alternate_modes_show, lg4ff_alternate_modes_store);
 
 /* Read current range and display it in terminal */
 static ssize_t range_show(struct device *dev, struct device_attribute *attr,
@@ -472,6 +782,41 @@ static ssize_t range_store(struct device *dev, struct device_attribute *attr,
 }
 static DEVICE_ATTR_RW(range);
 
+static ssize_t lg4ff_real_id_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct hid_device *hid = to_hid_device(dev);
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	size_t count;
+
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return 0;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return 0;
+	}
+
+	if (!entry->real_tag || !entry->real_name) {
+		hid_err(hid, "NULL pointer to string\n");
+		return 0;
+	}
+
+	count = scnprintf(buf, PAGE_SIZE, "%s: %s\n", entry->real_tag, entry->real_name);
+	return count;
+}
+
+static ssize_t lg4ff_real_id_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	/* Real ID is a read-only value */
+	return -EPERM;
+}
+static DEVICE_ATTR(real_id, S_IRUGO, lg4ff_real_id_show, lg4ff_real_id_store);
+
 #ifdef CONFIG_LEDS_CLASS
 static void lg4ff_set_leds(struct hid_device *hid, __u8 leds)
 {
@@ -555,20 +900,119 @@ static enum led_brightness lg4ff_led_get_brightness(struct led_classdev *led_cde
 }
 #endif
 
+static u16 lg4ff_identify_multimode_wheel(struct hid_device *hid, const u16 reported_product_id, const u16 bcdDevice)
+{
+	const struct lg4ff_wheel_ident_checklist *checklist;
+	int i, from_idx, to_idx;
+
+	switch (reported_product_id) {
+	case USB_DEVICE_ID_LOGITECH_WHEEL:
+	case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
+		checklist = &lg4ff_main_checklist;
+		from_idx = 0;
+		to_idx = checklist->count - 1;
+		break;
+	case USB_DEVICE_ID_LOGITECH_G25_WHEEL:
+		checklist = &lg4ff_main_checklist;
+		from_idx = 0;
+		to_idx = checklist->count - 2; /* End identity check at G25 */
+		break;
+	case USB_DEVICE_ID_LOGITECH_G27_WHEEL:
+		checklist = &lg4ff_main_checklist;
+		from_idx = 1; /* Start identity check at G27 */
+		to_idx = checklist->count - 3; /* End identity check at G27 */
+		break;
+	case USB_DEVICE_ID_LOGITECH_DFGT_WHEEL:
+		checklist = &lg4ff_main_checklist;
+		from_idx = 0;
+		to_idx = checklist->count - 4; /* End identity check at DFGT */
+		break;
+	default:
+		return 0;
+	}
+
+	for (i = from_idx; i <= to_idx; i++) {
+		const u16 mask = checklist->models[i]->mask;
+		const u16 result = checklist->models[i]->result;
+		const u16 real_product_id = checklist->models[i]->real_product_id;
+
+		if ((bcdDevice & mask) == result) {
+			dbg_hid("Found wheel with real PID %X whose reported PID is %X\n", real_product_id, reported_product_id);
+			return real_product_id;
+		}
+	}
+
+	/* No match found. This is either Driving Force or an unknown
+	 * wheel model, do not touch it */
+	dbg_hid("Wheel with bcdDevice %X was not recognized as multimode wheel, leaving in its current mode\n", bcdDevice);
+	return 0;
+}
+
+static int lg4ff_handle_multimode_wheel(struct hid_device *hid, u16 *real_product_id, const u16 bcdDevice)
+{
+	const u16 reported_product_id = hid->product;
+	int ret;
+
+	*real_product_id = lg4ff_identify_multimode_wheel(hid, reported_product_id, bcdDevice);
+	/* Probed wheel is not a multimode wheel */
+	if (!*real_product_id) {
+		*real_product_id = reported_product_id;
+		dbg_hid("Wheel is not a multimode wheel\n");
+		return LG4FF_MMODE_NOT_MULTIMODE;
+	}
+
+	/* Switch from "Driving Force" mode to native mode automatically.
+	 * Otherwise keep the wheel in its current mode */
+	if (reported_product_id == USB_DEVICE_ID_LOGITECH_WHEEL &&
+	    reported_product_id != *real_product_id &&
+	    !lg4ff_no_autoswitch) {
+		const struct lg4ff_compat_mode_switch *s = lg4ff_get_mode_switch_command(*real_product_id, *real_product_id);
+
+		if (!s) {
+			hid_err(hid, "Invalid product id %X\n", *real_product_id);
+			return LG4FF_MMODE_NOT_MULTIMODE;
+		}
+
+		ret = lg4ff_switch_compatibility_mode(hid, s);
+		if (ret) {
+			/* Wheel could not have been switched to native mode,
+			 * leave it in "Driving Force" mode and continue */
+			hid_err(hid, "Unable to switch wheel mode, errno %d\n", ret);
+			return LG4FF_MMODE_IS_MULTIMODE;
+		}
+		return LG4FF_MMODE_SWITCHED;
+	}
+
+	return LG4FF_MMODE_IS_MULTIMODE;
+}
+
+
 int lg4ff_init(struct hid_device *hid)
 {
 	struct hid_input *hidinput = list_entry(hid->inputs.next, struct hid_input, list);
 	struct input_dev *dev = hidinput->input;
+	const struct usb_device_descriptor *udesc = &(hid_to_usb_dev(hid)->descriptor);
+	const u16 bcdDevice = le16_to_cpu(udesc->bcdDevice);
 	struct lg4ff_device_entry *entry;
 	struct lg_drv_data *drv_data;
-	struct usb_device_descriptor *udesc;
 	int error, i, j;
-	__u16 bcdDevice, rev_maj, rev_min;
+	int mmode_ret, mmode_idx = -1;
+	u16 real_product_id;
 
 	/* Check that the report looks ok */
 	if (!hid_validate_values(hid, HID_OUTPUT_REPORT, 0, 0, 7))
 		return -1;
 
+	/* Check if a multimode wheel has been connected and
+	 * handle it appropriately */
+	mmode_ret = lg4ff_handle_multimode_wheel(hid, &real_product_id, bcdDevice);
+
+	/* Wheel has been told to switch to native mode. There is no point in going on
+	 * with the initialization as the wheel will do a USB reset when it switches mode
+	 */
+	if (mmode_ret == LG4FF_MMODE_SWITCHED)
+		return 0;
+
 	/* Check what wheel has been connected */
 	for (i = 0; i < ARRAY_SIZE(lg4ff_devices); i++) {
 		if (hid->product == lg4ff_devices[i].product_id) {
@@ -583,25 +1027,15 @@ int lg4ff_init(struct hid_device *hid)
 		return -1;
 	}
 
-	/* Attempt to switch wheel to native mode when applicable */
-	udesc = &(hid_to_usb_dev(hid)->descriptor);
-	if (!udesc) {
-		hid_err(hid, "NULL USB device descriptor\n");
-		return -1;
-	}
-	bcdDevice = le16_to_cpu(udesc->bcdDevice);
-	rev_maj = bcdDevice >> 8;
-	rev_min = bcdDevice & 0xff;
-
-	if (lg4ff_devices[i].product_id == USB_DEVICE_ID_LOGITECH_WHEEL) {
-		dbg_hid("Generic wheel detected, can it do native?\n");
-		dbg_hid("USB revision: %2x.%02x\n", rev_maj, rev_min);
+	if (mmode_ret == LG4FF_MMODE_IS_MULTIMODE) {
+		for (mmode_idx = 0; mmode_idx < ARRAY_SIZE(lg4ff_multimode_wheels); mmode_idx++) {
+			if (real_product_id == lg4ff_multimode_wheels[mmode_idx].product_id)
+				break;
+		}
 
-		for (j = 0; j < ARRAY_SIZE(lg4ff_revs); j++) {
-			if (lg4ff_revs[j].rev_maj == rev_maj && lg4ff_revs[j].rev_min == rev_min) {
-				hid_lg4ff_switch_native(hid, lg4ff_revs[j].command);
-				hid_info(hid, "Switched to native mode\n");
-			}
+		if (mmode_idx == ARRAY_SIZE(lg4ff_multimode_wheels)) {
+			hid_err(hid, "Device product ID %X is not listed as a multimode wheel", real_product_id);
+			return -1;
 		}
 	}
 
@@ -630,14 +1064,23 @@ int lg4ff_init(struct hid_device *hid)
 	drv_data->device_props = entry;
 
 	entry->product_id = lg4ff_devices[i].product_id;
+	entry->real_product_id = real_product_id;
 	entry->min_range = lg4ff_devices[i].min_range;
 	entry->max_range = lg4ff_devices[i].max_range;
 	entry->set_range = lg4ff_devices[i].set_range;
+	if (mmode_ret == LG4FF_MMODE_IS_MULTIMODE) {
+		BUG_ON(mmode_idx == -1);
+		entry->alternate_modes = lg4ff_multimode_wheels[mmode_idx].alternate_modes;
+		entry->real_tag = lg4ff_multimode_wheels[mmode_idx].real_tag;
+		entry->real_name = lg4ff_multimode_wheels[mmode_idx].real_name;
+	}
 
 	/* Check if autocentering is available and
 	 * set the centering force to zero by default */
 	if (test_bit(FF_AUTOCENTER, dev->ffbit)) {
-		if (rev_maj == FFEX_REV_MAJ && rev_min == FFEX_REV_MIN)	/* Formula Force EX expects different autocentering command */
+		/* Formula Force EX expects different autocentering command */
+		if ((bcdDevice >> 8) == LG4FF_FFEX_REV_MAJ &&
+		    (bcdDevice & 0xff) == LG4FF_FFEX_REV_MIN)
 			dev->ff->set_autocenter = hid_lg4ff_set_autocenter_ffex;
 		else
 			dev->ff->set_autocenter = hid_lg4ff_set_autocenter_default;
@@ -649,6 +1092,14 @@ int lg4ff_init(struct hid_device *hid)
 	error = device_create_file(&hid->dev, &dev_attr_range);
 	if (error)
 		return error;
+	if (mmode_ret == LG4FF_MMODE_IS_MULTIMODE) {
+		error = device_create_file(&hid->dev, &dev_attr_real_id);
+		if (error)
+			return error;
+		error = device_create_file(&hid->dev, &dev_attr_alternate_modes);
+		if (error)
+			return error;
+	}
 	dbg_hid("sysfs interface created\n");
 
 	/* Set the maximum range to start with */
@@ -711,24 +1162,26 @@ out:
 	return 0;
 }
 
-
-
 int lg4ff_deinit(struct hid_device *hid)
 {
 	struct lg4ff_device_entry *entry;
 	struct lg_drv_data *drv_data;
 
-	device_remove_file(&hid->dev, &dev_attr_range);
-
 	drv_data = hid_get_drvdata(hid);
 	if (!drv_data) {
 		hid_err(hid, "Error while deinitializing device, no private driver data.\n");
 		return -1;
 	}
 	entry = drv_data->device_props;
-	if (!entry) {
-		hid_err(hid, "Error while deinitializing device, no device properties data.\n");
-		return -1;
+	if (!entry)
+		goto out; /* Nothing more to do */
+
+	device_remove_file(&hid->dev, &dev_attr_range);
+
+	/* Multimode devices will have at least the "MODE_NATIVE" bit set */
+	if (entry->alternate_modes) {
+		device_remove_file(&hid->dev, &dev_attr_real_id);
+		device_remove_file(&hid->dev, &dev_attr_alternate_modes);
 	}
 
 #ifdef CONFIG_LEDS_CLASS
@@ -752,6 +1205,7 @@ int lg4ff_deinit(struct hid_device *hid)
 	/* Deallocate memory */
 	kfree(entry);
 
+out:
 	dbg_hid("Device successfully unregistered\n");
 	return 0;
 }
diff --git a/drivers/hid/hid-lg4ff.h b/drivers/hid/hid-lg4ff.h
new file mode 100644
index 000000000000..5b6a5086c47f
--- /dev/null
+++ b/drivers/hid/hid-lg4ff.h
@@ -0,0 +1,18 @@
+#ifndef __HID_LG4FF_H
+#define __HID_LG4FF_H
+
+#ifdef CONFIG_LOGIWHEELS_FF
+extern int lg4ff_no_autoswitch; /* From hid-lg.c */
+
+int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
+			     struct hid_usage *usage, __s32 value, struct lg_drv_data *drv_data);
+int lg4ff_init(struct hid_device *hdev);
+int lg4ff_deinit(struct hid_device *hdev);
+#else
+static inline int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
+					   struct hid_usage *usage, __s32 value, struct lg_drv_data *drv_data) { return 0; }
+static inline int lg4ff_init(struct hid_device *hdev) { return -1; }
+static inline int lg4ff_deinit(struct hid_device *hdev) { return -1; }
+#endif
+
+#endif
diff --git a/drivers/hid/hid-logitech-hidpp.c b/drivers/hid/hid-logitech-hidpp.c
index e77658cd037c..b3cf6fd4be96 100644
--- a/drivers/hid/hid-logitech-hidpp.c
+++ b/drivers/hid/hid-logitech-hidpp.c
@@ -28,6 +28,11 @@ MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Benjamin Tissoires <benjamin.tissoires@gmail.com>");
 MODULE_AUTHOR("Nestor Lopez Casado <nlopezcasad@logitech.com>");
 
+static bool disable_raw_mode;
+module_param(disable_raw_mode, bool, 0644);
+MODULE_PARM_DESC(disable_raw_mode,
+	"Disable Raw mode reporting for touchpads and keep firmware gestures.");
+
 #define REPORT_ID_HIDPP_SHORT			0x10
 #define REPORT_ID_HIDPP_LONG			0x11
 
@@ -1188,6 +1193,11 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 
 	hidpp->quirks = id->driver_data;
 
+	if (disable_raw_mode) {
+		hidpp->quirks &= ~HIDPP_QUIRK_CLASS_WTP;
+		hidpp->quirks &= ~HIDPP_QUIRK_DELAYED_INIT;
+	}
+
 	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP) {
 		ret = wtp_allocate(hdev, id);
 		if (ret)
@@ -1210,6 +1220,7 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	connected = hidpp_is_connected(hidpp);
 	if (id->group != HID_GROUP_LOGITECH_DJ_DEVICE) {
 		if (!connected) {
+			ret = -ENODEV;
 			hid_err(hdev, "Device not connected");
 			hid_device_io_stop(hdev);
 			goto hid_parse_fail;
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index f65e78b46999..6a9b05b328a9 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -42,7 +42,6 @@
 #include <linux/hid.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/usb.h>
 #include <linux/input/mt.h>
 #include <linux/string.h>
 
@@ -72,6 +71,8 @@ MODULE_LICENSE("GPL");
 #define MT_INPUTMODE_TOUCHSCREEN	0x02
 #define MT_INPUTMODE_TOUCHPAD		0x03
 
+#define MT_BUTTONTYPE_CLICKPAD		0
+
 struct mt_slot {
 	__s32 x, y, cx, cy, p, w, h;
 	__s32 contactid;	/* the device ContactID assigned to this slot */
@@ -116,6 +117,8 @@ struct mt_device {
 	__u8 touches_by_report;	/* how many touches are present in one report:
 				* 1 means we should use a serial protocol
 				* > 1 means hybrid (multitouch) protocol */
+	__u8 buttons_count;	/* number of physical buttons per touchpad */
+	bool is_buttonpad;	/* is this device a button pad? */
 	bool serial_maybe;	/* need to check for serial protocol */
 	bool curvalid;		/* is the current contact valid? */
 	unsigned mt_flags;	/* flags to pass to input-mt */
@@ -334,6 +337,16 @@ static void mt_feature_mapping(struct hid_device *hdev,
 			td->maxcontacts = td->mtclass.maxcontacts;
 
 		break;
+	case HID_DG_BUTTONTYPE:
+		if (usage->usage_index >= field->report_count) {
+			dev_err(&hdev->dev, "HID_DG_BUTTONTYPE out of range\n");
+			break;
+		}
+
+		if (field->value[usage->usage_index] == MT_BUTTONTYPE_CLICKPAD)
+			td->is_buttonpad = true;
+
+		break;
 	}
 }
 
@@ -379,6 +392,10 @@ static int mt_touch_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		td->inputmode_value = MT_INPUTMODE_TOUCHPAD;
 	}
 
+	/* count the buttons on touchpads */
+	if ((usage->hid & HID_USAGE_PAGE) == HID_UP_BUTTON)
+		td->buttons_count++;
+
 	if (usage->usage_index)
 		prev_usage = &field->usage[usage->usage_index - 1];
 
@@ -728,6 +745,13 @@ static void mt_touch_input_configured(struct hid_device *hdev,
 	if (cls->quirks & MT_QUIRK_NOT_SEEN_MEANS_UP)
 		td->mt_flags |= INPUT_MT_DROP_UNUSED;
 
+	/* check for clickpads */
+	if ((td->mt_flags & INPUT_MT_POINTER) && (td->buttons_count == 1))
+		td->is_buttonpad = true;
+
+	if (td->is_buttonpad)
+		__set_bit(INPUT_PROP_BUTTONPAD, input->propbit);
+
 	input_mt_init_slots(input, td->maxcontacts, td->mt_flags);
 
 	td->mt_flags = 0;
diff --git a/drivers/hid/hid-rmi.c b/drivers/hid/hid-rmi.c
index 49d4fe4f5987..368ffdf2c0a3 100644
--- a/drivers/hid/hid-rmi.c
+++ b/drivers/hid/hid-rmi.c
@@ -104,6 +104,7 @@ struct rmi_data {
 
 	unsigned long flags;
 
+	struct rmi_function f01;
 	struct rmi_function f11;
 	struct rmi_function f30;
 
@@ -124,6 +125,7 @@ struct rmi_data {
 	struct hid_device *hdev;
 
 	unsigned long device_flags;
+	unsigned long firmware_id;
 };
 
 #define RMI_PAGE(addr) (((addr) >> 8) & 0xff)
@@ -272,6 +274,46 @@ static inline int rmi_read(struct hid_device *hdev, u16 addr, void *buf)
 	return rmi_read_block(hdev, addr, buf, 1);
 }
 
+static int rmi_write_block(struct hid_device *hdev, u16 addr, void *buf,
+		const int len)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	int ret;
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_PAGE(addr) != data->page) {
+		ret = rmi_set_page(hdev, RMI_PAGE(addr));
+		if (ret < 0)
+			goto exit;
+	}
+
+	data->writeReport[0] = RMI_WRITE_REPORT_ID;
+	data->writeReport[1] = len;
+	data->writeReport[2] = addr & 0xFF;
+	data->writeReport[3] = (addr >> 8) & 0xFF;
+	memcpy(&data->writeReport[4], buf, len);
+
+	ret = rmi_write_report(hdev, data->writeReport,
+					data->output_report_size);
+	if (ret < 0) {
+		dev_err(&hdev->dev,
+			"failed to write request output report (%d)\n",
+			ret);
+		goto exit;
+	}
+	ret = 0;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return ret;
+}
+
+static inline int rmi_write(struct hid_device *hdev, u16 addr, void *buf)
+{
+	return rmi_write_block(hdev, addr, buf, 1);
+}
+
 static void rmi_f11_process_touch(struct rmi_data *hdata, int slot,
 		u8 finger_state, u8 *touch_data)
 {
@@ -532,6 +574,9 @@ static void rmi_register_function(struct rmi_data *data,
 	u16 page_base = page << 8;
 
 	switch (pdt_entry->function_number) {
+	case 0x01:
+		f = &data->f01;
+		break;
 	case 0x11:
 		f = &data->f11;
 		break;
@@ -604,6 +649,92 @@ error_exit:
 	return retval;
 }
 
+#define RMI_DEVICE_F01_BASIC_QUERY_LEN	11
+
+static int rmi_populate_f01(struct hid_device *hdev)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	u8 basic_queries[RMI_DEVICE_F01_BASIC_QUERY_LEN];
+	u8 info[3];
+	int ret;
+	bool has_query42;
+	bool has_lts;
+	bool has_sensor_id;
+	bool has_ds4_queries = false;
+	bool has_build_id_query = false;
+	bool has_package_id_query = false;
+	u16 query_offset = data->f01.query_base_addr;
+	u16 prod_info_addr;
+	u8 ds4_query_len;
+
+	ret = rmi_read_block(hdev, query_offset, basic_queries,
+				RMI_DEVICE_F01_BASIC_QUERY_LEN);
+	if (ret) {
+		hid_err(hdev, "Can not read basic queries from Function 0x1.\n");
+		return ret;
+	}
+
+	has_lts = !!(basic_queries[0] & BIT(2));
+	has_sensor_id = !!(basic_queries[1] & BIT(3));
+	has_query42 = !!(basic_queries[1] & BIT(7));
+
+	query_offset += 11;
+	prod_info_addr = query_offset + 6;
+	query_offset += 10;
+
+	if (has_lts)
+		query_offset += 20;
+
+	if (has_sensor_id)
+		query_offset++;
+
+	if (has_query42) {
+		ret = rmi_read(hdev, query_offset, info);
+		if (ret) {
+			hid_err(hdev, "Can not read query42.\n");
+			return ret;
+		}
+		has_ds4_queries = !!(info[0] & BIT(0));
+		query_offset++;
+	}
+
+	if (has_ds4_queries) {
+		ret = rmi_read(hdev, query_offset, &ds4_query_len);
+		if (ret) {
+			hid_err(hdev, "Can not read DS4 Query length.\n");
+			return ret;
+		}
+		query_offset++;
+
+		if (ds4_query_len > 0) {
+			ret = rmi_read(hdev, query_offset, info);
+			if (ret) {
+				hid_err(hdev, "Can not read DS4 query.\n");
+				return ret;
+			}
+
+			has_package_id_query = !!(info[0] & BIT(0));
+			has_build_id_query = !!(info[0] & BIT(1));
+		}
+	}
+
+	if (has_package_id_query)
+		prod_info_addr++;
+
+	if (has_build_id_query) {
+		ret = rmi_read_block(hdev, prod_info_addr, info, 3);
+		if (ret) {
+			hid_err(hdev, "Can not read product info.\n");
+			return ret;
+		}
+
+		data->firmware_id = info[1] << 8 | info[0];
+		data->firmware_id += info[2] * 65536;
+	}
+
+	return 0;
+}
+
 static int rmi_populate_f11(struct hid_device *hdev)
 {
 	struct rmi_data *data = hid_get_drvdata(hdev);
@@ -620,6 +751,8 @@ static int rmi_populate_f11(struct hid_device *hdev)
 	bool has_gestures;
 	bool has_rel;
 	bool has_data40 = false;
+	bool has_dribble = false;
+	bool has_palm_detect = false;
 	unsigned x_size, y_size;
 	u16 query_offset;
 
@@ -661,6 +794,14 @@ static int rmi_populate_f11(struct hid_device *hdev)
 	has_rel = !!(buf[0] & BIT(3));
 	has_gestures = !!(buf[0] & BIT(5));
 
+	ret = rmi_read(hdev, data->f11.query_base_addr + 5, buf);
+	if (ret) {
+		hid_err(hdev, "can not get absolute data sources: %d.\n", ret);
+		return ret;
+	}
+
+	has_dribble = !!(buf[0] & BIT(4));
+
 	/*
 	 * At least 4 queries are guaranteed to be present in F11
 	 * +1 for query 5 which is present since absolute events are
@@ -680,6 +821,7 @@ static int rmi_populate_f11(struct hid_device *hdev)
 				ret);
 			return ret;
 		}
+		has_palm_detect = !!(buf[0] & BIT(0));
 		has_query10 = !!(buf[0] & BIT(2));
 
 		query_offset += 2; /* query 7 and 8 are present */
@@ -766,17 +908,38 @@ static int rmi_populate_f11(struct hid_device *hdev)
 	 * retrieve the ctrl registers
 	 * the ctrl register has a size of 20 but a fw bug split it into 16 + 4,
 	 * and there is no way to know if the first 20 bytes are here or not.
-	 * We use only the first 10 bytes, so get only them.
+	 * We use only the first 12 bytes, so get only them.
 	 */
-	ret = rmi_read_block(hdev, data->f11.control_base_addr, buf, 10);
+	ret = rmi_read_block(hdev, data->f11.control_base_addr, buf, 12);
 	if (ret) {
-		hid_err(hdev, "can not read ctrl block of size 10: %d.\n", ret);
+		hid_err(hdev, "can not read ctrl block of size 11: %d.\n", ret);
 		return ret;
 	}
 
 	data->max_x = buf[6] | (buf[7] << 8);
 	data->max_y = buf[8] | (buf[9] << 8);
 
+	if (has_dribble) {
+		buf[0] = buf[0] & ~BIT(6);
+		ret = rmi_write(hdev, data->f11.control_base_addr, buf);
+		if (ret) {
+			hid_err(hdev, "can not write to control reg 0: %d.\n",
+				ret);
+			return ret;
+		}
+	}
+
+	if (has_palm_detect) {
+		buf[11] = buf[11] & ~BIT(0);
+		ret = rmi_write(hdev, data->f11.control_base_addr + 11,
+				&buf[11]);
+		if (ret) {
+			hid_err(hdev, "can not write to control reg 11: %d.\n",
+				ret);
+			return ret;
+		}
+	}
+
 	return 0;
 }
 
@@ -858,6 +1021,12 @@ static int rmi_populate(struct hid_device *hdev)
 		return ret;
 	}
 
+	ret = rmi_populate_f01(hdev);
+	if (ret) {
+		hid_err(hdev, "Error while initializing F01 (%d).\n", ret);
+		return ret;
+	}
+
 	ret = rmi_populate_f11(hdev);
 	if (ret) {
 		hid_err(hdev, "Error while initializing F11 (%d).\n", ret);
@@ -907,6 +1076,8 @@ static void rmi_input_configured(struct hid_device *hdev, struct hid_input *hi)
 	if (ret)
 		goto exit;
 
+	hid_info(hdev, "firmware id: %ld\n", data->firmware_id);
+
 	__set_bit(EV_ABS, input->evbit);
 	input_set_abs_params(input, ABS_MT_POSITION_X, 1, data->max_x, 0, 0);
 	input_set_abs_params(input, ABS_MT_POSITION_Y, 1, data->max_y, 0, 0);
diff --git a/drivers/hid/hid-sensor-custom.c b/drivers/hid/hid-sensor-custom.c
new file mode 100644
index 000000000000..5614fee82347
--- /dev/null
+++ b/drivers/hid/hid-sensor-custom.c
@@ -0,0 +1,849 @@
+/*
+ * hid-sensor-custom.c
+ * 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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/miscdevice.h>
+#include <linux/kfifo.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/poll.h>
+#include <linux/bsearch.h>
+#include <linux/platform_device.h>
+#include <linux/hid-sensor-hub.h>
+
+#define HID_CUSTOM_NAME_LENGTH		64
+#define HID_CUSTOM_MAX_CORE_ATTRS	10
+#define HID_CUSTOM_TOTAL_ATTRS		(HID_CUSTOM_MAX_CORE_ATTRS + 1)
+#define HID_CUSTOM_FIFO_SIZE		4096
+#define HID_CUSTOM_MAX_FEATURE_BYTES	64
+
+struct hid_sensor_custom_field {
+	int report_id;
+	char group_name[HID_CUSTOM_NAME_LENGTH];
+	struct hid_sensor_hub_attribute_info attribute;
+	struct device_attribute sd_attrs[HID_CUSTOM_MAX_CORE_ATTRS];
+	char attr_name[HID_CUSTOM_TOTAL_ATTRS][HID_CUSTOM_NAME_LENGTH];
+	struct attribute *attrs[HID_CUSTOM_TOTAL_ATTRS];
+	struct attribute_group hid_custom_attribute_group;
+};
+
+struct hid_sensor_custom {
+	struct mutex mutex;
+	struct platform_device *pdev;
+	struct hid_sensor_hub_device *hsdev;
+	struct hid_sensor_hub_callbacks callbacks;
+	int sensor_field_count;
+	struct hid_sensor_custom_field *fields;
+	int input_field_count;
+	int input_report_size;
+	int input_report_recd_size;
+	bool input_skip_sample;
+	bool enable;
+	struct hid_sensor_custom_field *power_state;
+	struct hid_sensor_custom_field *report_state;
+	struct miscdevice custom_dev;
+	struct kfifo data_fifo;
+	unsigned long misc_opened;
+	wait_queue_head_t wait;
+};
+
+/* Header for each sample to user space via dev interface */
+struct hid_sensor_sample {
+	u32 usage_id;
+	u64 timestamp;
+	u32 raw_len;
+} __packed;
+
+static struct attribute hid_custom_attrs[] = {
+	{.name = "name", .mode = S_IRUGO},
+	{.name = "units", .mode = S_IRUGO},
+	{.name = "unit-expo", .mode = S_IRUGO},
+	{.name = "minimum", .mode = S_IRUGO},
+	{.name = "maximum", .mode = S_IRUGO},
+	{.name = "size", .mode = S_IRUGO},
+	{.name = "value", .mode = S_IWUSR | S_IRUGO},
+	{.name = NULL}
+};
+
+static const struct hid_custom_usage_desc {
+	int usage_id;
+	char *desc;
+} hid_custom_usage_desc_table[] = {
+	{0x200201,	"event-sensor-state"},
+	{0x200202,	"event-sensor-event"},
+	{0x200301,	"property-friendly-name"},
+	{0x200302,	"property-persistent-unique-id"},
+	{0x200303,	"property-sensor-status"},
+	{0x200304,	"property-min-report-interval"},
+	{0x200305,	"property-sensor-manufacturer"},
+	{0x200306,	"property-sensor-model"},
+	{0x200307,	"property-sensor-serial-number"},
+	{0x200308,	"property-sensor-description"},
+	{0x200309,	"property-sensor-connection-type"},
+	{0x20030A,	"property-sensor-device-path"},
+	{0x20030B,	"property-hardware-revision"},
+	{0x20030C,	"property-firmware-version"},
+	{0x20030D,	"property-release-date"},
+	{0x20030E,	"property-report-interval"},
+	{0x20030F,	"property-change-sensitivity-absolute"},
+	{0x200310,	"property-change-sensitivity-percent-range"},
+	{0x200311,	"property-change-sensitivity-percent-relative"},
+	{0x200312,	"property-accuracy"},
+	{0x200313,	"property-resolution"},
+	{0x200314,	"property-maximum"},
+	{0x200315,	"property-minimum"},
+	{0x200316,	"property-reporting-state"},
+	{0x200317,	"property-sampling-rate"},
+	{0x200318,	"property-response-curve"},
+	{0x200319,	"property-power-state"},
+	{0x200540,	"data-field-custom"},
+	{0x200541,	"data-field-custom-usage"},
+	{0x200542,	"data-field-custom-boolean-array"},
+	{0x200543,	"data-field-custom-value"},
+	{0x200544,	"data-field-custom-value_1"},
+	{0x200545,	"data-field-custom-value_2"},
+	{0x200546,	"data-field-custom-value_3"},
+	{0x200547,	"data-field-custom-value_4"},
+	{0x200548,	"data-field-custom-value_5"},
+	{0x200549,	"data-field-custom-value_6"},
+	{0x20054A,	"data-field-custom-value_7"},
+	{0x20054B,	"data-field-custom-value_8"},
+	{0x20054C,	"data-field-custom-value_9"},
+	{0x20054D,	"data-field-custom-value_10"},
+	{0x20054E,	"data-field-custom-value_11"},
+	{0x20054F,	"data-field-custom-value_12"},
+	{0x200550,	"data-field-custom-value_13"},
+	{0x200551,	"data-field-custom-value_14"},
+	{0x200552,	"data-field-custom-value_15"},
+	{0x200553,	"data-field-custom-value_16"},
+	{0x200554,	"data-field-custom-value_17"},
+	{0x200555,	"data-field-custom-value_18"},
+	{0x200556,	"data-field-custom-value_19"},
+	{0x200557,	"data-field-custom-value_20"},
+	{0x200558,	"data-field-custom-value_21"},
+	{0x200559,	"data-field-custom-value_22"},
+	{0x20055A,	"data-field-custom-value_23"},
+	{0x20055B,	"data-field-custom-value_24"},
+	{0x20055C,	"data-field-custom-value_25"},
+	{0x20055D,	"data-field-custom-value_26"},
+	{0x20055E,	"data-field-custom-value_27"},
+	{0x20055F,	"data-field-custom-value_28"},
+};
+
+static int usage_id_cmp(const void *p1, const void *p2)
+{
+	if (*(int *)p1 < *(int *)p2)
+		return -1;
+
+	if (*(int *)p1 > *(int *)p2)
+		return 1;
+
+	return 0;
+}
+
+static ssize_t enable_sensor_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct hid_sensor_custom *sensor_inst = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", sensor_inst->enable);
+}
+
+static int set_power_report_state(struct hid_sensor_custom *sensor_inst,
+				  bool state)
+{
+	int power_val = -1;
+	int report_val = -1;
+	u32 power_state_usage_id;
+	u32 report_state_usage_id;
+	int ret;
+
+	/*
+	 * It is possible that the power/report state ids are not present.
+	 * In this case this function will return success. But if the
+	 * ids are present, then it will return error if set fails.
+	 */
+	if (state) {
+		power_state_usage_id =
+			HID_USAGE_SENSOR_PROP_POWER_STATE_D0_FULL_POWER_ENUM;
+		report_state_usage_id =
+			HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM;
+	} else {
+		power_state_usage_id =
+			HID_USAGE_SENSOR_PROP_POWER_STATE_D4_POWER_OFF_ENUM;
+		report_state_usage_id =
+			HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM;
+	}
+
+	if (sensor_inst->power_state)
+		power_val = hid_sensor_get_usage_index(sensor_inst->hsdev,
+				sensor_inst->power_state->attribute.report_id,
+				sensor_inst->power_state->attribute.index,
+				power_state_usage_id);
+	if (sensor_inst->report_state)
+		report_val = hid_sensor_get_usage_index(sensor_inst->hsdev,
+				sensor_inst->report_state->attribute.report_id,
+				sensor_inst->report_state->attribute.index,
+				report_state_usage_id);
+
+	if (power_val >= 0) {
+		power_val +=
+			sensor_inst->power_state->attribute.logical_minimum;
+		ret = sensor_hub_set_feature(sensor_inst->hsdev,
+				sensor_inst->power_state->attribute.report_id,
+				sensor_inst->power_state->attribute.index,
+				sizeof(power_val),
+				&power_val);
+		if (ret) {
+			hid_err(sensor_inst->hsdev->hdev,
+				"Set power state failed\n");
+			return ret;
+		}
+	}
+
+	if (report_val >= 0) {
+		report_val +=
+			sensor_inst->report_state->attribute.logical_minimum;
+		ret = sensor_hub_set_feature(sensor_inst->hsdev,
+				sensor_inst->report_state->attribute.report_id,
+				sensor_inst->report_state->attribute.index,
+				sizeof(report_val),
+				&report_val);
+		if (ret) {
+			hid_err(sensor_inst->hsdev->hdev,
+				"Set report state failed\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static ssize_t enable_sensor_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct hid_sensor_custom *sensor_inst = platform_get_drvdata(pdev);
+	int value;
+	int ret = -EINVAL;
+
+	if (kstrtoint(buf, 0, &value) != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor_inst->mutex);
+	if (value && !sensor_inst->enable) {
+		ret = sensor_hub_device_open(sensor_inst->hsdev);
+		if (ret)
+			goto unlock_state;
+
+		ret = set_power_report_state(sensor_inst, true);
+		if (ret) {
+			sensor_hub_device_close(sensor_inst->hsdev);
+			goto unlock_state;
+		}
+		sensor_inst->enable = true;
+	} else if (!value && sensor_inst->enable) {
+		ret = set_power_report_state(sensor_inst, false);
+		sensor_hub_device_close(sensor_inst->hsdev);
+		sensor_inst->enable = false;
+	}
+unlock_state:
+	mutex_unlock(&sensor_inst->mutex);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+static DEVICE_ATTR_RW(enable_sensor);
+
+static struct attribute *enable_sensor_attrs[] = {
+	&dev_attr_enable_sensor.attr,
+	NULL,
+};
+
+static struct attribute_group enable_sensor_attr_group = {
+	.attrs = enable_sensor_attrs,
+};
+
+static ssize_t show_value(struct device *dev, struct device_attribute *attr,
+			  char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct hid_sensor_custom *sensor_inst = platform_get_drvdata(pdev);
+	struct hid_sensor_hub_attribute_info *attribute;
+	int index, usage, field_index;
+	char name[HID_CUSTOM_NAME_LENGTH];
+	bool feature = false;
+	bool input = false;
+	int value = 0;
+
+	if (sscanf(attr->attr.name, "feature-%d-%x-%s", &index, &usage,
+		   name) == 3) {
+		feature = true;
+		field_index = index + sensor_inst->input_field_count;
+	} else if (sscanf(attr->attr.name, "input-%d-%x-%s", &index, &usage,
+		   name) == 3) {
+		input = true;
+		field_index = index;
+	} else
+		return -EINVAL;
+
+	if (!strncmp(name, "value", strlen("value"))) {
+		u32 report_id;
+		int ret;
+
+		attribute = &sensor_inst->fields[field_index].attribute;
+		report_id = attribute->report_id;
+		if (feature) {
+			u8 values[HID_CUSTOM_MAX_FEATURE_BYTES];
+			int len = 0;
+			u64 value = 0;
+			int i = 0;
+
+			ret = sensor_hub_get_feature(sensor_inst->hsdev,
+						     report_id,
+						     index,
+						     sizeof(values), values);
+			if (ret < 0)
+				return ret;
+
+			while (i < ret) {
+				if (i + attribute->size > ret) {
+					len += snprintf(&buf[len],
+							PAGE_SIZE - len,
+							"%d ", values[i]);
+					break;
+				}
+				switch (attribute->size) {
+				case 2:
+					value = (u64) *(u16 *)&values[i];
+					i += attribute->size;
+					break;
+				case 4:
+					value = (u64) *(u32 *)&values[i];
+					i += attribute->size;
+					break;
+				case 8:
+					value = *(u64 *)&values[i];
+					i += attribute->size;
+					break;
+				default:
+					value = (u64) values[i];
+					++i;
+					break;
+				}
+				len += snprintf(&buf[len], PAGE_SIZE - len,
+						"%lld ", value);
+			}
+			len += snprintf(&buf[len], PAGE_SIZE - len, "\n");
+
+			return len;
+		} else if (input)
+			value = sensor_hub_input_attr_get_raw_value(
+						sensor_inst->hsdev,
+						sensor_inst->hsdev->usage,
+						usage, report_id,
+						SENSOR_HUB_SYNC);
+	} else if (!strncmp(name, "units", strlen("units")))
+		value = sensor_inst->fields[field_index].attribute.units;
+	else if (!strncmp(name, "unit-expo", strlen("unit-expo")))
+		value = sensor_inst->fields[field_index].attribute.unit_expo;
+	else if (!strncmp(name, "size", strlen("size")))
+		value = sensor_inst->fields[field_index].attribute.size;
+	else if (!strncmp(name, "minimum", strlen("minimum")))
+		value = sensor_inst->fields[field_index].attribute.
+							logical_minimum;
+	else if (!strncmp(name, "maximum", strlen("maximum")))
+		value = sensor_inst->fields[field_index].attribute.
+							logical_maximum;
+	else if (!strncmp(name, "name", strlen("name"))) {
+		struct hid_custom_usage_desc *usage_desc;
+
+		usage_desc = bsearch(&usage, hid_custom_usage_desc_table,
+				     ARRAY_SIZE(hid_custom_usage_desc_table),
+				     sizeof(struct hid_custom_usage_desc),
+				     usage_id_cmp);
+		if (usage_desc)
+			return snprintf(buf, PAGE_SIZE, "%s\n",
+					usage_desc->desc);
+		else
+			return sprintf(buf, "not-specified\n");
+	 } else
+		return -EINVAL;
+
+	return sprintf(buf, "%d\n", value);
+}
+
+static ssize_t store_value(struct device *dev, struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct hid_sensor_custom *sensor_inst = platform_get_drvdata(pdev);
+	int index, field_index, usage;
+	char name[HID_CUSTOM_NAME_LENGTH];
+	int value;
+
+	if (sscanf(attr->attr.name, "feature-%d-%x-%s", &index, &usage,
+		   name) == 3) {
+		field_index = index + sensor_inst->input_field_count;
+	} else
+		return -EINVAL;
+
+	if (!strncmp(name, "value", strlen("value"))) {
+		u32 report_id;
+		int ret;
+
+		if (kstrtoint(buf, 0, &value) != 0)
+			return -EINVAL;
+
+		report_id = sensor_inst->fields[field_index].attribute.
+								report_id;
+		ret = sensor_hub_set_feature(sensor_inst->hsdev, report_id,
+					     index, sizeof(value), &value);
+	} else
+		return -EINVAL;
+
+	return count;
+}
+
+static int hid_sensor_capture_sample(struct hid_sensor_hub_device *hsdev,
+				  unsigned usage_id, size_t raw_len,
+				  char *raw_data, void *priv)
+{
+	struct hid_sensor_custom *sensor_inst = platform_get_drvdata(priv);
+	struct hid_sensor_sample header;
+
+	/* If any error occurs in a sample, rest of the fields are ignored */
+	if (sensor_inst->input_skip_sample) {
+		hid_err(sensor_inst->hsdev->hdev, "Skipped remaining data\n");
+		return 0;
+	}
+
+	hid_dbg(sensor_inst->hsdev->hdev, "%s received %d of %d\n", __func__,
+		(int) (sensor_inst->input_report_recd_size + raw_len),
+		sensor_inst->input_report_size);
+
+	if (!test_bit(0, &sensor_inst->misc_opened))
+		return 0;
+
+	if (!sensor_inst->input_report_recd_size) {
+		int required_size = sizeof(struct hid_sensor_sample) +
+						sensor_inst->input_report_size;
+		header.usage_id = hsdev->usage;
+		header.raw_len = sensor_inst->input_report_size;
+		header.timestamp = ktime_get_real_ns();
+		if (kfifo_avail(&sensor_inst->data_fifo) >= required_size) {
+			kfifo_in(&sensor_inst->data_fifo,
+				 (unsigned char *)&header,
+				 sizeof(header));
+		} else
+			sensor_inst->input_skip_sample = true;
+	}
+	if (kfifo_avail(&sensor_inst->data_fifo) >= raw_len)
+		kfifo_in(&sensor_inst->data_fifo, (unsigned char *)raw_data,
+			 raw_len);
+
+	sensor_inst->input_report_recd_size += raw_len;
+
+	return 0;
+}
+
+static int hid_sensor_send_event(struct hid_sensor_hub_device *hsdev,
+				 unsigned usage_id, void *priv)
+{
+	struct hid_sensor_custom *sensor_inst = platform_get_drvdata(priv);
+
+	if (!test_bit(0, &sensor_inst->misc_opened))
+		return 0;
+
+	sensor_inst->input_report_recd_size = 0;
+	sensor_inst->input_skip_sample = false;
+
+	wake_up(&sensor_inst->wait);
+
+	return 0;
+}
+
+static int hid_sensor_custom_add_field(struct hid_sensor_custom *sensor_inst,
+				       int index, int report_type,
+				       struct hid_report *report,
+				       struct hid_field *field)
+{
+	struct hid_sensor_custom_field *sensor_field;
+	void *fields;
+
+	fields = krealloc(sensor_inst->fields,
+			  (sensor_inst->sensor_field_count + 1) *
+			   sizeof(struct hid_sensor_custom_field), GFP_KERNEL);
+	if (!fields) {
+		kfree(sensor_inst->fields);
+		return -ENOMEM;
+	}
+	sensor_inst->fields = fields;
+	sensor_field = &sensor_inst->fields[sensor_inst->sensor_field_count];
+	sensor_field->attribute.usage_id = sensor_inst->hsdev->usage;
+	if (field->logical)
+		sensor_field->attribute.attrib_id = field->logical;
+	else
+		sensor_field->attribute.attrib_id = field->usage[0].hid;
+
+	sensor_field->attribute.index = index;
+	sensor_field->attribute.report_id = report->id;
+	sensor_field->attribute.units = field->unit;
+	sensor_field->attribute.unit_expo = field->unit_exponent;
+	sensor_field->attribute.size = (field->report_size / 8);
+	sensor_field->attribute.logical_minimum = field->logical_minimum;
+	sensor_field->attribute.logical_maximum = field->logical_maximum;
+
+	if (report_type == HID_FEATURE_REPORT)
+		snprintf(sensor_field->group_name,
+			 sizeof(sensor_field->group_name), "feature-%x-%x",
+			 sensor_field->attribute.index,
+			 sensor_field->attribute.attrib_id);
+	else if (report_type == HID_INPUT_REPORT) {
+		snprintf(sensor_field->group_name,
+			 sizeof(sensor_field->group_name),
+			 "input-%x-%x", sensor_field->attribute.index,
+			 sensor_field->attribute.attrib_id);
+		sensor_inst->input_field_count++;
+		sensor_inst->input_report_size += (field->report_size *
+						   field->report_count) / 8;
+	}
+
+	memset(&sensor_field->hid_custom_attribute_group, 0,
+	       sizeof(struct attribute_group));
+	sensor_inst->sensor_field_count++;
+
+	return 0;
+}
+
+static int hid_sensor_custom_add_fields(struct hid_sensor_custom *sensor_inst,
+					struct hid_report_enum *report_enum,
+					int report_type)
+{
+	int i;
+	int ret;
+	struct hid_report *report;
+	struct hid_field *field;
+	struct hid_sensor_hub_device *hsdev = sensor_inst->hsdev;
+
+	list_for_each_entry(report, &report_enum->report_list, list) {
+		for (i = 0; i < report->maxfield; ++i) {
+			field = report->field[i];
+			if (field->maxusage &&
+			    ((field->usage[0].collection_index >=
+			      hsdev->start_collection_index) &&
+			      (field->usage[0].collection_index <
+			       hsdev->end_collection_index))) {
+
+				ret = hid_sensor_custom_add_field(sensor_inst,
+								  i,
+								  report_type,
+								  report,
+								  field);
+				if (ret)
+					return ret;
+
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int hid_sensor_custom_add_attributes(struct hid_sensor_custom
+								*sensor_inst)
+{
+	struct hid_sensor_hub_device *hsdev = sensor_inst->hsdev;
+	struct hid_device *hdev = hsdev->hdev;
+	int ret = -1;
+	int i, j;
+
+	for (j = 0; j < HID_REPORT_TYPES; ++j) {
+		if (j == HID_OUTPUT_REPORT)
+			continue;
+
+		ret = hid_sensor_custom_add_fields(sensor_inst,
+						   &hdev->report_enum[j], j);
+		if (ret)
+			return ret;
+
+	}
+
+	/* Create sysfs attributes */
+	for (i = 0; i < sensor_inst->sensor_field_count; ++i) {
+		j = 0;
+		while (j < HID_CUSTOM_TOTAL_ATTRS &&
+		       hid_custom_attrs[j].name) {
+			struct device_attribute *device_attr;
+
+			device_attr = &sensor_inst->fields[i].sd_attrs[j];
+
+			snprintf((char *)&sensor_inst->fields[i].attr_name[j],
+				 HID_CUSTOM_NAME_LENGTH, "%s-%s",
+				 sensor_inst->fields[i].group_name,
+				 hid_custom_attrs[j].name);
+			sysfs_attr_init(&device_attr->attr);
+			device_attr->attr.name =
+				(char *)&sensor_inst->fields[i].attr_name[j];
+			device_attr->attr.mode = hid_custom_attrs[j].mode;
+			device_attr->show = show_value;
+			if (hid_custom_attrs[j].mode & S_IWUSR)
+				device_attr->store = store_value;
+			sensor_inst->fields[i].attrs[j] = &device_attr->attr;
+			++j;
+		}
+		sensor_inst->fields[i].attrs[j] = NULL;
+		sensor_inst->fields[i].hid_custom_attribute_group.attrs =
+						sensor_inst->fields[i].attrs;
+		sensor_inst->fields[i].hid_custom_attribute_group.name =
+					sensor_inst->fields[i].group_name;
+		ret = sysfs_create_group(&sensor_inst->pdev->dev.kobj,
+					 &sensor_inst->fields[i].
+					 hid_custom_attribute_group);
+		if (ret)
+			break;
+
+		/* For power or report field store indexes */
+		if (sensor_inst->fields[i].attribute.attrib_id ==
+					HID_USAGE_SENSOR_PROY_POWER_STATE)
+			sensor_inst->power_state = &sensor_inst->fields[i];
+		else if (sensor_inst->fields[i].attribute.attrib_id ==
+					HID_USAGE_SENSOR_PROP_REPORT_STATE)
+			sensor_inst->report_state = &sensor_inst->fields[i];
+	}
+
+	return ret;
+}
+
+static void hid_sensor_custom_remove_attributes(struct hid_sensor_custom *
+								sensor_inst)
+{
+	int i;
+
+	for (i = 0; i < sensor_inst->sensor_field_count; ++i)
+		sysfs_remove_group(&sensor_inst->pdev->dev.kobj,
+				   &sensor_inst->fields[i].
+				   hid_custom_attribute_group);
+
+	kfree(sensor_inst->fields);
+}
+
+static ssize_t hid_sensor_custom_read(struct file *file, char __user *buf,
+				      size_t count, loff_t *f_ps)
+{
+	struct hid_sensor_custom *sensor_inst;
+	unsigned int copied;
+	int ret;
+
+	sensor_inst = container_of(file->private_data,
+				   struct hid_sensor_custom, custom_dev);
+
+	if (count < sizeof(struct hid_sensor_sample))
+		return -EINVAL;
+
+	do {
+		if (kfifo_is_empty(&sensor_inst->data_fifo)) {
+			if (file->f_flags & O_NONBLOCK)
+				return -EAGAIN;
+
+			ret = wait_event_interruptible(sensor_inst->wait,
+				!kfifo_is_empty(&sensor_inst->data_fifo));
+			if (ret)
+				return ret;
+		}
+		ret = kfifo_to_user(&sensor_inst->data_fifo, buf, count,
+				    &copied);
+		if (ret)
+			return ret;
+
+	} while (copied == 0);
+
+	return copied;
+}
+
+static int hid_sensor_custom_release(struct inode *inode, struct file *file)
+{
+	struct hid_sensor_custom *sensor_inst;
+
+	sensor_inst = container_of(file->private_data,
+				   struct hid_sensor_custom, custom_dev);
+
+	clear_bit(0, &sensor_inst->misc_opened);
+
+	return 0;
+}
+
+static int hid_sensor_custom_open(struct inode *inode, struct file *file)
+{
+	struct hid_sensor_custom *sensor_inst;
+
+	sensor_inst = container_of(file->private_data,
+				   struct hid_sensor_custom, custom_dev);
+	/* We essentially have single reader and writer */
+	if (test_and_set_bit(0, &sensor_inst->misc_opened))
+		return -EBUSY;
+
+	return nonseekable_open(inode, file);
+}
+
+static unsigned int hid_sensor_custom_poll(struct file *file,
+					   struct poll_table_struct *wait)
+{
+	struct hid_sensor_custom *sensor_inst;
+	unsigned int mask = 0;
+
+	sensor_inst = container_of(file->private_data,
+				   struct hid_sensor_custom, custom_dev);
+
+	poll_wait(file, &sensor_inst->wait, wait);
+
+	if (!kfifo_is_empty(&sensor_inst->data_fifo))
+		mask = POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+static const struct file_operations hid_sensor_custom_fops = {
+	.open =  hid_sensor_custom_open,
+	.read =  hid_sensor_custom_read,
+	.release = hid_sensor_custom_release,
+	.poll = hid_sensor_custom_poll,
+	.llseek = noop_llseek,
+};
+
+static int hid_sensor_custom_dev_if_add(struct hid_sensor_custom *sensor_inst)
+{
+	int ret;
+
+	ret = kfifo_alloc(&sensor_inst->data_fifo, HID_CUSTOM_FIFO_SIZE,
+			  GFP_KERNEL);
+	if (ret)
+		return ret;
+
+	init_waitqueue_head(&sensor_inst->wait);
+
+	sensor_inst->custom_dev.minor = MISC_DYNAMIC_MINOR;
+	sensor_inst->custom_dev.name = dev_name(&sensor_inst->pdev->dev);
+	sensor_inst->custom_dev.fops = &hid_sensor_custom_fops,
+	ret = misc_register(&sensor_inst->custom_dev);
+	if (ret) {
+		kfifo_free(&sensor_inst->data_fifo);
+		return ret;
+	}
+	return 0;
+}
+
+static void hid_sensor_custom_dev_if_remove(struct hid_sensor_custom
+								*sensor_inst)
+{
+	wake_up(&sensor_inst->wait);
+	misc_deregister(&sensor_inst->custom_dev);
+	kfifo_free(&sensor_inst->data_fifo);
+
+}
+
+static int hid_sensor_custom_probe(struct platform_device *pdev)
+{
+	struct hid_sensor_custom *sensor_inst;
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	int ret;
+
+	sensor_inst = devm_kzalloc(&pdev->dev, sizeof(*sensor_inst),
+				   GFP_KERNEL);
+	if (!sensor_inst)
+		return -ENOMEM;
+
+	sensor_inst->callbacks.capture_sample = hid_sensor_capture_sample;
+	sensor_inst->callbacks.send_event = hid_sensor_send_event;
+	sensor_inst->callbacks.pdev = pdev;
+	sensor_inst->hsdev = hsdev;
+	sensor_inst->pdev = pdev;
+	mutex_init(&sensor_inst->mutex);
+	platform_set_drvdata(pdev, sensor_inst);
+	ret = sensor_hub_register_callback(hsdev, hsdev->usage,
+					   &sensor_inst->callbacks);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "callback reg failed\n");
+		return ret;
+	}
+
+	ret = sysfs_create_group(&sensor_inst->pdev->dev.kobj,
+				 &enable_sensor_attr_group);
+	if (ret)
+		goto err_remove_callback;
+
+	ret = hid_sensor_custom_add_attributes(sensor_inst);
+	if (ret)
+		goto err_remove_group;
+
+	ret = hid_sensor_custom_dev_if_add(sensor_inst);
+	if (ret)
+		goto err_remove_attributes;
+
+	return 0;
+
+err_remove_attributes:
+	hid_sensor_custom_remove_attributes(sensor_inst);
+err_remove_group:
+	sysfs_remove_group(&sensor_inst->pdev->dev.kobj,
+			   &enable_sensor_attr_group);
+err_remove_callback:
+	sensor_hub_remove_callback(hsdev, hsdev->usage);
+
+	return ret;
+}
+
+static int hid_sensor_custom_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_custom *sensor_inst = platform_get_drvdata(pdev);
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+
+	hid_sensor_custom_dev_if_remove(sensor_inst);
+	hid_sensor_custom_remove_attributes(sensor_inst);
+	sysfs_remove_group(&sensor_inst->pdev->dev.kobj,
+			   &enable_sensor_attr_group);
+	sensor_hub_remove_callback(hsdev, hsdev->usage);
+
+	return 0;
+}
+
+static struct platform_device_id hid_sensor_custom_ids[] = {
+	{
+		.name = "HID-SENSOR-2000e1",
+	},
+	{
+		.name = "HID-SENSOR-2000e2",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, hid_sensor_custom_ids);
+
+static struct platform_driver hid_sensor_custom_platform_driver = {
+	.id_table = hid_sensor_custom_ids,
+	.driver = {
+		.name	= KBUILD_MODNAME,
+	},
+	.probe		= hid_sensor_custom_probe,
+	.remove		= hid_sensor_custom_remove,
+};
+module_platform_driver(hid_sensor_custom_platform_driver);
+
+MODULE_DESCRIPTION("HID Sensor Custom and Generic sensor Driver");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index e54ce1097e2c..c3f6f1e311ea 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -29,29 +29,10 @@
 #define HID_SENSOR_HUB_ENUM_QUIRK	0x01
 
 /**
- * struct sensor_hub_pending - Synchronous read pending information
- * @status:		Pending status true/false.
- * @ready:		Completion synchronization data.
- * @usage_id:		Usage id for physical device, E.g. Gyro usage id.
- * @attr_usage_id:	Usage Id of a field, E.g. X-AXIS for a gyro.
- * @raw_size:		Response size for a read request.
- * @raw_data:		Place holder for received response.
- */
-struct sensor_hub_pending {
-	bool status;
-	struct completion ready;
-	u32 usage_id;
-	u32 attr_usage_id;
-	int raw_size;
-	u8  *raw_data;
-};
-
-/**
  * struct sensor_hub_data - Hold a instance data for a HID hub device
  * @hsdev:		Stored hid instance for current hub device.
  * @mutex:		Mutex to serialize synchronous request.
  * @lock:		Spin lock to protect pending request structure.
- * @pending:		Holds information of pending sync read request.
  * @dyn_callback_list:	Holds callback function
  * @dyn_callback_lock:	spin lock to protect callback list
  * @hid_sensor_hub_client_devs:	Stores all MFD cells for a hub instance.
@@ -61,7 +42,6 @@ struct sensor_hub_pending {
 struct sensor_hub_data {
 	struct mutex mutex;
 	spinlock_t lock;
-	struct sensor_hub_pending pending;
 	struct list_head dyn_callback_list;
 	spinlock_t dyn_callback_lock;
 	struct mfd_cell *hid_sensor_hub_client_devs;
@@ -106,7 +86,8 @@ static int sensor_hub_get_physical_device_count(struct hid_device *hdev)
 
 	for (i = 0; i < hdev->maxcollection; ++i) {
 		struct hid_collection *collection = &hdev->collection[i];
-		if (collection->type == HID_COLLECTION_PHYSICAL)
+		if (collection->type == HID_COLLECTION_PHYSICAL ||
+		    collection->type == HID_COLLECTION_APPLICATION)
 			++count;
 	}
 
@@ -139,7 +120,8 @@ static struct hid_sensor_hub_callbacks *sensor_hub_get_callback(
 
 	spin_lock_irqsave(&pdata->dyn_callback_lock, flags);
 	list_for_each_entry(callback, &pdata->dyn_callback_list, list)
-		if (callback->usage_id == usage_id &&
+		if ((callback->usage_id == usage_id ||
+		     callback->usage_id == HID_USAGE_SENSOR_COLLECTION) &&
 			(collection_index >=
 				callback->hsdev->start_collection_index) &&
 			(collection_index <
@@ -179,7 +161,18 @@ int sensor_hub_register_callback(struct hid_sensor_hub_device *hsdev,
 	callback->usage_callback = usage_callback;
 	callback->usage_id = usage_id;
 	callback->priv = NULL;
-	list_add_tail(&callback->list, &pdata->dyn_callback_list);
+	/*
+	 * If there is a handler registered for the collection type, then
+	 * it will handle all reports for sensors in this collection. If
+	 * there is also an individual sensor handler registration, then
+	 * we want to make sure that the reports are directed to collection
+	 * handler, as this may be a fusion sensor. So add collection handlers
+	 * to the beginning of the list, so that they are matched first.
+	 */
+	if (usage_id == HID_USAGE_SENSOR_COLLECTION)
+		list_add(&callback->list, &pdata->dyn_callback_list);
+	else
+		list_add_tail(&callback->list, &pdata->dyn_callback_list);
 	spin_unlock_irqrestore(&pdata->dyn_callback_lock, flags);
 
 	return 0;
@@ -208,10 +201,14 @@ int sensor_hub_remove_callback(struct hid_sensor_hub_device *hsdev,
 EXPORT_SYMBOL_GPL(sensor_hub_remove_callback);
 
 int sensor_hub_set_feature(struct hid_sensor_hub_device *hsdev, u32 report_id,
-				u32 field_index, s32 value)
+			   u32 field_index, int buffer_size, void *buffer)
 {
 	struct hid_report *report;
 	struct sensor_hub_data *data = hid_get_drvdata(hsdev->hdev);
+	__s32 *buf32 = buffer;
+	int i = 0;
+	int remaining_bytes;
+	__s32 value;
 	int ret = 0;
 
 	mutex_lock(&data->mutex);
@@ -220,7 +217,21 @@ int sensor_hub_set_feature(struct hid_sensor_hub_device *hsdev, u32 report_id,
 		ret = -EINVAL;
 		goto done_proc;
 	}
-	hid_set_field(report->field[field_index], 0, value);
+
+	remaining_bytes = do_div(buffer_size, sizeof(__s32));
+	if (buffer_size) {
+		for (i = 0; i < buffer_size; ++i) {
+			hid_set_field(report->field[field_index], i,
+				      (__force __s32)cpu_to_le32(*buf32));
+			++buf32;
+		}
+	}
+	if (remaining_bytes) {
+		value = 0;
+		memcpy(&value, (u8 *)buf32, remaining_bytes);
+		hid_set_field(report->field[field_index], i,
+			      (__force __s32)cpu_to_le32(value));
+	}
 	hid_hw_request(hsdev->hdev, report, HID_REQ_SET_REPORT);
 	hid_hw_wait(hsdev->hdev);
 
@@ -232,10 +243,11 @@ done_proc:
 EXPORT_SYMBOL_GPL(sensor_hub_set_feature);
 
 int sensor_hub_get_feature(struct hid_sensor_hub_device *hsdev, u32 report_id,
-				u32 field_index, s32 *value)
+			   u32 field_index, int buffer_size, void *buffer)
 {
 	struct hid_report *report;
 	struct sensor_hub_data *data = hid_get_drvdata(hsdev->hdev);
+	int report_size;
 	int ret = 0;
 
 	mutex_lock(&data->mutex);
@@ -247,7 +259,17 @@ int sensor_hub_get_feature(struct hid_sensor_hub_device *hsdev, u32 report_id,
 	}
 	hid_hw_request(hsdev->hdev, report, HID_REQ_GET_REPORT);
 	hid_hw_wait(hsdev->hdev);
-	*value = report->field[field_index]->value[0];
+
+	/* calculate number of bytes required to read this field */
+	report_size = DIV_ROUND_UP(report->field[field_index]->report_size,
+				   8) *
+				   report->field[field_index]->report_count;
+	if (!report_size) {
+		ret = -EINVAL;
+		goto done_proc;
+	}
+	ret = min(report_size, buffer_size);
+	memcpy(buffer, report->field[field_index]->value, ret);
 
 done_proc:
 	mutex_unlock(&data->mutex);
@@ -259,47 +281,54 @@ EXPORT_SYMBOL_GPL(sensor_hub_get_feature);
 
 int sensor_hub_input_attr_get_raw_value(struct hid_sensor_hub_device *hsdev,
 					u32 usage_id,
-					u32 attr_usage_id, u32 report_id)
+					u32 attr_usage_id, u32 report_id,
+					enum sensor_hub_read_flags flag)
 {
 	struct sensor_hub_data *data = hid_get_drvdata(hsdev->hdev);
 	unsigned long flags;
 	struct hid_report *report;
 	int ret_val = 0;
 
-	mutex_lock(&data->mutex);
-	memset(&data->pending, 0, sizeof(data->pending));
-	init_completion(&data->pending.ready);
-	data->pending.usage_id = usage_id;
-	data->pending.attr_usage_id = attr_usage_id;
-	data->pending.raw_size = 0;
-
-	spin_lock_irqsave(&data->lock, flags);
-	data->pending.status = true;
-	spin_unlock_irqrestore(&data->lock, flags);
-	report = sensor_hub_report(report_id, hsdev->hdev, HID_INPUT_REPORT);
+	report = sensor_hub_report(report_id, hsdev->hdev,
+				   HID_INPUT_REPORT);
 	if (!report)
-		goto err_free;
-
-	hid_hw_request(hsdev->hdev, report, HID_REQ_GET_REPORT);
-	wait_for_completion_interruptible_timeout(&data->pending.ready, HZ*5);
-	switch (data->pending.raw_size) {
-	case 1:
-		ret_val = *(u8 *)data->pending.raw_data;
-		break;
-	case 2:
-		ret_val = *(u16 *)data->pending.raw_data;
-		break;
-	case 4:
-		ret_val = *(u32 *)data->pending.raw_data;
-		break;
-	default:
-		ret_val = 0;
+		return -EINVAL;
+
+	mutex_lock(&hsdev->mutex);
+	if (flag == SENSOR_HUB_SYNC) {
+		memset(&hsdev->pending, 0, sizeof(hsdev->pending));
+		init_completion(&hsdev->pending.ready);
+		hsdev->pending.usage_id = usage_id;
+		hsdev->pending.attr_usage_id = attr_usage_id;
+		hsdev->pending.raw_size = 0;
+
+		spin_lock_irqsave(&data->lock, flags);
+		hsdev->pending.status = true;
+		spin_unlock_irqrestore(&data->lock, flags);
 	}
-	kfree(data->pending.raw_data);
-
-err_free:
-	data->pending.status = false;
+	mutex_lock(&data->mutex);
+	hid_hw_request(hsdev->hdev, report, HID_REQ_GET_REPORT);
 	mutex_unlock(&data->mutex);
+	if (flag == SENSOR_HUB_SYNC) {
+		wait_for_completion_interruptible_timeout(
+						&hsdev->pending.ready, HZ*5);
+		switch (hsdev->pending.raw_size) {
+		case 1:
+			ret_val = *(u8 *)hsdev->pending.raw_data;
+			break;
+		case 2:
+			ret_val = *(u16 *)hsdev->pending.raw_data;
+			break;
+		case 4:
+			ret_val = *(u32 *)hsdev->pending.raw_data;
+			break;
+		default:
+			ret_val = 0;
+		}
+		kfree(hsdev->pending.raw_data);
+		hsdev->pending.status = false;
+	}
+	mutex_unlock(&hsdev->mutex);
 
 	return ret_val;
 }
@@ -455,16 +484,6 @@ static int sensor_hub_raw_event(struct hid_device *hdev,
 					report->field[i]->report_count)/8);
 		sz = (report->field[i]->report_size *
 					report->field[i]->report_count)/8;
-		if (pdata->pending.status && pdata->pending.attr_usage_id ==
-				report->field[i]->usage->hid) {
-			hid_dbg(hdev, "data was pending ...\n");
-			pdata->pending.raw_data = kmemdup(ptr, sz, GFP_ATOMIC);
-			if (pdata->pending.raw_data)
-				pdata->pending.raw_size = sz;
-			else
-				pdata->pending.raw_size = 0;
-			complete(&pdata->pending.ready);
-		}
 		collection = &hdev->collection[
 				report->field[i]->usage->collection_index];
 		hid_dbg(hdev, "collection->usage %x\n",
@@ -474,8 +493,23 @@ static int sensor_hub_raw_event(struct hid_device *hdev,
 				report->field[i]->physical,
 				report->field[i]->usage[0].collection_index,
 				&hsdev, &priv);
-
-		if (callback && callback->capture_sample) {
+		if (!callback) {
+			ptr += sz;
+			continue;
+		}
+		if (hsdev->pending.status && (hsdev->pending.attr_usage_id ==
+					      report->field[i]->usage->hid ||
+					      hsdev->pending.attr_usage_id ==
+					      report->field[i]->logical)) {
+			hid_dbg(hdev, "data was pending ...\n");
+			hsdev->pending.raw_data = kmemdup(ptr, sz, GFP_ATOMIC);
+			if (hsdev->pending.raw_data)
+				hsdev->pending.raw_size = sz;
+			else
+				hsdev->pending.raw_size = 0;
+			complete(&hsdev->pending.ready);
+		}
+		if (callback->capture_sample) {
 			if (report->field[i]->logical)
 				callback->capture_sample(hsdev,
 					report->field[i]->logical, sz, ptr,
@@ -572,6 +606,7 @@ static int sensor_hub_probe(struct hid_device *hdev,
 	int dev_cnt;
 	struct hid_sensor_hub_device *hsdev;
 	struct hid_sensor_hub_device *last_hsdev = NULL;
+	struct hid_sensor_hub_device *collection_hsdev = NULL;
 
 	sd = devm_kzalloc(&hdev->dev, sizeof(*sd), GFP_KERNEL);
 	if (!sd) {
@@ -618,7 +653,8 @@ static int sensor_hub_probe(struct hid_device *hdev,
 	for (i = 0; i < hdev->maxcollection; ++i) {
 		struct hid_collection *collection = &hdev->collection[i];
 
-		if (collection->type == HID_COLLECTION_PHYSICAL) {
+		if (collection->type == HID_COLLECTION_PHYSICAL ||
+		    collection->type == HID_COLLECTION_APPLICATION) {
 
 			hsdev = devm_kzalloc(&hdev->dev, sizeof(*hsdev),
 					     GFP_KERNEL);
@@ -630,6 +666,8 @@ static int sensor_hub_probe(struct hid_device *hdev,
 			hsdev->hdev = hdev;
 			hsdev->vendor_id = hdev->vendor;
 			hsdev->product_id = hdev->product;
+			hsdev->usage = collection->usage;
+			mutex_init(&hsdev->mutex);
 			hsdev->start_collection_index = i;
 			if (last_hsdev)
 				last_hsdev->end_collection_index = i;
@@ -653,10 +691,17 @@ static int sensor_hub_probe(struct hid_device *hdev,
 			hid_dbg(hdev, "Adding %s:%d\n", name,
 					hsdev->start_collection_index);
 			sd->hid_sensor_client_cnt++;
+			if (collection_hsdev)
+				collection_hsdev->end_collection_index = i;
+			if (collection->type == HID_COLLECTION_APPLICATION &&
+			    collection->usage == HID_USAGE_SENSOR_COLLECTION)
+				collection_hsdev = hsdev;
 		}
 	}
 	if (last_hsdev)
 		last_hsdev->end_collection_index = i;
+	if (collection_hsdev)
+		collection_hsdev->end_collection_index = i;
 
 	ret = mfd_add_hotplug_devices(&hdev->dev,
 			sd->hid_sensor_hub_client_devs,
@@ -676,13 +721,18 @@ static void sensor_hub_remove(struct hid_device *hdev)
 {
 	struct sensor_hub_data *data = hid_get_drvdata(hdev);
 	unsigned long flags;
+	int i;
 
 	hid_dbg(hdev, " hardware removed\n");
 	hid_hw_close(hdev);
 	hid_hw_stop(hdev);
 	spin_lock_irqsave(&data->lock, flags);
-	if (data->pending.status)
-		complete(&data->pending.ready);
+	for (i = 0; i < data->hid_sensor_client_cnt; ++i) {
+		struct hid_sensor_hub_device *hsdev =
+			data->hid_sensor_hub_client_devs[i].platform_data;
+		if (hsdev->pending.status)
+			complete(&hsdev->pending.ready);
+	}
 	spin_unlock_irqrestore(&data->lock, flags);
 	mfd_remove_devices(&hdev->dev);
 	hid_set_drvdata(hdev, NULL);
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 1896c019e302..6ca96cebb44c 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -802,7 +802,8 @@ union sixaxis_output_report_01 {
 #define DS4_REPORT_0x05_SIZE 32
 #define DS4_REPORT_0x11_SIZE 78
 #define DS4_REPORT_0x81_SIZE 7
-#define SIXAXIS_REPORT_0xF2_SIZE 18
+#define SIXAXIS_REPORT_0xF2_SIZE 17
+#define SIXAXIS_REPORT_0xF5_SIZE 8
 
 static DEFINE_SPINLOCK(sony_dev_list_lock);
 static LIST_HEAD(sony_device_list);
@@ -815,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;
 
@@ -1130,18 +1132,38 @@ static void sony_input_configured(struct hid_device *hdev,
  */
 static int sixaxis_set_operational_usb(struct hid_device *hdev)
 {
+	const int buf_size =
+		max(SIXAXIS_REPORT_0xF2_SIZE, SIXAXIS_REPORT_0xF5_SIZE);
+	__u8 *buf;
 	int ret;
-	char *buf = kmalloc(18, GFP_KERNEL);
 
+	buf = kmalloc(buf_size, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
-	ret = hid_hw_raw_request(hdev, 0xf2, buf, 17, HID_FEATURE_REPORT,
-				 HID_REQ_GET_REPORT);
+	ret = hid_hw_raw_request(hdev, 0xf2, buf, SIXAXIS_REPORT_0xF2_SIZE,
+				 HID_FEATURE_REPORT, HID_REQ_GET_REPORT);
+	if (ret < 0) {
+		hid_err(hdev, "can't set operational mode: step 1\n");
+		goto out;
+	}
 
+	/*
+	 * Some compatible controllers like the Speedlink Strike FX and
+	 * Gasia need another query plus an USB interrupt to get operational.
+	 */
+	ret = hid_hw_raw_request(hdev, 0xf5, buf, SIXAXIS_REPORT_0xF5_SIZE,
+				 HID_FEATURE_REPORT, HID_REQ_GET_REPORT);
+	if (ret < 0) {
+		hid_err(hdev, "can't set operational mode: step 2\n");
+		goto out;
+	}
+
+	ret = hid_hw_output_report(hdev, buf, 1);
 	if (ret < 0)
-		hid_err(hdev, "can't set operational mode\n");
+		hid_err(hdev, "can't set operational mode: step 3\n");
 
+out:
 	kfree(buf);
 
 	return ret;
@@ -1660,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;
@@ -1699,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;
 
@@ -1708,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-steelseries.c b/drivers/hid/hid-steelseries.c
index 29f328f411fb..3edd4ac36494 100644
--- a/drivers/hid/hid-steelseries.c
+++ b/drivers/hid/hid-steelseries.c
@@ -12,7 +12,6 @@
  */
 
 #include <linux/device.h>
-#include <linux/usb.h>
 #include <linux/hid.h>
 #include <linux/module.h>
 
diff --git a/drivers/hid/hid-tivo.c b/drivers/hid/hid-tivo.c
index d790d8d71f7f..d98696927453 100644
--- a/drivers/hid/hid-tivo.c
+++ b/drivers/hid/hid-tivo.c
@@ -64,6 +64,7 @@ static const struct hid_device_id tivo_devices[] = {
 	/* TiVo Slide Bluetooth remote, pairs with a Broadcom dongle */
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE_BT) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE_PRO) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, tivo_devices);
diff --git a/drivers/hid/hid-uclogic.c b/drivers/hid/hid-uclogic.c
index fb8b516ff0ed..94167310e15a 100644
--- a/drivers/hid/hid-uclogic.c
+++ b/drivers/hid/hid-uclogic.c
@@ -1,7 +1,8 @@
 /*
  *  HID driver for UC-Logic devices not fully compliant with HID standard
  *
- *  Copyright (c) 2010 Nikolai Kondrashov
+ *  Copyright (c) 2010-2014 Nikolai Kondrashov
+ *  Copyright (c) 2013 Martin Rusko
  */
 
 /*
@@ -15,6 +16,8 @@
 #include <linux/hid.h>
 #include <linux/module.h>
 #include <linux/usb.h>
+#include <asm/unaligned.h>
+#include "usbhid/usbhid.h"
 
 #include "hid-ids.h"
 
@@ -546,11 +549,93 @@ static __u8 twha60_rdesc_fixed1[] = {
 	0xC0        /*  End Collection              */
 };
 
+/* Report descriptor template placeholder head */
+#define UCLOGIC_PH_HEAD	0xFE, 0xED, 0x1D
+
+/* Report descriptor template placeholder IDs */
+enum uclogic_ph_id {
+	UCLOGIC_PH_ID_X_LM,
+	UCLOGIC_PH_ID_X_PM,
+	UCLOGIC_PH_ID_Y_LM,
+	UCLOGIC_PH_ID_Y_PM,
+	UCLOGIC_PH_ID_PRESSURE_LM,
+	UCLOGIC_PH_ID_NUM
+};
+
+/* Report descriptor template placeholder */
+#define UCLOGIC_PH(_ID) UCLOGIC_PH_HEAD, UCLOGIC_PH_ID_##_ID
+#define UCLOGIC_PEN_REPORT_ID	0x07
+
+/* Fixed report descriptor template */
+static const __u8 uclogic_tablet_rdesc_template[] = {
+	0x05, 0x0D,             /*  Usage Page (Digitizer),                 */
+	0x09, 0x02,             /*  Usage (Pen),                            */
+	0xA1, 0x01,             /*  Collection (Application),               */
+	0x85, 0x07,             /*      Report ID (7),                      */
+	0x09, 0x20,             /*      Usage (Stylus),                     */
+	0xA0,                   /*      Collection (Physical),              */
+	0x14,                   /*          Logical Minimum (0),            */
+	0x25, 0x01,             /*          Logical Maximum (1),            */
+	0x75, 0x01,             /*          Report Size (1),                */
+	0x09, 0x42,             /*          Usage (Tip Switch),             */
+	0x09, 0x44,             /*          Usage (Barrel Switch),          */
+	0x09, 0x46,             /*          Usage (Tablet Pick),            */
+	0x95, 0x03,             /*          Report Count (3),               */
+	0x81, 0x02,             /*          Input (Variable),               */
+	0x95, 0x03,             /*          Report Count (3),               */
+	0x81, 0x03,             /*          Input (Constant, Variable),     */
+	0x09, 0x32,             /*          Usage (In Range),               */
+	0x95, 0x01,             /*          Report Count (1),               */
+	0x81, 0x02,             /*          Input (Variable),               */
+	0x95, 0x01,             /*          Report Count (1),               */
+	0x81, 0x03,             /*          Input (Constant, Variable),     */
+	0x75, 0x10,             /*          Report Size (16),               */
+	0x95, 0x01,             /*          Report Count (1),               */
+	0xA4,                   /*          Push,                           */
+	0x05, 0x01,             /*          Usage Page (Desktop),           */
+	0x65, 0x13,             /*          Unit (Inch),                    */
+	0x55, 0xFD,             /*          Unit Exponent (-3),             */
+	0x34,                   /*          Physical Minimum (0),           */
+	0x09, 0x30,             /*          Usage (X),                      */
+	0x27, UCLOGIC_PH(X_LM), /*          Logical Maximum (PLACEHOLDER),  */
+	0x47, UCLOGIC_PH(X_PM), /*          Physical Maximum (PLACEHOLDER), */
+	0x81, 0x02,             /*          Input (Variable),               */
+	0x09, 0x31,             /*          Usage (Y),                      */
+	0x27, UCLOGIC_PH(Y_LM), /*          Logical Maximum (PLACEHOLDER),  */
+	0x47, UCLOGIC_PH(Y_PM), /*          Physical Maximum (PLACEHOLDER), */
+	0x81, 0x02,             /*          Input (Variable),               */
+	0xB4,                   /*          Pop,                            */
+	0x09, 0x30,             /*          Usage (Tip Pressure),           */
+	0x27,
+	UCLOGIC_PH(PRESSURE_LM),/*          Logical Maximum (PLACEHOLDER),  */
+	0x81, 0x02,             /*          Input (Variable),               */
+	0xC0,                   /*      End Collection,                     */
+	0xC0                    /*  End Collection                          */
+};
+
+/* Parameter indices */
+enum uclogic_prm {
+	UCLOGIC_PRM_X_LM	= 1,
+	UCLOGIC_PRM_Y_LM	= 2,
+	UCLOGIC_PRM_PRESSURE_LM	= 4,
+	UCLOGIC_PRM_RESOLUTION	= 5,
+	UCLOGIC_PRM_NUM
+};
+
+/* Driver data */
+struct uclogic_drvdata {
+	__u8 *rdesc;
+	unsigned int rsize;
+	bool invert_pen_inrange;
+	bool ignore_pen_usage;
+};
+
 static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 					unsigned int *rsize)
 {
 	struct usb_interface *iface = to_usb_interface(hdev->dev.parent);
 	__u8 iface_num = iface->cur_altsetting->desc.bInterfaceNumber;
+	struct uclogic_drvdata *drvdata = hid_get_drvdata(hdev);
 
 	switch (hdev->product) {
 	case USB_DEVICE_ID_UCLOGIC_TABLET_PF1209:
@@ -621,11 +706,241 @@ static __u8 *uclogic_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 			break;
 		}
 		break;
+	default:
+		if (drvdata->rdesc != NULL) {
+			rdesc = drvdata->rdesc;
+			*rsize = drvdata->rsize;
+		}
 	}
 
 	return rdesc;
 }
 
+static int uclogic_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	struct uclogic_drvdata *drvdata = hid_get_drvdata(hdev);
+
+	/* discard the unused pen interface */
+	if ((drvdata->ignore_pen_usage) &&
+	    (field->application == HID_DG_PEN))
+		return -1;
+
+	/* let hid-core decide what to do */
+	return 0;
+}
+
+static void uclogic_input_configured(struct hid_device *hdev,
+		struct hid_input *hi)
+{
+	char *name;
+	const char *suffix = NULL;
+	struct hid_field *field;
+	size_t len;
+
+	/* no report associated (HID_QUIRK_MULTI_INPUT not set) */
+	if (!hi->report)
+		return;
+
+	field = hi->report->field[0];
+
+	switch (field->application) {
+	case HID_GD_KEYBOARD:
+		suffix = "Keyboard";
+		break;
+	case HID_GD_MOUSE:
+		suffix = "Mouse";
+		break;
+	case HID_GD_KEYPAD:
+		suffix = "Pad";
+		break;
+	case HID_DG_PEN:
+		suffix = "Pen";
+		break;
+	case HID_CP_CONSUMER_CONTROL:
+		suffix = "Consumer Control";
+		break;
+	case HID_GD_SYSTEM_CONTROL:
+		suffix = "System Control";
+		break;
+	}
+
+	if (suffix) {
+		len = strlen(hdev->name) + 2 + strlen(suffix);
+		name = devm_kzalloc(&hi->input->dev, len, GFP_KERNEL);
+		if (name) {
+			snprintf(name, len, "%s %s", hdev->name, suffix);
+			hi->input->name = name;
+		}
+	}
+}
+
+/**
+ * Enable fully-functional tablet mode and determine device parameters.
+ *
+ * @hdev:	HID device
+ */
+static int uclogic_tablet_enable(struct hid_device *hdev)
+{
+	int rc;
+	struct usb_device *usb_dev = hid_to_usb_dev(hdev);
+	struct uclogic_drvdata *drvdata = hid_get_drvdata(hdev);
+	__le16 *buf = NULL;
+	size_t len;
+	s32 params[UCLOGIC_PH_ID_NUM];
+	s32 resolution;
+	__u8 *p;
+	s32 v;
+
+	/*
+	 * Read string descriptor containing tablet parameters. The specific
+	 * string descriptor and data were discovered by sniffing the Windows
+	 * driver traffic.
+	 * NOTE: This enables fully-functional tablet mode.
+	 */
+	len = UCLOGIC_PRM_NUM * sizeof(*buf);
+	buf = kmalloc(len, GFP_KERNEL);
+	if (buf == NULL) {
+		hid_err(hdev, "failed to allocate parameter buffer\n");
+		rc = -ENOMEM;
+		goto cleanup;
+	}
+	rc = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
+				USB_REQ_GET_DESCRIPTOR, USB_DIR_IN,
+				(USB_DT_STRING << 8) + 0x64,
+				0x0409, buf, len,
+				USB_CTRL_GET_TIMEOUT);
+	if (rc == -EPIPE) {
+		hid_err(hdev, "device parameters not found\n");
+		rc = -ENODEV;
+		goto cleanup;
+	} else if (rc < 0) {
+		hid_err(hdev, "failed to get device parameters: %d\n", rc);
+		rc = -ENODEV;
+		goto cleanup;
+	} else if (rc != len) {
+		hid_err(hdev, "invalid device parameters\n");
+		rc = -ENODEV;
+		goto cleanup;
+	}
+
+	/* Extract device parameters */
+	params[UCLOGIC_PH_ID_X_LM] = le16_to_cpu(buf[UCLOGIC_PRM_X_LM]);
+	params[UCLOGIC_PH_ID_Y_LM] = le16_to_cpu(buf[UCLOGIC_PRM_Y_LM]);
+	params[UCLOGIC_PH_ID_PRESSURE_LM] =
+		le16_to_cpu(buf[UCLOGIC_PRM_PRESSURE_LM]);
+	resolution = le16_to_cpu(buf[UCLOGIC_PRM_RESOLUTION]);
+	if (resolution == 0) {
+		params[UCLOGIC_PH_ID_X_PM] = 0;
+		params[UCLOGIC_PH_ID_Y_PM] = 0;
+	} else {
+		params[UCLOGIC_PH_ID_X_PM] = params[UCLOGIC_PH_ID_X_LM] *
+						1000 / resolution;
+		params[UCLOGIC_PH_ID_Y_PM] = params[UCLOGIC_PH_ID_Y_LM] *
+						1000 / resolution;
+	}
+
+	/* Allocate fixed report descriptor */
+	drvdata->rdesc = devm_kzalloc(&hdev->dev,
+				sizeof(uclogic_tablet_rdesc_template),
+				GFP_KERNEL);
+	if (drvdata->rdesc == NULL) {
+		hid_err(hdev, "failed to allocate fixed rdesc\n");
+		rc = -ENOMEM;
+		goto cleanup;
+	}
+	drvdata->rsize = sizeof(uclogic_tablet_rdesc_template);
+
+	/* Format fixed report descriptor */
+	memcpy(drvdata->rdesc, uclogic_tablet_rdesc_template,
+		drvdata->rsize);
+	for (p = drvdata->rdesc;
+	     p <= drvdata->rdesc + drvdata->rsize - 4;) {
+		if (p[0] == 0xFE && p[1] == 0xED && p[2] == 0x1D &&
+		    p[3] < sizeof(params)) {
+			v = params[p[3]];
+			put_unaligned(cpu_to_le32(v), (s32 *)p);
+			p += 4;
+		} else {
+			p++;
+		}
+	}
+
+	rc = 0;
+
+cleanup:
+	kfree(buf);
+	return rc;
+}
+
+static int uclogic_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int rc;
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct uclogic_drvdata *drvdata;
+
+	/*
+	 * libinput requires the pad interface to be on a different node
+	 * than the pen, so use QUIRK_MULTI_INPUT for all tablets.
+	 */
+	hdev->quirks |= HID_QUIRK_MULTI_INPUT;
+	hdev->quirks |= HID_QUIRK_NO_EMPTY_INPUT;
+
+	/* Allocate and assign driver data */
+	drvdata = devm_kzalloc(&hdev->dev, sizeof(*drvdata), GFP_KERNEL);
+	if (drvdata == NULL)
+		return -ENOMEM;
+
+	hid_set_drvdata(hdev, drvdata);
+
+	switch (id->product) {
+	case USB_DEVICE_ID_HUION_TABLET:
+		/* If this is the pen interface */
+		if (intf->cur_altsetting->desc.bInterfaceNumber == 0) {
+			rc = uclogic_tablet_enable(hdev);
+			if (rc) {
+				hid_err(hdev, "tablet enabling failed\n");
+				return rc;
+			}
+			drvdata->invert_pen_inrange = true;
+		} else {
+			drvdata->ignore_pen_usage = true;
+		}
+		break;
+	}
+
+	rc = hid_parse(hdev);
+	if (rc) {
+		hid_err(hdev, "parse failed\n");
+		return rc;
+	}
+
+	rc = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (rc) {
+		hid_err(hdev, "hw start failed\n");
+		return rc;
+	}
+
+	return 0;
+}
+
+static int uclogic_raw_event(struct hid_device *hdev, struct hid_report *report,
+			u8 *data, int size)
+{
+	struct uclogic_drvdata *drvdata = hid_get_drvdata(hdev);
+
+	if ((drvdata->invert_pen_inrange) &&
+	    (report->type == HID_INPUT_REPORT) &&
+	    (report->id == UCLOGIC_PEN_REPORT_ID) &&
+	    (size >= 2))
+		/* Invert the in-range bit */
+		data[1] ^= 0x40;
+
+	return 0;
+}
+
 static const struct hid_device_id uclogic_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC,
 				USB_DEVICE_ID_UCLOGIC_TABLET_PF1209) },
@@ -641,6 +956,8 @@ static const struct hid_device_id uclogic_devices[] = {
 				USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC,
 				USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_HUION, USB_DEVICE_ID_HUION_TABLET) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_HUION_TABLET) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, uclogic_devices);
@@ -648,8 +965,14 @@ MODULE_DEVICE_TABLE(hid, uclogic_devices);
 static struct hid_driver uclogic_driver = {
 	.name = "uclogic",
 	.id_table = uclogic_devices,
+	.probe = uclogic_probe,
 	.report_fixup = uclogic_report_fixup,
+	.raw_event = uclogic_raw_event,
+	.input_mapping = uclogic_input_mapping,
+	.input_configured = uclogic_input_configured,
 };
 module_hid_driver(uclogic_driver);
 
+MODULE_AUTHOR("Martin Rusko");
+MODULE_AUTHOR("Nikolai Kondrashov");
 MODULE_LICENSE("GPL");
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/i2c-hid/i2c-hid.c b/drivers/hid/i2c-hid/i2c-hid.c
index 36053f33d6d9..ab4dd952b6ba 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -37,6 +37,7 @@
 #include <linux/mutex.h>
 #include <linux/acpi.h>
 #include <linux/of.h>
+#include <linux/gpio/consumer.h>
 
 #include <linux/i2c/i2c-hid.h>
 
@@ -144,6 +145,8 @@ struct i2c_hid {
 	unsigned long		flags;		/* device flags */
 
 	wait_queue_head_t	wait;		/* For waiting the interrupt */
+	struct gpio_desc	*desc;
+	int			irq;
 
 	struct i2c_hid_platform_data pdata;
 };
@@ -785,16 +788,16 @@ static int i2c_hid_init_irq(struct i2c_client *client)
 	struct i2c_hid *ihid = i2c_get_clientdata(client);
 	int ret;
 
-	dev_dbg(&client->dev, "Requesting IRQ: %d\n", client->irq);
+	dev_dbg(&client->dev, "Requesting IRQ: %d\n", ihid->irq);
 
-	ret = request_threaded_irq(client->irq, NULL, i2c_hid_irq,
+	ret = request_threaded_irq(ihid->irq, NULL, i2c_hid_irq,
 			IRQF_TRIGGER_LOW | IRQF_ONESHOT,
 			client->name, ihid);
 	if (ret < 0) {
 		dev_warn(&client->dev,
 			"Could not register for %s interrupt, irq = %d,"
 			" ret = %d\n",
-			client->name, client->irq, ret);
+			client->name, ihid->irq, ret);
 
 		return ret;
 	}
@@ -841,6 +844,14 @@ static int i2c_hid_fetch_hid_descriptor(struct i2c_hid *ihid)
 }
 
 #ifdef CONFIG_ACPI
+
+/* Default GPIO mapping */
+static const struct acpi_gpio_params i2c_hid_irq_gpio = { 0, 0, true };
+static const struct acpi_gpio_mapping i2c_hid_acpi_gpios[] = {
+	{ "gpios", &i2c_hid_irq_gpio, 1 },
+	{ },
+};
+
 static int i2c_hid_acpi_pdata(struct i2c_client *client,
 		struct i2c_hid_platform_data *pdata)
 {
@@ -866,7 +877,7 @@ static int i2c_hid_acpi_pdata(struct i2c_client *client,
 	pdata->hid_descriptor_address = obj->integer.value;
 	ACPI_FREE(obj);
 
-	return 0;
+	return acpi_dev_add_driver_gpios(adev, i2c_hid_acpi_gpios);
 }
 
 static const struct acpi_device_id i2c_hid_acpi_match[] = {
@@ -930,12 +941,6 @@ static int i2c_hid_probe(struct i2c_client *client,
 
 	dbg_hid("HID probe called for i2c 0x%02x\n", client->addr);
 
-	if (!client->irq) {
-		dev_err(&client->dev,
-			"HID over i2c has not been provided an Int IRQ\n");
-		return -EINVAL;
-	}
-
 	ihid = kzalloc(sizeof(struct i2c_hid), GFP_KERNEL);
 	if (!ihid)
 		return -ENOMEM;
@@ -955,6 +960,23 @@ static int i2c_hid_probe(struct i2c_client *client,
 		ihid->pdata = *platform_data;
 	}
 
+	if (client->irq > 0) {
+		ihid->irq = client->irq;
+	} else if (ACPI_COMPANION(&client->dev)) {
+		ihid->desc = gpiod_get(&client->dev, NULL, GPIOD_IN);
+		if (IS_ERR(ihid->desc)) {
+			dev_err(&client->dev, "Failed to get GPIO interrupt\n");
+			return PTR_ERR(ihid->desc);
+		}
+
+		ihid->irq = gpiod_to_irq(ihid->desc);
+		if (ihid->irq < 0) {
+			gpiod_put(ihid->desc);
+			dev_err(&client->dev, "Failed to convert GPIO to IRQ\n");
+			return ihid->irq;
+		}
+	}
+
 	i2c_set_clientdata(client, ihid);
 
 	ihid->client = client;
@@ -1017,13 +1039,16 @@ err_mem_free:
 	hid_destroy_device(hid);
 
 err_irq:
-	free_irq(client->irq, ihid);
+	free_irq(ihid->irq, ihid);
 
 err_pm:
 	pm_runtime_put_noidle(&client->dev);
 	pm_runtime_disable(&client->dev);
 
 err:
+	if (ihid->desc)
+		gpiod_put(ihid->desc);
+
 	i2c_hid_free_buffers(ihid);
 	kfree(ihid);
 	return ret;
@@ -1042,13 +1067,18 @@ static int i2c_hid_remove(struct i2c_client *client)
 	hid = ihid->hid;
 	hid_destroy_device(hid);
 
-	free_irq(client->irq, ihid);
+	free_irq(ihid->irq, ihid);
 
 	if (ihid->bufsize)
 		i2c_hid_free_buffers(ihid);
 
+	if (ihid->desc)
+		gpiod_put(ihid->desc);
+
 	kfree(ihid);
 
+	acpi_dev_remove_driver_gpios(ACPI_COMPANION(&client->dev));
+
 	return 0;
 }
 
@@ -1060,9 +1090,9 @@ static int i2c_hid_suspend(struct device *dev)
 	struct hid_device *hid = ihid->hid;
 	int ret = 0;
 
-	disable_irq(client->irq);
+	disable_irq(ihid->irq);
 	if (device_may_wakeup(&client->dev))
-		enable_irq_wake(client->irq);
+		enable_irq_wake(ihid->irq);
 
 	if (hid->driver && hid->driver->suspend)
 		ret = hid->driver->suspend(hid, PMSG_SUSPEND);
@@ -1080,13 +1110,13 @@ static int i2c_hid_resume(struct device *dev)
 	struct i2c_hid *ihid = i2c_get_clientdata(client);
 	struct hid_device *hid = ihid->hid;
 
-	enable_irq(client->irq);
+	enable_irq(ihid->irq);
 	ret = i2c_hid_hwreset(client);
 	if (ret)
 		return ret;
 
 	if (device_may_wakeup(&client->dev))
-		disable_irq_wake(client->irq);
+		disable_irq_wake(ihid->irq);
 
 	if (hid->driver && hid->driver->reset_resume) {
 		ret = hid->driver->reset_resume(hid);
@@ -1101,17 +1131,19 @@ static int i2c_hid_resume(struct device *dev)
 static int i2c_hid_runtime_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
+	struct i2c_hid *ihid = i2c_get_clientdata(client);
 
 	i2c_hid_set_power(client, I2C_HID_PWR_SLEEP);
-	disable_irq(client->irq);
+	disable_irq(ihid->irq);
 	return 0;
 }
 
 static int i2c_hid_runtime_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
+	struct i2c_hid *ihid = i2c_get_clientdata(client);
 
-	enable_irq(client->irq);
+	enable_irq(ihid->irq);
 	i2c_hid_set_power(client, I2C_HID_PWR_ON);
 	return 0;
 }
diff --git a/drivers/hid/usbhid/hid-pidff.c b/drivers/hid/usbhid/hid-pidff.c
index 0b531c6a76a5..08174d341f4a 100644
--- a/drivers/hid/usbhid/hid-pidff.c
+++ b/drivers/hid/usbhid/hid-pidff.c
@@ -568,6 +568,12 @@ static int pidff_upload_effect(struct input_dev *dev, struct ff_effect *effect,
 	int type_id;
 	int error;
 
+	pidff->block_load[PID_EFFECT_BLOCK_INDEX].value[0] = 0;
+	if (old) {
+		pidff->block_load[PID_EFFECT_BLOCK_INDEX].value[0] =
+			pidff->pid_id[effect->id];
+	}
+
 	switch (effect->type) {
 	case FF_CONSTANT:
 		if (!old) {
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 9be99a67bfe2..a775143e6265 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -78,6 +78,13 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_TS2700, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_FORMOSA, USB_DEVICE_ID_FORMOSA_IR_RECEIVER, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_FREESCALE, USB_DEVICE_ID_FREESCALE_MX28, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_HP, USB_PRODUCT_ID_HP_LOGITECH_OEM_USB_OPTICAL_MOUSE_0A4A, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_HP, USB_PRODUCT_ID_HP_LOGITECH_OEM_USB_OPTICAL_MOUSE_0B4A, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_HP, USB_PRODUCT_ID_HP_PIXART_OEM_USB_OPTICAL_MOUSE, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_C077, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_MOUSE_C01A, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_MOUSE_C05A, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_MOUSE_C06A, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_MGE, USB_DEVICE_ID_MGE_UPS, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3_JP, HID_QUIRK_NO_INIT_REPORTS },
@@ -91,6 +98,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_MOUSE_4D22, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_PRODIGE, USB_DEVICE_ID_PRODIGE_CORDLESS, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3001, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008, HID_QUIRK_NOGET },
@@ -106,12 +114,8 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_SYMBOL, USB_DEVICE_ID_SYMBOL_SCANNER_2, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_TPV, USB_DEVICE_ID_TPV_OPTICAL_TOUCHSCREEN, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_TURBOX, USB_DEVICE_ID_TURBOX_KEYBOARD, HID_QUIRK_NOGET },
-	{ USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_PF1209, HID_QUIRK_MULTI_INPUT },
-	{ USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP4030U, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_KNA5, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_TWA60, HID_QUIRK_MULTI_INPUT },
-	{ USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP5540U, HID_QUIRK_MULTI_INPUT },
-	{ USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP8060U, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_10_6_INCH, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_14_1_INCH, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET, HID_QUIRK_MULTI_INPUT },
@@ -127,6 +131,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X_2, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X, HID_QUIRK_MULTI_INPUT },
+	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_PENSKETCH_M912, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_DUOSENSE, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SEMICO, USB_DEVICE_ID_SEMICO_USB_KEYKOARD, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_LTS1, HID_QUIRK_NO_INIT_REPORTS },
diff --git a/drivers/hid/wacom.h b/drivers/hid/wacom.h
index 7db432809e9e..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)
@@ -129,13 +131,6 @@ static inline void wacom_schedule_work(struct wacom_wac *wacom_wac)
 	schedule_work(&wacom->work);
 }
 
-static inline void wacom_notify_battery(struct wacom_wac *wacom_wac)
-{
-	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
-
-	power_supply_changed(&wacom->battery);
-}
-
 extern const struct hid_device_id wacom_ids[];
 
 void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len);
@@ -149,4 +144,5 @@ void wacom_wac_usage_mapping(struct hid_device *hdev,
 int wacom_wac_event(struct hid_device *hdev, struct hid_field *field,
 		struct hid_usage *usage, __s32 value);
 void wacom_wac_report(struct hid_device *hdev, struct hid_report *report);
+void wacom_battery_work(struct work_struct *work);
 #endif
diff --git a/drivers/hid/wacom_sys.c b/drivers/hid/wacom_sys.c
index f0568a7e6de9..e8607d096138 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -406,6 +406,9 @@ static int wacom_query_tablet_data(struct hid_device *hdev,
 		else if (features->type == WACOM_27QHDT) {
 			return wacom_set_device_mode(hdev, 131, 3, 2);
 		}
+		else if (features->type == BAMBOO_PAD) {
+			return wacom_set_device_mode(hdev, 2, 2, 2);
+		}
 	} else if (features->device_type == BTN_TOOL_PEN) {
 		if (features->type <= BAMBOO_PT && features->type != WIRELESS) {
 			return wacom_set_device_mode(hdev, 2, 2, 2);
@@ -524,6 +527,11 @@ static int wacom_add_shared_data(struct hid_device *hdev)
 
 	wacom_wac->shared = &data->shared;
 
+	if (wacom_wac->features.device_type == BTN_TOOL_FINGER)
+		wacom_wac->shared->touch = hdev;
+	else if (wacom_wac->features.device_type == BTN_TOOL_PEN)
+		wacom_wac->shared->pen = hdev;
+
 out:
 	mutex_unlock(&wacom_udev_list_lock);
 	return retval;
@@ -541,14 +549,22 @@ static void wacom_release_shared_data(struct kref *kref)
 	kfree(data);
 }
 
-static void wacom_remove_shared_data(struct wacom_wac *wacom)
+static void wacom_remove_shared_data(struct wacom *wacom)
 {
 	struct wacom_hdev_data *data;
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+
+	if (wacom_wac->shared) {
+		data = container_of(wacom_wac->shared, struct wacom_hdev_data,
+				    shared);
+
+		if (wacom_wac->shared->touch == wacom->hdev)
+			wacom_wac->shared->touch = NULL;
+		else if (wacom_wac->shared->pen == wacom->hdev)
+			wacom_wac->shared->pen = NULL;
 
-	if (wacom->shared) {
-		data = container_of(wacom->shared, struct wacom_hdev_data, shared);
 		kref_put(&data->kref, wacom_release_shared_data);
-		wacom->shared = NULL;
+		wacom_wac->shared = NULL;
 	}
 }
 
@@ -929,6 +945,7 @@ static void wacom_destroy_leds(struct wacom *wacom)
 }
 
 static enum power_supply_property wacom_battery_props[] = {
+	POWER_SUPPLY_PROP_PRESENT,
 	POWER_SUPPLY_PROP_STATUS,
 	POWER_SUPPLY_PROP_SCOPE,
 	POWER_SUPPLY_PROP_CAPACITY
@@ -944,10 +961,13 @@ 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) {
+		case POWER_SUPPLY_PROP_PRESENT:
+			val->intval = wacom->wacom_wac.bat_connected;
+			break;
 		case POWER_SUPPLY_PROP_SCOPE:
 			val->intval = POWER_SUPPLY_SCOPE_DEVICE;
 			break;
@@ -961,6 +981,8 @@ static int wacom_battery_get_property(struct power_supply *psy,
 			else if (wacom->wacom_wac.battery_capacity == 100 &&
 				    wacom->wacom_wac.ps_connected)
 				val->intval = POWER_SUPPLY_STATUS_FULL;
+			else if (wacom->wacom_wac.ps_connected)
+				val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
 			else
 				val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
 			break;
@@ -976,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) {
@@ -998,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;
@@ -1041,12 +1067,11 @@ static int wacom_initialize_battery(struct wacom *wacom)
 
 static void wacom_destroy_battery(struct wacom *wacom)
 {
-	if ((wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) &&
-	     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;
 	}
 }
 
@@ -1313,6 +1338,20 @@ fail:
 	return;
 }
 
+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) {
+		wacom_initialize_battery(wacom);
+	}
+	else if (!(wacom->wacom_wac.features.quirks & WACOM_QUIRK_BATTERY) &&
+		 wacom->battery) {
+		wacom_destroy_battery(wacom);
+	}
+}
+
 /*
  * Not all devices report physical dimensions from HID.
  * Compute the default from hardcoded logical dimension
@@ -1373,6 +1412,9 @@ static int wacom_probe(struct hid_device *hdev,
 
 	hdev->quirks |= HID_QUIRK_NO_INIT_REPORTS;
 
+	/* hid-core sets this quirk for the boot interface */
+	hdev->quirks &= ~HID_QUIRK_NOGET;
+
 	wacom = kzalloc(sizeof(struct wacom), GFP_KERNEL);
 	if (!wacom)
 		return -ENOMEM;
@@ -1412,6 +1454,21 @@ static int wacom_probe(struct hid_device *hdev,
 			goto fail_allocate_inputs;
 	}
 
+	/*
+	 * Bamboo Pad has a generic hid handling for the Pen, and we switch it
+	 * into debug mode for the touch part.
+	 * We ignore the other interfaces.
+	 */
+	if (features->type == BAMBOO_PAD) {
+		if (features->pktlen == WACOM_PKGLEN_PENABLED) {
+			features->type = HID_GENERIC;
+		} else if ((features->pktlen != WACOM_PKGLEN_BPAD_TOUCH) &&
+			   (features->pktlen != WACOM_PKGLEN_BPAD_TOUCH_USB)) {
+			error = -ENODEV;
+			goto fail_shared_data;
+		}
+	}
+
 	/* set the default size in case we do not get them from hid */
 	wacom_set_default_phy(features);
 
@@ -1446,6 +1503,12 @@ static int wacom_probe(struct hid_device *hdev,
 		features->y_max = 4096;
 	}
 
+	/*
+	 * Same thing for Bamboo PAD
+	 */
+	if (features->type == BAMBOO_PAD)
+		features->device_type = BTN_TOOL_FINGER;
+
 	if (hdev->bus == BUS_BLUETOOTH)
 		features->quirks |= WACOM_QUIRK_BATTERY;
 
@@ -1462,19 +1525,17 @@ static int wacom_probe(struct hid_device *hdev,
 	snprintf(wacom_wac->pad_name, sizeof(wacom_wac->pad_name),
 		"%s Pad", features->name);
 
-	if (features->quirks & WACOM_QUIRK_MULTI_INPUT) {
-		/* Append the device type to the name */
-		if (features->device_type != BTN_TOOL_FINGER)
-			strlcat(wacom_wac->name, " Pen", WACOM_NAME_MAX);
-		else if (features->touch_max)
-			strlcat(wacom_wac->name, " Finger", WACOM_NAME_MAX);
-		else
-			strlcat(wacom_wac->name, " Pad", WACOM_NAME_MAX);
+	/* Append the device type to the name */
+	if (features->device_type != BTN_TOOL_FINGER)
+		strlcat(wacom_wac->name, " Pen", WACOM_NAME_MAX);
+	else if (features->touch_max)
+		strlcat(wacom_wac->name, " Finger", WACOM_NAME_MAX);
+	else
+		strlcat(wacom_wac->name, " Pad", WACOM_NAME_MAX);
 
-		error = wacom_add_shared_data(hdev);
-		if (error)
-			goto fail_shared_data;
-	}
+	error = wacom_add_shared_data(hdev);
+	if (error)
+		goto fail_shared_data;
 
 	if (!(features->quirks & WACOM_QUIRK_MONITOR) &&
 	     (features->quirks & WACOM_QUIRK_BATTERY)) {
@@ -1527,7 +1588,7 @@ fail_register_inputs:
 	wacom_clean_inputs(wacom);
 	wacom_destroy_battery(wacom);
 fail_battery:
-	wacom_remove_shared_data(wacom_wac);
+	wacom_remove_shared_data(wacom);
 fail_shared_data:
 	wacom_clean_inputs(wacom);
 fail_allocate_inputs:
@@ -1550,7 +1611,7 @@ static void wacom_remove(struct hid_device *hdev)
 	if (hdev->bus == BUS_BLUETOOTH)
 		device_remove_file(&hdev->dev, &dev_attr_speed);
 	wacom_destroy_battery(wacom);
-	wacom_remove_shared_data(&wacom->wacom_wac);
+	wacom_remove_shared_data(wacom);
 
 	hid_set_drvdata(hdev, NULL);
 	kfree(wacom);
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index 046351cf17f3..fa54d3290659 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -45,6 +45,27 @@ static unsigned short batcap_gr[8] = { 1, 15, 25, 35, 50, 70, 100, 100 };
  */
 static unsigned short batcap_i4[8] = { 1, 15, 30, 45, 60, 70, 85, 100 };
 
+static void wacom_notify_battery(struct wacom_wac *wacom_wac,
+	int bat_capacity, bool bat_charging, bool bat_connected,
+	bool ps_connected)
+{
+	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
+	bool changed = wacom_wac->battery_capacity != bat_capacity  ||
+		       wacom_wac->bat_charging     != bat_charging  ||
+		       wacom_wac->bat_connected    != bat_connected ||
+		       wacom_wac->ps_connected     != ps_connected;
+
+	if (changed) {
+		wacom_wac->battery_capacity = bat_capacity;
+		wacom_wac->bat_charging = bat_charging;
+		wacom_wac->bat_connected = bat_connected;
+		wacom_wac->ps_connected = ps_connected;
+
+		if (wacom->battery)
+			power_supply_changed(wacom->battery);
+	}
+}
+
 static int wacom_penpartner_irq(struct wacom_wac *wacom)
 {
 	unsigned char *data = wacom->data;
@@ -419,17 +440,26 @@ static int wacom_graphire_irq(struct wacom_wac *wacom)
 		rw = (data[7] >> 2 & 0x07);
 		battery_capacity = batcap_gr[rw];
 		ps_connected = rw == 7;
-		if ((wacom->battery_capacity != battery_capacity) ||
-		    (wacom->ps_connected != ps_connected)) {
-			wacom->battery_capacity = battery_capacity;
-			wacom->ps_connected = ps_connected;
-			wacom_notify_battery(wacom);
-		}
+		wacom_notify_battery(wacom, battery_capacity, ps_connected,
+				     1, ps_connected);
 	}
 exit:
 	return retval;
 }
 
+static void wacom_intuos_schedule_prox_event(struct wacom_wac *wacom_wac)
+{
+	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
+	struct hid_report *r;
+	struct hid_report_enum *re;
+
+	re = &(wacom->hdev->report_enum[HID_FEATURE_REPORT]);
+	r = re->report_id_hash[WACOM_REPORT_INTUOSREAD];
+	if (r) {
+		hid_hw_request(wacom->hdev, r, HID_REQ_GET_REPORT);
+	}
+}
+
 static int wacom_intuos_inout(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
@@ -551,8 +581,9 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 	   (features->type == CINTIQ && !(data[1] & 0x40)))
 		return 1;
 
-	if (features->quirks & WACOM_QUIRK_MULTI_INPUT)
-		wacom->shared->stylus_in_proximity = true;
+	wacom->shared->stylus_in_proximity = true;
+	if (wacom->shared->touch_down)
+		return 1;
 
 	/* in Range while exiting */
 	if (((data[1] & 0xfe) == 0x20) && wacom->reporting_data) {
@@ -564,8 +595,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
 	/* Exit report */
 	if ((data[1] & 0xfe) == 0x80) {
-		if (features->quirks & WACOM_QUIRK_MULTI_INPUT)
-			wacom->shared->stylus_in_proximity = false;
+		wacom->shared->stylus_in_proximity = false;
 		wacom->reporting_data = false;
 
 		/* don't report exit if we don't know the ID */
@@ -606,8 +636,11 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 	}
 
 	/* don't report other events if we don't know the ID */
-	if (!wacom->id[idx])
+	if (!wacom->id[idx]) {
+		/* but reschedule a read of the current tool */
+		wacom_intuos_schedule_prox_event(wacom);
 		return 1;
+	}
 
 	return 0;
 }
@@ -1019,15 +1052,9 @@ static int wacom_intuos_bt_irq(struct wacom_wac *wacom, size_t len)
 		bat_charging = (power_raw & 0x08) ? 1 : 0;
 		ps_connected = (power_raw & 0x10) ? 1 : 0;
 		battery_capacity = batcap_i4[power_raw & 0x07];
-		if ((wacom->battery_capacity != battery_capacity) ||
-		    (wacom->bat_charging != bat_charging) ||
-		    (wacom->ps_connected != ps_connected)) {
-			wacom->battery_capacity = battery_capacity;
-			wacom->bat_charging = bat_charging;
-			wacom->ps_connected = ps_connected;
-			wacom_notify_battery(wacom);
-		}
-
+		wacom_notify_battery(wacom, battery_capacity, bat_charging,
+				     battery_capacity || bat_charging,
+				     ps_connected);
 		break;
 	default:
 		dev_dbg(wacom->input->dev.parent,
@@ -1038,12 +1065,34 @@ static int wacom_intuos_bt_irq(struct wacom_wac *wacom, size_t len)
 	return 0;
 }
 
+static int wacom_wac_finger_count_touches(struct wacom_wac *wacom)
+{
+	struct input_dev *input = wacom->input;
+	unsigned touch_max = wacom->features.touch_max;
+	int count = 0;
+	int i;
+
+	/* non-HID_GENERIC single touch input doesn't call this routine */
+	if ((touch_max == 1) && (wacom->features.type == HID_GENERIC))
+		return wacom->hid_data.tipswitch &&
+		       !wacom->shared->stylus_in_proximity;
+
+	for (i = 0; i < input->mt->num_slots; i++) {
+		struct input_mt_slot *ps = &input->mt->slots[i];
+		int id = input_mt_get_value(ps, ABS_MT_TRACKING_ID);
+		if (id >= 0)
+			count++;
+	}
+
+	return count;
+}
+
 static int wacom_24hdt_irq(struct wacom_wac *wacom)
 {
 	struct input_dev *input = wacom->input;
 	unsigned char *data = wacom->data;
 	int i;
-	int current_num_contacts = 0;
+	int current_num_contacts = data[61];
 	int contacts_to_send = 0;
 	int num_contacts_left = 4; /* maximum contacts per packet */
 	int byte_per_packet = WACOM_BYTES_PER_24HDT_PACKET;
@@ -1054,8 +1103,6 @@ static int wacom_24hdt_irq(struct wacom_wac *wacom)
 		num_contacts_left = 10;
 		byte_per_packet = WACOM_BYTES_PER_QHDTHID_PACKET;
 		y_offset = 0;
-	} else {
-		current_num_contacts = data[61];
 	}
 
 	/*
@@ -1098,13 +1145,13 @@ static int wacom_24hdt_irq(struct wacom_wac *wacom)
 			}
 		}
 	}
-	input_mt_report_pointer_emulation(input, true);
+	input_mt_sync_frame(input);
 
 	wacom->num_contacts_left -= contacts_to_send;
-	if (wacom->num_contacts_left <= 0)
+	if (wacom->num_contacts_left <= 0) {
 		wacom->num_contacts_left = 0;
-
-	wacom->shared->touch_down = (wacom->num_contacts_left > 0);
+		wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
+	}
 	return 1;
 }
 
@@ -1149,13 +1196,13 @@ static int wacom_mt_touch(struct wacom_wac *wacom)
 			input_report_abs(input, ABS_MT_POSITION_Y, y);
 		}
 	}
-	input_mt_report_pointer_emulation(input, true);
+	input_mt_sync_frame(input);
 
 	wacom->num_contacts_left -= contacts_to_send;
-	if (wacom->num_contacts_left < 0)
+	if (wacom->num_contacts_left <= 0) {
 		wacom->num_contacts_left = 0;
-
-	wacom->shared->touch_down = (wacom->num_contacts_left > 0);
+		wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
+	}
 	return 1;
 }
 
@@ -1163,7 +1210,6 @@ static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 {
 	struct input_dev *input = wacom->input;
 	unsigned char *data = wacom->data;
-	int contact_with_no_pen_down_count = 0;
 	int i;
 
 	for (i = 0; i < 2; i++) {
@@ -1178,13 +1224,12 @@ static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 
 			input_report_abs(input, ABS_MT_POSITION_X, x);
 			input_report_abs(input, ABS_MT_POSITION_Y, y);
-			contact_with_no_pen_down_count++;
 		}
 	}
-	input_mt_report_pointer_emulation(input, true);
+	input_mt_sync_frame(input);
 
 	/* keep touch state for pen event */
-	wacom->shared->touch_down = (contact_with_no_pen_down_count > 0);
+	wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
 
 	return 1;
 }
@@ -1193,29 +1238,25 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 {
 	unsigned char *data = wacom->data;
 	struct input_dev *input = wacom->input;
-	bool prox;
+	bool prox = !wacom->shared->stylus_in_proximity;
 	int x = 0, y = 0;
 
 	if (wacom->features.touch_max > 1 || len > WACOM_PKGLEN_TPC2FG)
 		return 0;
 
-	if (!wacom->shared->stylus_in_proximity) {
-		if (len == WACOM_PKGLEN_TPC1FG) {
-			prox = data[0] & 0x01;
-			x = get_unaligned_le16(&data[1]);
-			y = get_unaligned_le16(&data[3]);
-		} else if (len == WACOM_PKGLEN_TPC1FG_B) {
-			prox = data[2] & 0x01;
-			x = get_unaligned_le16(&data[3]);
-			y = get_unaligned_le16(&data[5]);
-		} else {
-			prox = data[1] & 0x01;
-			x = le16_to_cpup((__le16 *)&data[2]);
-			y = le16_to_cpup((__le16 *)&data[4]);
-		}
-	} else
-		/* force touch out when pen is in prox */
-		prox = 0;
+	if (len == WACOM_PKGLEN_TPC1FG) {
+		prox = prox && (data[0] & 0x01);
+		x = get_unaligned_le16(&data[1]);
+		y = get_unaligned_le16(&data[3]);
+	} else if (len == WACOM_PKGLEN_TPC1FG_B) {
+		prox = prox && (data[2] & 0x01);
+		x = get_unaligned_le16(&data[3]);
+		y = get_unaligned_le16(&data[5]);
+	} else {
+		prox = prox && (data[1] & 0x01);
+		x = le16_to_cpup((__le16 *)&data[2]);
+		y = le16_to_cpup((__le16 *)&data[4]);
+	}
 
 	if (prox) {
 		input_report_abs(input, ABS_X, x);
@@ -1516,29 +1557,6 @@ static int wacom_wac_finger_event(struct hid_device *hdev,
 	return 0;
 }
 
-static int wacom_wac_finger_count_touches(struct hid_device *hdev)
-{
-	struct wacom *wacom = hid_get_drvdata(hdev);
-	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-	struct input_dev *input = wacom_wac->input;
-	unsigned touch_max = wacom_wac->features.touch_max;
-	int count = 0;
-	int i;
-
-	if (touch_max == 1)
-		return wacom_wac->hid_data.tipswitch &&
-		       !wacom_wac->shared->stylus_in_proximity;
-
-	for (i = 0; i < input->mt->num_slots; i++) {
-		struct input_mt_slot *ps = &input->mt->slots[i];
-		int id = input_mt_get_value(ps, ABS_MT_TRACKING_ID);
-		if (id >= 0)
-			count++;
-	}
-
-	return count;
-}
-
 static void wacom_wac_finger_report(struct hid_device *hdev,
 		struct hid_report *report)
 {
@@ -1553,7 +1571,7 @@ static void wacom_wac_finger_report(struct hid_device *hdev,
 	input_sync(input);
 
 	/* keep touch state for pen event */
-	wacom_wac->shared->touch_down = wacom_wac_finger_count_touches(hdev);
+	wacom_wac->shared->touch_down = wacom_wac_finger_count_touches(wacom_wac);
 }
 
 void wacom_wac_usage_mapping(struct hid_device *hdev,
@@ -1643,12 +1661,13 @@ static int wacom_bpt_touch(struct wacom_wac *wacom)
 		}
 	}
 
-	input_mt_report_pointer_emulation(input, true);
+	input_mt_sync_frame(input);
 
 	input_report_key(pad_input, BTN_LEFT, (data[1] & 0x08) != 0);
 	input_report_key(pad_input, BTN_FORWARD, (data[1] & 0x04) != 0);
 	input_report_key(pad_input, BTN_BACK, (data[1] & 0x02) != 0);
 	input_report_key(pad_input, BTN_RIGHT, (data[1] & 0x01) != 0);
+	wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
 
 	return 1;
 }
@@ -1733,7 +1752,8 @@ static int wacom_bpt3_touch(struct wacom_wac *wacom)
 			wacom_bpt3_button_msg(wacom, data + offset);
 
 	}
-	input_mt_report_pointer_emulation(input, true);
+	input_mt_sync_frame(input);
+	wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
 
 	return 1;
 }
@@ -1745,20 +1765,9 @@ static int wacom_bpt_pen(struct wacom_wac *wacom)
 	unsigned char *data = wacom->data;
 	int prox = 0, x = 0, y = 0, p = 0, d = 0, pen = 0, btn1 = 0, btn2 = 0;
 
-	if (data[0] != WACOM_REPORT_PENABLED && data[0] != WACOM_REPORT_USB)
+	if (data[0] != WACOM_REPORT_PENABLED)
 	    return 0;
 
-	if (data[0] == WACOM_REPORT_USB) {
-		if (features->type == INTUOSHT &&
-		    wacom->shared->touch_input &&
-		    features->touch_max) {
-			input_report_switch(wacom->shared->touch_input,
-					    SW_MUTE_DEVICE, data[8] & 0x40);
-			input_sync(wacom->shared->touch_input);
-		}
-		return 0;
-	}
-
 	prox = (data[1] & 0x20) == 0x20;
 
 	/*
@@ -1771,17 +1780,21 @@ static int wacom_bpt_pen(struct wacom_wac *wacom)
 	 *
 	 * Hardware does report zero in most out-of-prox cases but not all.
 	 */
-	if (prox) {
-		if (!wacom->shared->stylus_in_proximity) {
-			if (data[1] & 0x08) {
-				wacom->tool[0] = BTN_TOOL_RUBBER;
-				wacom->id[0] = ERASER_DEVICE_ID;
-			} else {
-				wacom->tool[0] = BTN_TOOL_PEN;
-				wacom->id[0] = STYLUS_DEVICE_ID;
-			}
-			wacom->shared->stylus_in_proximity = true;
+	if (!wacom->shared->stylus_in_proximity) {
+		if (data[1] & 0x08) {
+			wacom->tool[0] = BTN_TOOL_RUBBER;
+			wacom->id[0] = ERASER_DEVICE_ID;
+		} else {
+			wacom->tool[0] = BTN_TOOL_PEN;
+			wacom->id[0] = STYLUS_DEVICE_ID;
 		}
+	}
+
+	wacom->shared->stylus_in_proximity = prox;
+	if (wacom->shared->touch_down)
+		return 0;
+
+	if (prox) {
 		x = le16_to_cpup((__le16 *)&data[2]);
 		y = le16_to_cpup((__le16 *)&data[4]);
 		p = le16_to_cpup((__le16 *)&data[6]);
@@ -1797,6 +1810,8 @@ static int wacom_bpt_pen(struct wacom_wac *wacom)
 		pen = data[1] & 0x01;
 		btn1 = data[1] & 0x02;
 		btn2 = data[1] & 0x04;
+	} else {
+		wacom->id[0] = 0;
 	}
 
 	input_report_key(input, BTN_TOUCH, pen);
@@ -1808,11 +1823,6 @@ static int wacom_bpt_pen(struct wacom_wac *wacom)
 	input_report_abs(input, ABS_PRESSURE, p);
 	input_report_abs(input, ABS_DISTANCE, d);
 
-	if (!prox) {
-		wacom->id[0] = 0;
-		wacom->shared->stylus_in_proximity = false;
-	}
-
 	input_report_key(input, wacom->tool[0], prox); /* PEN or RUBBER */
 	input_report_abs(input, ABS_MISC, wacom->id[0]); /* TOOL ID */
 
@@ -1831,6 +1841,91 @@ static int wacom_bpt_irq(struct wacom_wac *wacom, size_t len)
 	return 0;
 }
 
+static void wacom_bamboo_pad_pen_event(struct wacom_wac *wacom,
+		unsigned char *data)
+{
+	unsigned char prefix;
+
+	/*
+	 * We need to reroute the event from the debug interface to the
+	 * pen interface.
+	 * We need to add the report ID to the actual pen report, so we
+	 * temporary overwrite the first byte to prevent having to kzalloc/kfree
+	 * and memcpy the report.
+	 */
+	prefix = data[0];
+	data[0] = WACOM_REPORT_BPAD_PEN;
+
+	/*
+	 * actually reroute the event.
+	 * No need to check if wacom->shared->pen is valid, hid_input_report()
+	 * will check for us.
+	 */
+	hid_input_report(wacom->shared->pen, HID_INPUT_REPORT, data,
+			 WACOM_PKGLEN_PENABLED, 1);
+
+	data[0] = prefix;
+}
+
+static int wacom_bamboo_pad_touch_event(struct wacom_wac *wacom,
+		unsigned char *data)
+{
+	struct input_dev *input = wacom->input;
+	unsigned char *finger_data, prefix;
+	unsigned id;
+	int x, y;
+	bool valid;
+
+	prefix = data[0];
+
+	for (id = 0; id < wacom->features.touch_max; id++) {
+		valid = !!(prefix & BIT(id)) &&
+			!wacom->shared->stylus_in_proximity;
+
+		input_mt_slot(input, id);
+		input_mt_report_slot_state(input, MT_TOOL_FINGER, valid);
+
+		if (!valid)
+			continue;
+
+		finger_data = data + 1 + id * 3;
+		x = finger_data[0] | ((finger_data[1] & 0x0f) << 8);
+		y = (finger_data[2] << 4) | (finger_data[1] >> 4);
+
+		input_report_abs(input, ABS_MT_POSITION_X, x);
+		input_report_abs(input, ABS_MT_POSITION_Y, y);
+	}
+
+	input_mt_sync_frame(input);
+
+	input_report_key(input, BTN_LEFT, prefix & 0x40);
+	input_report_key(input, BTN_RIGHT, prefix & 0x80);
+
+	/* keep touch state for pen event */
+	wacom->shared->touch_down = !!prefix &&
+				    !wacom->shared->stylus_in_proximity;
+
+	return 1;
+}
+
+static int wacom_bamboo_pad_irq(struct wacom_wac *wacom, size_t len)
+{
+	unsigned char *data = wacom->data;
+
+	if (!((len == WACOM_PKGLEN_BPAD_TOUCH) ||
+	      (len == WACOM_PKGLEN_BPAD_TOUCH_USB)) ||
+	    (data[0] != WACOM_REPORT_BPAD_TOUCH))
+		return 0;
+
+	if (data[1] & 0x01)
+		wacom_bamboo_pad_pen_event(wacom, &data[1]);
+
+	if (data[1] & 0x02)
+		return wacom_bamboo_pad_touch_event(wacom, &data[9]);
+
+	return 0;
+}
+
 static int wacom_wireless_irq(struct wacom_wac *wacom, size_t len)
 {
 	unsigned char *data = wacom->data;
@@ -1841,7 +1936,7 @@ static int wacom_wireless_irq(struct wacom_wac *wacom, size_t len)
 
 	connected = data[1] & 0x01;
 	if (connected) {
-		int pid, battery, ps_connected;
+		int pid, battery, charging;
 
 		if ((wacom->shared->type == INTUOSHT) &&
 		    wacom->shared->touch_input &&
@@ -1853,33 +1948,66 @@ static int wacom_wireless_irq(struct wacom_wac *wacom, size_t len)
 
 		pid = get_unaligned_be16(&data[6]);
 		battery = (data[5] & 0x3f) * 100 / 31;
-		ps_connected = !!(data[5] & 0x80);
+		charging = !!(data[5] & 0x80);
 		if (wacom->pid != pid) {
 			wacom->pid = pid;
 			wacom_schedule_work(wacom);
 		}
 
-		if (wacom->shared->type &&
-		    (battery != wacom->battery_capacity ||
-		     ps_connected != wacom->ps_connected)) {
-			wacom->battery_capacity = battery;
-			wacom->ps_connected = ps_connected;
-			wacom->bat_charging = ps_connected &&
-						wacom->battery_capacity < 100;
-			wacom_notify_battery(wacom);
-		}
+		if (wacom->shared->type)
+			wacom_notify_battery(wacom, battery, charging, 1, 0);
+
 	} else if (wacom->pid != 0) {
 		/* disconnected while previously connected */
 		wacom->pid = 0;
 		wacom_schedule_work(wacom);
-		wacom->battery_capacity = 0;
-		wacom->bat_charging = 0;
-		wacom->ps_connected = 0;
+		wacom_notify_battery(wacom, 0, 0, 0, 0);
 	}
 
 	return 0;
 }
 
+static int wacom_status_irq(struct wacom_wac *wacom_wac, size_t len)
+{
+	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
+	struct wacom_features *features = &wacom_wac->features;
+	unsigned char *data = wacom_wac->data;
+
+	if (data[0] != WACOM_REPORT_USB)
+		return 0;
+
+	if (features->type == INTUOSHT &&
+	    wacom_wac->shared->touch_input &&
+	    features->touch_max) {
+		input_report_switch(wacom_wac->shared->touch_input,
+				    SW_MUTE_DEVICE, data[8] & 0x40);
+		input_sync(wacom_wac->shared->touch_input);
+	}
+
+	if (data[9] & 0x02) { /* wireless module is attached */
+		int battery = (data[8] & 0x3f) * 100 / 31;
+		bool charging = !!(data[8] & 0x80);
+
+		wacom_notify_battery(wacom_wac, battery, charging,
+				     battery || charging, 1);
+
+		if (!wacom->battery &&
+		    !(features->quirks & WACOM_QUIRK_BATTERY)) {
+			features->quirks |= WACOM_QUIRK_BATTERY;
+			INIT_WORK(&wacom->work, wacom_battery_work);
+			wacom_schedule_work(wacom_wac);
+		}
+	}
+	else if ((features->quirks & WACOM_QUIRK_BATTERY) &&
+		 wacom->battery) {
+		features->quirks &= ~WACOM_QUIRK_BATTERY;
+		INIT_WORK(&wacom->work, wacom_battery_work);
+		wacom_schedule_work(wacom_wac);
+		wacom_notify_battery(wacom_wac, 0, 0, 0, 0);
+	}
+	return 0;
+}
+
 void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 {
 	bool sync;
@@ -1949,6 +2077,8 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 	case INTUOSPL:
 		if (len == WACOM_PKGLEN_BBTOUCH3)
 			sync = wacom_bpt3_touch(wacom_wac);
+		else if (wacom_wac->data[0] == WACOM_REPORT_USB)
+			sync = wacom_status_irq(wacom_wac, len);
 		else
 			sync = wacom_intuos_irq(wacom_wac);
 		break;
@@ -1964,7 +2094,14 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 
 	case BAMBOO_PT:
 	case INTUOSHT:
-		sync = wacom_bpt_irq(wacom_wac, len);
+		if (wacom_wac->data[0] == WACOM_REPORT_USB)
+			sync = wacom_status_irq(wacom_wac, len);
+		else
+			sync = wacom_bpt_irq(wacom_wac, len);
+		break;
+
+	case BAMBOO_PAD:
+		sync = wacom_bamboo_pad_irq(wacom_wac, len);
 		break;
 
 	case WIRELESS:
@@ -2036,12 +2173,6 @@ void wacom_setup_device_quirks(struct wacom_features *features)
 		features->y_max = 1023;
 	}
 
-	/* these device have multiple inputs */
-	if (features->type >= WIRELESS ||
-	    (features->type >= INTUOS5S && features->type <= INTUOSHT) ||
-	    (features->oVid && features->oPid))
-		features->quirks |= WACOM_QUIRK_MULTI_INPUT;
-
 	/* quirk for bamboo touch with 2 low res touches */
 	if (features->type == BAMBOO_PT &&
 	    features->pktlen == WACOM_PKGLEN_BBTOUCH) {
@@ -2305,6 +2436,13 @@ int wacom_setup_pentouch_input_capabilities(struct input_dev *input_dev,
 					      0, 0);
 		}
 		break;
+	case BAMBOO_PAD:
+		__clear_bit(ABS_MISC, input_dev->absbit);
+		input_mt_init_slots(input_dev, features->touch_max,
+				    INPUT_MT_POINTER);
+		__set_bit(BTN_LEFT, input_dev->keybit);
+		__set_bit(BTN_RIGHT, input_dev->keybit);
+		break;
 	}
 	return 0;
 }
@@ -2754,6 +2892,15 @@ static const struct wacom_features wacom_features_0x304 =
 	{ "Wacom Cintiq 13HD", 59152, 33448, 1023, 63,
 	  WACOM_13HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
 	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET };
+static const struct wacom_features wacom_features_0x333 =
+	{ "Wacom Cintiq 13HD touch", 59152, 33448, 2047, 63,
+	  WACOM_13HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
+	  WACOM_CINTIQ_OFFSET, WACOM_CINTIQ_OFFSET,
+	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x335 };
+static const struct wacom_features wacom_features_0x335 =
+	{ "Wacom Cintiq 13HD touch", .type = WACOM_24HDT, /* Touch */
+	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x333, .touch_max = 10,
+	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
 static const struct wacom_features wacom_features_0xC7 =
 	{ "Wacom DTU1931", 37832, 30305, 511, 0,
 	  PL, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -2958,6 +3105,12 @@ static const struct wacom_features wacom_features_0x30C =
 	{ "Wacom ISDv5 30C", .type = WACOM_24HDT, /* Touch */
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x30A, .touch_max = 10,
 	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
+static const struct wacom_features wacom_features_0x318 =
+	{ "Wacom USB Bamboo PAD", 4095, 4095, /* Touch */
+	  .type = BAMBOO_PAD, 35, 48, .touch_max = 4 };
+static const struct wacom_features wacom_features_0x319 =
+	{ "Wacom Wireless Bamboo PAD", 4095, 4095, /* Touch */
+	  .type = BAMBOO_PAD, 35, 48, .touch_max = 4 };
 static const struct wacom_features wacom_features_0x323 =
 	{ "Wacom Intuos P M", 21600, 13500, 1023, 31,
 	  INTUOSHT, WACOM_INTUOS_RES, WACOM_INTUOS_RES,
@@ -2974,6 +3127,10 @@ static const struct wacom_features wacom_features_HID_ANY_ID =
 	HID_DEVICE(BUS_BLUETOOTH, HID_GROUP_WACOM, USB_VENDOR_ID_WACOM, prod),\
 	.driver_data = (kernel_ulong_t)&wacom_features_##prod
 
+#define I2C_DEVICE_WACOM(prod)						\
+	HID_DEVICE(BUS_I2C, HID_GROUP_WACOM, USB_VENDOR_ID_WACOM, prod),\
+	.driver_data = (kernel_ulong_t)&wacom_features_##prod
+
 #define USB_DEVICE_LENOVO(prod)					\
 	HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, prod),			\
 	.driver_data = (kernel_ulong_t)&wacom_features_##prod
@@ -3106,11 +3263,15 @@ const struct hid_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x314) },
 	{ USB_DEVICE_WACOM(0x315) },
 	{ USB_DEVICE_WACOM(0x317) },
+	{ USB_DEVICE_WACOM(0x318) },
+	{ USB_DEVICE_WACOM(0x319) },
 	{ USB_DEVICE_WACOM(0x323) },
 	{ USB_DEVICE_WACOM(0x32A) },
 	{ USB_DEVICE_WACOM(0x32B) },
 	{ USB_DEVICE_WACOM(0x32C) },
 	{ USB_DEVICE_WACOM(0x32F) },
+	{ USB_DEVICE_WACOM(0x333) },
+	{ USB_DEVICE_WACOM(0x335) },
 	{ USB_DEVICE_WACOM(0x4001) },
 	{ USB_DEVICE_WACOM(0x4004) },
 	{ USB_DEVICE_WACOM(0x5000) },
@@ -3118,6 +3279,7 @@ const struct hid_device_id wacom_ids[] = {
 	{ USB_DEVICE_LENOVO(0x6004) },
 
 	{ USB_DEVICE_WACOM(HID_ANY_ID) },
+	{ I2C_DEVICE_WACOM(HID_ANY_ID) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, wacom_ids);
diff --git a/drivers/hid/wacom_wac.h b/drivers/hid/wacom_wac.h
index 021ee1c1980a..4700ac994a3b 100644
--- a/drivers/hid/wacom_wac.h
+++ b/drivers/hid/wacom_wac.h
@@ -33,6 +33,8 @@
 #define WACOM_PKGLEN_MTTPC	40
 #define WACOM_PKGLEN_DTUS	68
 #define WACOM_PKGLEN_PENABLED	 8
+#define WACOM_PKGLEN_BPAD_TOUCH	32
+#define WACOM_PKGLEN_BPAD_TOUCH_USB	64
 
 /* wacom data size per MT contact */
 #define WACOM_BYTES_PER_MT_PACKET	11
@@ -67,13 +69,14 @@
 #define WACOM_REPORT_24HDT		1
 #define WACOM_REPORT_WL			128
 #define WACOM_REPORT_USB		192
+#define WACOM_REPORT_BPAD_PEN		3
+#define WACOM_REPORT_BPAD_TOUCH		16
 
 /* device quirks */
-#define WACOM_QUIRK_MULTI_INPUT		0x0001
-#define WACOM_QUIRK_BBTOUCH_LOWRES	0x0002
-#define WACOM_QUIRK_NO_INPUT		0x0004
-#define WACOM_QUIRK_MONITOR		0x0008
-#define WACOM_QUIRK_BATTERY		0x0010
+#define WACOM_QUIRK_BBTOUCH_LOWRES	0x0001
+#define WACOM_QUIRK_NO_INPUT		0x0002
+#define WACOM_QUIRK_MONITOR		0x0004
+#define WACOM_QUIRK_BATTERY		0x0008
 
 #define WACOM_PEN_FIELD(f)	(((f)->logical == HID_DG_STYLUS) || \
 				 ((f)->physical == HID_DG_STYLUS) || \
@@ -122,6 +125,7 @@ enum {
 	BAMBOO_PT,
 	WACOM_24HDT,
 	WACOM_27QHDT,
+	BAMBOO_PAD,
 	TABLETPC,   /* add new TPC below */
 	TABLETPCE,
 	TABLETPC2FG,
@@ -169,6 +173,8 @@ struct wacom_shared {
 	unsigned touch_max;
 	int type;
 	struct input_dev *touch_input;
+	struct hid_device *pen;
+	struct hid_device *touch;
 };
 
 struct hid_data {
@@ -205,6 +211,7 @@ struct wacom_wac {
 	int battery_capacity;
 	int num_contacts_left;
 	int bat_charging;
+	int bat_connected;
 	int ps_connected;
 	u8 bt_features;
 	u8 bt_high_speed;
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/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/Kconfig b/drivers/i2c/busses/Kconfig
index 22da9c2ffa22..2255af23b9c7 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -485,6 +485,15 @@ config I2C_DESIGNWARE_BAYTRAIL
 	  the platform firmware controlling it. You should say Y if running on
 	  a BayTrail system using the AXP288.
 
+config I2C_DIGICOLOR
+	tristate "Conexant Digicolor I2C driver"
+	depends on ARCH_DIGICOLOR
+	help
+	  Support for Conexant Digicolor SoCs (CX92755) I2C controller driver.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-digicolor.
+
 config I2C_EFM32
 	tristate "EFM32 I2C controller"
 	depends on ARCH_EFM32 || COMPILE_TEST
@@ -574,6 +583,15 @@ config I2C_IOP3XX
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-iop3xx.
 
+config I2C_JZ4780
+	tristate "JZ4780 I2C controller interface support"
+	depends on MACH_JZ4780 || COMPILE_TEST
+	help
+	 If you say yes to this option, support will be included for the
+	 Ingenic JZ4780 I2C controller.
+
+	 If you don't know what to do here, say N.
+
 config I2C_KEMPLD
 	tristate "Kontron COM I2C Controller"
 	depends on MFD_KEMPLD
@@ -898,6 +916,16 @@ config I2C_XLR
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-xlr.
 
+config I2C_XLP9XX
+	tristate "XLP9XX I2C support"
+	depends on CPU_XLP || COMPILE_TEST
+	help
+	  This driver enables support for the on-chip I2C interface of
+	  the Broadcom XLP9xx/XLP5xx MIPS processors.
+
+	  This driver can also be built as a module.  If so, the module will
+	  be called i2c-xlp9xx.
+
 config I2C_RCAR
 	tristate "Renesas R-Car I2C Controller"
 	depends on ARCH_SHMOBILE || COMPILE_TEST
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 3638feb6677e..cdf941da91c6 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -45,6 +45,7 @@ i2c-designware-platform-objs := i2c-designware-platdrv.o
 i2c-designware-platform-$(CONFIG_I2C_DESIGNWARE_BAYTRAIL) += i2c-designware-baytrail.o
 obj-$(CONFIG_I2C_DESIGNWARE_PCI)	+= i2c-designware-pci.o
 i2c-designware-pci-objs := i2c-designware-pcidrv.o
+obj-$(CONFIG_I2C_DIGICOLOR)	+= i2c-digicolor.o
 obj-$(CONFIG_I2C_EFM32)		+= i2c-efm32.o
 obj-$(CONFIG_I2C_EG20T)		+= i2c-eg20t.o
 obj-$(CONFIG_I2C_EXYNOS5)	+= i2c-exynos5.o
@@ -55,6 +56,7 @@ obj-$(CONFIG_I2C_IBM_IIC)	+= i2c-ibm_iic.o
 obj-$(CONFIG_I2C_IMG)		+= i2c-img-scb.o
 obj-$(CONFIG_I2C_IMX)		+= i2c-imx.o
 obj-$(CONFIG_I2C_IOP3XX)	+= i2c-iop3xx.o
+obj-$(CONFIG_I2C_JZ4780)	+= i2c-jz4780.o
 obj-$(CONFIG_I2C_KEMPLD)	+= i2c-kempld.o
 obj-$(CONFIG_I2C_MESON)		+= i2c-meson.o
 obj-$(CONFIG_I2C_MPC)		+= i2c-mpc.o
@@ -87,6 +89,7 @@ obj-$(CONFIG_I2C_WMT)		+= i2c-wmt.o
 obj-$(CONFIG_I2C_OCTEON)	+= i2c-octeon.o
 obj-$(CONFIG_I2C_XILINX)	+= i2c-xiic.o
 obj-$(CONFIG_I2C_XLR)		+= i2c-xlr.o
+obj-$(CONFIG_I2C_XLP9XX)	+= i2c-xlp9xx.o
 obj-$(CONFIG_I2C_RCAR)		+= i2c-rcar.o
 
 # External I2C/SMBus adapter drivers
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index 636fd2efad88..ff23d1bdd230 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -381,6 +381,7 @@ static irqreturn_t atmel_twi_interrupt(int irq, void *dev_id)
 static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 {
 	int ret;
+	unsigned long time_left;
 	bool has_unre_flag = dev->pdata->has_unre_flag;
 
 	dev_dbg(dev->dev, "transfer: %s %d bytes.\n",
@@ -436,9 +437,9 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 		}
 	}
 
-	ret = wait_for_completion_timeout(&dev->cmd_complete,
-					     dev->adapter.timeout);
-	if (ret == 0) {
+	time_left = wait_for_completion_timeout(&dev->cmd_complete,
+					      dev->adapter.timeout);
+	if (time_left == 0) {
 		dev_err(dev->dev, "controller timed out\n");
 		at91_init_twi_bus(dev);
 		ret = -ETIMEDOUT;
@@ -487,30 +488,10 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 	if (ret < 0)
 		goto out;
 
-	/*
-	 * The hardware can handle at most two messages concatenated by a
-	 * repeated start via it's internal address feature.
-	 */
-	if (num > 2) {
-		dev_err(dev->dev,
-			"cannot handle more than two concatenated messages.\n");
-		ret = 0;
-		goto out;
-	} else if (num == 2) {
+	if (num == 2) {
 		int internal_address = 0;
 		int i;
 
-		if (msg->flags & I2C_M_RD) {
-			dev_err(dev->dev, "first transfer must be write.\n");
-			ret = -EINVAL;
-			goto out;
-		}
-		if (msg->len > 3) {
-			dev_err(dev->dev, "first message size must be <= 3.\n");
-			ret = -EINVAL;
-			goto out;
-		}
-
 		/* 1st msg is put into the internal address, start with 2nd */
 		m_start = &msg[1];
 		for (i = 0; i < msg->len; ++i) {
@@ -540,6 +521,15 @@ out:
 	return ret;
 }
 
+/*
+ * The hardware can handle at most two messages concatenated by a
+ * repeated start via it's internal address feature.
+ */
+static struct i2c_adapter_quirks at91_twi_quirks = {
+	.flags = I2C_AQ_COMB | I2C_AQ_COMB_WRITE_FIRST | I2C_AQ_COMB_SAME_ADDR,
+	.max_comb_1st_msg_len = 3,
+};
+
 static u32 at91_twi_func(struct i2c_adapter *adapter)
 {
 	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL
@@ -777,6 +767,7 @@ static int at91_twi_probe(struct platform_device *pdev)
 	dev->adapter.owner = THIS_MODULE;
 	dev->adapter.class = I2C_CLASS_DEPRECATED;
 	dev->adapter.algo = &at91_twi_algorithm;
+	dev->adapter.quirks = &at91_twi_quirks;
 	dev->adapter.dev.parent = dev->dev;
 	dev->adapter.nr = pdev->id;
 	dev->adapter.timeout = AT91_I2C_TIMEOUT;
diff --git a/drivers/i2c/busses/i2c-axxia.c b/drivers/i2c/busses/i2c-axxia.c
index 768a598d8d03..32d883490863 100644
--- a/drivers/i2c/busses/i2c-axxia.c
+++ b/drivers/i2c/busses/i2c-axxia.c
@@ -334,12 +334,7 @@ static int axxia_i2c_xfer_msg(struct axxia_i2c_dev *idev, struct i2c_msg *msg)
 	u32 int_mask = MST_STATUS_ERR | MST_STATUS_SNS;
 	u32 rx_xfer, tx_xfer;
 	u32 addr_1, addr_2;
-	int ret;
-
-	if (msg->len > 255) {
-		dev_warn(idev->dev, "unsupported length %u\n", msg->len);
-		return -EINVAL;
-	}
+	unsigned long time_left;
 
 	idev->msg = msg;
 	idev->msg_xfrd = 0;
@@ -388,15 +383,15 @@ static int axxia_i2c_xfer_msg(struct axxia_i2c_dev *idev, struct i2c_msg *msg)
 
 	i2c_int_enable(idev, int_mask);
 
-	ret = wait_for_completion_timeout(&idev->msg_complete,
-					  I2C_XFER_TIMEOUT);
+	time_left = wait_for_completion_timeout(&idev->msg_complete,
+					      I2C_XFER_TIMEOUT);
 
 	i2c_int_disable(idev, int_mask);
 
 	if (readl(idev->base + MST_COMMAND) & CMD_BUSY)
 		dev_warn(idev->dev, "busy after xfer\n");
 
-	if (ret == 0)
+	if (time_left == 0)
 		idev->msg_err = -ETIMEDOUT;
 
 	if (unlikely(idev->msg_err) && idev->msg_err != -ENXIO)
@@ -408,17 +403,17 @@ static int axxia_i2c_xfer_msg(struct axxia_i2c_dev *idev, struct i2c_msg *msg)
 static int axxia_i2c_stop(struct axxia_i2c_dev *idev)
 {
 	u32 int_mask = MST_STATUS_ERR | MST_STATUS_SCC;
-	int ret;
+	unsigned long time_left;
 
 	reinit_completion(&idev->msg_complete);
 
 	/* Issue stop */
 	writel(0xb, idev->base + MST_COMMAND);
 	i2c_int_enable(idev, int_mask);
-	ret = wait_for_completion_timeout(&idev->msg_complete,
-					  I2C_STOP_TIMEOUT);
+	time_left = wait_for_completion_timeout(&idev->msg_complete,
+					      I2C_STOP_TIMEOUT);
 	i2c_int_disable(idev, int_mask);
-	if (ret == 0)
+	if (time_left == 0)
 		return -ETIMEDOUT;
 
 	if (readl(idev->base + MST_COMMAND) & CMD_BUSY)
@@ -454,6 +449,11 @@ static const struct i2c_algorithm axxia_i2c_algo = {
 	.functionality = axxia_i2c_func,
 };
 
+static struct i2c_adapter_quirks axxia_i2c_quirks = {
+	.max_read_len = 255,
+	.max_write_len = 255,
+};
+
 static int axxia_i2c_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
@@ -511,6 +511,7 @@ static int axxia_i2c_probe(struct platform_device *pdev)
 	strlcpy(idev->adapter.name, pdev->name, sizeof(idev->adapter.name));
 	idev->adapter.owner = THIS_MODULE;
 	idev->adapter.algo = &axxia_i2c_algo;
+	idev->adapter.quirks = &axxia_i2c_quirks;
 	idev->adapter.dev.parent = &pdev->dev;
 	idev->adapter.dev.of_node = pdev->dev.of_node;
 
diff --git a/drivers/i2c/busses/i2c-bcm-iproc.c b/drivers/i2c/busses/i2c-bcm-iproc.c
index d3c89157b337..f9f2c2082151 100644
--- a/drivers/i2c/busses/i2c-bcm-iproc.c
+++ b/drivers/i2c/busses/i2c-bcm-iproc.c
@@ -160,14 +160,6 @@ static int bcm_iproc_i2c_xfer_single_msg(struct bcm_iproc_i2c_dev *iproc_i2c,
 	u32 val;
 	unsigned long time_left = msecs_to_jiffies(I2C_TIMEOUT_MESC);
 
-	/* need to reserve one byte in the FIFO for the slave address */
-	if (msg->len > M_TX_RX_FIFO_SIZE - 1) {
-		dev_err(iproc_i2c->device,
-			"only support data length up to %u bytes\n",
-			M_TX_RX_FIFO_SIZE - 1);
-		return -EOPNOTSUPP;
-	}
-
 	/* check if bus is busy */
 	if (!!(readl(iproc_i2c->base + M_CMD_OFFSET) &
 	       BIT(M_CMD_START_BUSY_SHIFT))) {
@@ -287,6 +279,12 @@ static const struct i2c_algorithm bcm_iproc_algo = {
 	.functionality = bcm_iproc_i2c_functionality,
 };
 
+static struct i2c_adapter_quirks bcm_iproc_i2c_quirks = {
+	/* need to reserve one byte in the FIFO for the slave address */
+	.max_read_len = M_TX_RX_FIFO_SIZE - 1,
+	.max_write_len = M_TX_RX_FIFO_SIZE - 1,
+};
+
 static int bcm_iproc_i2c_cfg_speed(struct bcm_iproc_i2c_dev *iproc_i2c)
 {
 	unsigned int bus_speed;
@@ -413,6 +411,7 @@ static int bcm_iproc_i2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(adap, iproc_i2c);
 	strlcpy(adap->name, "Broadcom iProc I2C adapter", sizeof(adap->name));
 	adap->algo = &bcm_iproc_algo;
+	adap->quirks = &bcm_iproc_i2c_quirks;
 	adap->dev.parent = &pdev->dev;
 	adap->dev.of_node = pdev->dev.of_node;
 
diff --git a/drivers/i2c/busses/i2c-bcm2835.c b/drivers/i2c/busses/i2c-bcm2835.c
index 5d6feb937b9d..c9336a3202d5 100644
--- a/drivers/i2c/busses/i2c-bcm2835.c
+++ b/drivers/i2c/busses/i2c-bcm2835.c
@@ -147,7 +147,7 @@ static int bcm2835_i2c_xfer_msg(struct bcm2835_i2c_dev *i2c_dev,
 				struct i2c_msg *msg)
 {
 	u32 c;
-	int time_left;
+	unsigned long time_left;
 
 	i2c_dev->msg_buf = msg->buf;
 	i2c_dev->msg_buf_remaining = msg->len;
diff --git a/drivers/i2c/busses/i2c-cadence.c b/drivers/i2c/busses/i2c-cadence.c
index 7d7a14cdadfb..2ee78e099d30 100644
--- a/drivers/i2c/busses/i2c-cadence.c
+++ b/drivers/i2c/busses/i2c-cadence.c
@@ -475,7 +475,7 @@ static void cdns_i2c_master_reset(struct i2c_adapter *adap)
 static int cdns_i2c_process_msg(struct cdns_i2c *id, struct i2c_msg *msg,
 		struct i2c_adapter *adap)
 {
-	int ret;
+	unsigned long time_left;
 	u32 reg;
 
 	id->p_msg = msg;
@@ -501,8 +501,8 @@ static int cdns_i2c_process_msg(struct cdns_i2c *id, struct i2c_msg *msg,
 		cdns_i2c_msend(id);
 
 	/* Wait for the signal of completion */
-	ret = wait_for_completion_timeout(&id->xfer_done, adap->timeout);
-	if (!ret) {
+	time_left = wait_for_completion_timeout(&id->xfer_done, adap->timeout);
+	if (time_left == 0) {
 		cdns_i2c_master_reset(adap);
 		dev_err(id->adap.dev.parent,
 				"timeout waiting on completion\n");
diff --git a/drivers/i2c/busses/i2c-cpm.c b/drivers/i2c/busses/i2c-cpm.c
index 2d466538b2e2..714bdc837769 100644
--- a/drivers/i2c/busses/i2c-cpm.c
+++ b/drivers/i2c/busses/i2c-cpm.c
@@ -308,22 +308,12 @@ static int cpm_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 	struct i2c_reg __iomem *i2c_reg = cpm->i2c_reg;
 	struct i2c_ram __iomem *i2c_ram = cpm->i2c_ram;
 	struct i2c_msg *pmsg;
-	int ret, i;
+	int ret;
 	int tptr;
 	int rptr;
 	cbd_t __iomem *tbdf;
 	cbd_t __iomem *rbdf;
 
-	if (num > CPM_MAXBD)
-		return -EINVAL;
-
-	/* Check if we have any oversized READ requests */
-	for (i = 0; i < num; i++) {
-		pmsg = &msgs[i];
-		if (pmsg->len >= CPM_MAX_READ)
-			return -EINVAL;
-	}
-
 	/* Reset to use first buffer */
 	out_be16(&i2c_ram->rbptr, in_be16(&i2c_ram->rbase));
 	out_be16(&i2c_ram->tbptr, in_be16(&i2c_ram->tbase));
@@ -424,10 +414,18 @@ static const struct i2c_algorithm cpm_i2c_algo = {
 	.functionality = cpm_i2c_func,
 };
 
+/* CPM_MAX_READ is also limiting writes according to the code! */
+static struct i2c_adapter_quirks cpm_i2c_quirks = {
+	.max_num_msgs = CPM_MAXBD,
+	.max_read_len = CPM_MAX_READ,
+	.max_write_len = CPM_MAX_READ,
+};
+
 static const struct i2c_adapter cpm_ops = {
 	.owner		= THIS_MODULE,
 	.name		= "i2c-cpm",
 	.algo		= &cpm_i2c_algo,
+	.quirks		= &cpm_i2c_quirks,
 };
 
 static int cpm_i2c_setup(struct cpm_i2c *cpm)
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index 6dc7ff5d3d9a..4788a32afb86 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -60,6 +60,12 @@
 #define DAVINCI_I2C_IVR_REG	0x28
 #define DAVINCI_I2C_EMDR_REG	0x2c
 #define DAVINCI_I2C_PSC_REG	0x30
+#define DAVINCI_I2C_FUNC_REG	0x48
+#define DAVINCI_I2C_DIR_REG	0x4c
+#define DAVINCI_I2C_DIN_REG	0x50
+#define DAVINCI_I2C_DOUT_REG	0x54
+#define DAVINCI_I2C_DSET_REG	0x58
+#define DAVINCI_I2C_DCLR_REG	0x5c
 
 #define DAVINCI_I2C_IVR_AAS	0x07
 #define DAVINCI_I2C_IVR_SCD	0x06
@@ -93,6 +99,29 @@
 #define DAVINCI_I2C_IMR_NACK	BIT(1)
 #define DAVINCI_I2C_IMR_AL	BIT(0)
 
+/* set SDA and SCL as GPIO */
+#define DAVINCI_I2C_FUNC_PFUNC0	BIT(0)
+
+/* set SCL as output when used as GPIO*/
+#define DAVINCI_I2C_DIR_PDIR0	BIT(0)
+/* set SDA as output when used as GPIO*/
+#define DAVINCI_I2C_DIR_PDIR1	BIT(1)
+
+/* read SCL GPIO level */
+#define DAVINCI_I2C_DIN_PDIN0 BIT(0)
+/* read SDA GPIO level */
+#define DAVINCI_I2C_DIN_PDIN1 BIT(1)
+
+/*set the SCL GPIO high */
+#define DAVINCI_I2C_DSET_PDSET0	BIT(0)
+/*set the SDA GPIO high */
+#define DAVINCI_I2C_DSET_PDSET1	BIT(1)
+
+/* set the SCL GPIO low */
+#define DAVINCI_I2C_DCLR_PDCLR0	BIT(0)
+/* set the SDA GPIO low */
+#define DAVINCI_I2C_DCLR_PDCLR1	BIT(1)
+
 struct davinci_i2c_dev {
 	struct device           *dev;
 	void __iomem		*base;
@@ -129,43 +158,6 @@ static inline u16 davinci_i2c_read_reg(struct davinci_i2c_dev *i2c_dev, int reg)
 	return readw_relaxed(i2c_dev->base + reg);
 }
 
-/* Generate a pulse on the i2c clock pin. */
-static void davinci_i2c_clock_pulse(unsigned int scl_pin)
-{
-	u16 i;
-
-	if (scl_pin) {
-		/* Send high and low on the SCL line */
-		for (i = 0; i < 9; i++) {
-			gpio_set_value(scl_pin, 0);
-			udelay(20);
-			gpio_set_value(scl_pin, 1);
-			udelay(20);
-		}
-	}
-}
-
-/* This routine does i2c bus recovery as specified in the
- * i2c protocol Rev. 03 section 3.16 titled "Bus clear"
- */
-static void davinci_i2c_recover_bus(struct davinci_i2c_dev *dev)
-{
-	u32 flag = 0;
-	struct davinci_i2c_platform_data *pdata = dev->pdata;
-
-	dev_err(dev->dev, "initiating i2c bus recovery\n");
-	/* Send NACK to the slave */
-	flag = davinci_i2c_read_reg(dev, DAVINCI_I2C_MDR_REG);
-	flag |=  DAVINCI_I2C_MDR_NACK;
-	/* write the data into mode register */
-	davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, flag);
-	davinci_i2c_clock_pulse(pdata->scl_pin);
-	/* Send STOP */
-	flag = davinci_i2c_read_reg(dev, DAVINCI_I2C_MDR_REG);
-	flag |= DAVINCI_I2C_MDR_STP;
-	davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, flag);
-}
-
 static inline void davinci_i2c_reset_ctrl(struct davinci_i2c_dev *i2c_dev,
 								int val)
 {
@@ -263,6 +255,99 @@ static int i2c_davinci_init(struct davinci_i2c_dev *dev)
 }
 
 /*
+ * This routine does i2c bus recovery by using i2c_generic_gpio_recovery
+ * which is provided by I2C Bus recovery infrastructure.
+ */
+static void davinci_i2c_prepare_recovery(struct i2c_adapter *adap)
+{
+	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
+
+	/* Disable interrupts */
+	davinci_i2c_write_reg(dev, DAVINCI_I2C_IMR_REG, 0);
+
+	/* put I2C into reset */
+	davinci_i2c_reset_ctrl(dev, 0);
+}
+
+static void davinci_i2c_unprepare_recovery(struct i2c_adapter *adap)
+{
+	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
+
+	i2c_davinci_init(dev);
+}
+
+static struct i2c_bus_recovery_info davinci_i2c_gpio_recovery_info = {
+	.recover_bus = i2c_generic_gpio_recovery,
+	.prepare_recovery = davinci_i2c_prepare_recovery,
+	.unprepare_recovery = davinci_i2c_unprepare_recovery,
+};
+
+static void davinci_i2c_set_scl(struct i2c_adapter *adap, int val)
+{
+	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
+
+	if (val)
+		davinci_i2c_write_reg(dev, DAVINCI_I2C_DSET_REG,
+				      DAVINCI_I2C_DSET_PDSET0);
+	else
+		davinci_i2c_write_reg(dev, DAVINCI_I2C_DCLR_REG,
+				      DAVINCI_I2C_DCLR_PDCLR0);
+}
+
+static int davinci_i2c_get_scl(struct i2c_adapter *adap)
+{
+	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
+	int val;
+
+	/* read the state of SCL */
+	val = davinci_i2c_read_reg(dev, DAVINCI_I2C_DIN_REG);
+	return val & DAVINCI_I2C_DIN_PDIN0;
+}
+
+static int davinci_i2c_get_sda(struct i2c_adapter *adap)
+{
+	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
+	int val;
+
+	/* read the state of SDA */
+	val = davinci_i2c_read_reg(dev, DAVINCI_I2C_DIN_REG);
+	return val & DAVINCI_I2C_DIN_PDIN1;
+}
+
+static void davinci_i2c_scl_prepare_recovery(struct i2c_adapter *adap)
+{
+	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
+
+	davinci_i2c_prepare_recovery(adap);
+
+	/* SCL output, SDA input */
+	davinci_i2c_write_reg(dev, DAVINCI_I2C_DIR_REG, DAVINCI_I2C_DIR_PDIR0);
+
+	/* change to GPIO mode */
+	davinci_i2c_write_reg(dev, DAVINCI_I2C_FUNC_REG,
+			      DAVINCI_I2C_FUNC_PFUNC0);
+}
+
+static void davinci_i2c_scl_unprepare_recovery(struct i2c_adapter *adap)
+{
+	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
+
+	/* change back to I2C mode */
+	davinci_i2c_write_reg(dev, DAVINCI_I2C_FUNC_REG, 0);
+
+	davinci_i2c_unprepare_recovery(adap);
+}
+
+static struct i2c_bus_recovery_info davinci_i2c_scl_recovery_info = {
+	.recover_bus = i2c_generic_scl_recovery,
+	.set_scl = davinci_i2c_set_scl,
+	.get_scl = davinci_i2c_get_scl,
+	.get_sda = davinci_i2c_get_sda,
+	.prepare_recovery = davinci_i2c_scl_prepare_recovery,
+	.unprepare_recovery = davinci_i2c_scl_unprepare_recovery,
+};
+
+/*
  * Waiting for bus not busy
  */
 static int i2c_davinci_wait_bus_not_busy(struct davinci_i2c_dev *dev,
@@ -282,8 +367,7 @@ static int i2c_davinci_wait_bus_not_busy(struct davinci_i2c_dev *dev,
 				return -ETIMEDOUT;
 			} else {
 				to_cnt = 0;
-				davinci_i2c_recover_bus(dev);
-				i2c_davinci_init(dev);
+				i2c_recover_bus(&dev->adapter);
 			}
 		}
 		if (allow_sleep)
@@ -304,7 +388,7 @@ i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 	struct davinci_i2c_platform_data *pdata = dev->pdata;
 	u32 flag;
 	u16 w;
-	int r;
+	unsigned long time_left;
 
 	/* Introduce a delay, required for some boards (e.g Davinci EVM) */
 	if (pdata->bus_delay)
@@ -368,11 +452,11 @@ i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 		flag |= DAVINCI_I2C_MDR_STP;
 	davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, flag);
 
-	r = wait_for_completion_timeout(&dev->cmd_complete, dev->adapter.timeout);
-	if (r == 0) {
+	time_left = wait_for_completion_timeout(&dev->cmd_complete,
+						dev->adapter.timeout);
+	if (!time_left) {
 		dev_err(dev->dev, "controller timed out\n");
-		davinci_i2c_recover_bus(dev);
-		i2c_davinci_init(dev);
+		i2c_recover_bus(adap);
 		dev->buf_len = 0;
 		return -ETIMEDOUT;
 	}
@@ -380,17 +464,13 @@ i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 		/* This should be 0 if all bytes were transferred
 		 * or dev->cmd_err denotes an error.
 		 */
-		if (r >= 0) {
-			dev_err(dev->dev, "abnormal termination buf_len=%i\n",
-				dev->buf_len);
-			r = -EREMOTEIO;
-		}
+		dev_err(dev->dev, "abnormal termination buf_len=%i\n",
+			dev->buf_len);
 		dev->terminate = 1;
 		wmb();
 		dev->buf_len = 0;
+		return -EREMOTEIO;
 	}
-	if (r < 0)
-		return r;
 
 	/* no error */
 	if (likely(!dev->cmd_err))
@@ -674,6 +754,10 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 		if (!of_property_read_u32(pdev->dev.of_node, "clock-frequency",
 			&prop))
 			dev->pdata->bus_freq = prop / 1000;
+
+		dev->pdata->has_pfunc =
+			of_property_read_bool(pdev->dev.of_node,
+					      "ti,has-pfunc");
 	} else if (!dev->pdata) {
 		dev->pdata = &davinci_i2c_platform_data_default;
 	}
@@ -715,6 +799,14 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 	adap->timeout = DAVINCI_I2C_TIMEOUT;
 	adap->dev.of_node = pdev->dev.of_node;
 
+	if (dev->pdata->has_pfunc)
+		adap->bus_recovery_info = &davinci_i2c_scl_recovery_info;
+	else if (dev->pdata->scl_pin) {
+		adap->bus_recovery_info = &davinci_i2c_gpio_recovery_info;
+		adap->bus_recovery_info->scl_gpio = dev->pdata->scl_pin;
+		adap->bus_recovery_info->sda_gpio = dev->pdata->sda_pin;
+	}
+
 	adap->nr = pdev->id;
 	r = i2c_add_numbered_adapter(adap);
 	if (r) {
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index 6e25c010e690..6f19a33773fe 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -656,8 +656,7 @@ i2c_dw_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	i2c_dw_xfer_init(dev);
 
 	/* wait for tx to complete */
-	ret = wait_for_completion_timeout(&dev->cmd_complete, HZ);
-	if (ret == 0) {
+	if (!wait_for_completion_timeout(&dev->cmd_complete, HZ)) {
 		dev_err(dev->dev, "controller timed out\n");
 		/* i2c_dw_init implicitly disables the adapter */
 		i2c_dw_init(dev);
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index c270f5f9a8f9..0a80e4aabaed 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -143,10 +143,8 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	u32 clk_freq, ht = 0;
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		dev_err(&pdev->dev, "no irq resource?\n");
-		return irq; /* -ENXIO */
-	}
+	if (irq < 0)
+		return irq;
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(struct dw_i2c_dev), GFP_KERNEL);
 	if (!dev)
@@ -166,7 +164,7 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	/* fast mode by default because of legacy reasons */
 	clk_freq = 400000;
 
-	if (ACPI_COMPANION(&pdev->dev)) {
+	if (has_acpi_companion(&pdev->dev)) {
 		dw_i2c_acpi_configure(pdev);
 	} else if (pdev->dev.of_node) {
 		of_property_read_u32(pdev->dev.of_node,
@@ -286,7 +284,7 @@ static int dw_i2c_remove(struct platform_device *pdev)
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
-	if (ACPI_COMPANION(&pdev->dev))
+	if (has_acpi_companion(&pdev->dev))
 		dw_i2c_acpi_unconfigure(pdev);
 
 	return 0;
diff --git a/drivers/i2c/busses/i2c-digicolor.c b/drivers/i2c/busses/i2c-digicolor.c
new file mode 100644
index 000000000000..03f1e5549896
--- /dev/null
+++ b/drivers/i2c/busses/i2c-digicolor.c
@@ -0,0 +1,385 @@
+/*
+ * I2C bus driver for Conexant Digicolor SoCs
+ *
+ * Author: Baruch Siach <baruch@tkos.co.il>
+ *
+ * Copyright (C) 2015 Paradox Innovation Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#define DEFAULT_FREQ		100000
+#define TIMEOUT_MS		100
+
+#define II_CONTROL		0x0
+#define II_CONTROL_LOCAL_RESET	BIT(0)
+
+#define II_CLOCKTIME		0x1
+
+#define II_COMMAND		0x2
+#define II_CMD_START		1
+#define II_CMD_RESTART		2
+#define II_CMD_SEND_ACK		3
+#define II_CMD_GET_ACK		6
+#define II_CMD_GET_NOACK	7
+#define II_CMD_STOP		10
+#define II_COMMAND_GO		BIT(7)
+#define II_COMMAND_COMPLETION_STATUS(r)	(((r) >> 5) & 3)
+#define II_CMD_STATUS_NORMAL	0
+#define II_CMD_STATUS_ACK_GOOD	1
+#define II_CMD_STATUS_ACK_BAD	2
+#define II_CMD_STATUS_ABORT	3
+
+#define II_DATA			0x3
+#define II_INTFLAG_CLEAR	0x8
+#define II_INTENABLE		0xa
+
+struct dc_i2c {
+	struct i2c_adapter	adap;
+	struct device		*dev;
+	void __iomem		*regs;
+	struct clk		*clk;
+	unsigned int		frequency;
+
+	struct i2c_msg		*msg;
+	unsigned int		msgbuf_ptr;
+	int			last;
+	spinlock_t		lock;
+	struct completion	done;
+	int			state;
+	int			error;
+};
+
+enum {
+	STATE_IDLE,
+	STATE_START,
+	STATE_ADDR,
+	STATE_WRITE,
+	STATE_READ,
+	STATE_STOP,
+};
+
+static void dc_i2c_cmd(struct dc_i2c *i2c, u8 cmd)
+{
+	writeb_relaxed(cmd | II_COMMAND_GO, i2c->regs + II_COMMAND);
+}
+
+static u8 dc_i2c_addr_cmd(struct i2c_msg *msg)
+{
+	u8 addr = (msg->addr & 0x7f) << 1;
+
+	if (msg->flags & I2C_M_RD)
+		addr |= 1;
+
+	return addr;
+}
+
+static void dc_i2c_data(struct dc_i2c *i2c, u8 data)
+{
+	writeb_relaxed(data, i2c->regs + II_DATA);
+}
+
+static void dc_i2c_write_byte(struct dc_i2c *i2c, u8 byte)
+{
+	dc_i2c_data(i2c, byte);
+	dc_i2c_cmd(i2c, II_CMD_SEND_ACK);
+}
+
+static void dc_i2c_write_buf(struct dc_i2c *i2c)
+{
+	dc_i2c_write_byte(i2c, i2c->msg->buf[i2c->msgbuf_ptr++]);
+}
+
+static void dc_i2c_next_read(struct dc_i2c *i2c)
+{
+	bool last = (i2c->msgbuf_ptr + 1 == i2c->msg->len);
+
+	dc_i2c_cmd(i2c, last ? II_CMD_GET_NOACK : II_CMD_GET_ACK);
+}
+
+static void dc_i2c_stop(struct dc_i2c *i2c)
+{
+	i2c->state = STATE_STOP;
+	if (i2c->last)
+		dc_i2c_cmd(i2c, II_CMD_STOP);
+	else
+		complete(&i2c->done);
+}
+
+static u8 dc_i2c_read_byte(struct dc_i2c *i2c)
+{
+	return readb_relaxed(i2c->regs + II_DATA);
+}
+
+static void dc_i2c_read_buf(struct dc_i2c *i2c)
+{
+	i2c->msg->buf[i2c->msgbuf_ptr++] = dc_i2c_read_byte(i2c);
+	dc_i2c_next_read(i2c);
+}
+
+static void dc_i2c_set_irq(struct dc_i2c *i2c, int enable)
+{
+	if (enable)
+		writeb_relaxed(1, i2c->regs + II_INTFLAG_CLEAR);
+	writeb_relaxed(!!enable, i2c->regs + II_INTENABLE);
+}
+
+static int dc_i2c_cmd_status(struct dc_i2c *i2c)
+{
+	u8 cmd = readb_relaxed(i2c->regs + II_COMMAND);
+
+	return II_COMMAND_COMPLETION_STATUS(cmd);
+}
+
+static void dc_i2c_start_msg(struct dc_i2c *i2c, int first)
+{
+	struct i2c_msg *msg = i2c->msg;
+
+	if (!(msg->flags & I2C_M_NOSTART)) {
+		i2c->state = STATE_START;
+		dc_i2c_cmd(i2c, first ? II_CMD_START : II_CMD_RESTART);
+	} else if (msg->flags & I2C_M_RD) {
+		i2c->state = STATE_READ;
+		dc_i2c_next_read(i2c);
+	} else {
+		i2c->state = STATE_WRITE;
+		dc_i2c_write_buf(i2c);
+	}
+}
+
+static irqreturn_t dc_i2c_irq(int irq, void *dev_id)
+{
+	struct dc_i2c *i2c = dev_id;
+	int cmd_status = dc_i2c_cmd_status(i2c);
+	unsigned long flags;
+	u8 addr_cmd;
+
+	writeb_relaxed(1, i2c->regs + II_INTFLAG_CLEAR);
+
+	spin_lock_irqsave(&i2c->lock, flags);
+
+	if (cmd_status == II_CMD_STATUS_ACK_BAD
+	    || cmd_status == II_CMD_STATUS_ABORT) {
+		i2c->error = -EIO;
+		complete(&i2c->done);
+		goto out;
+	}
+
+	switch (i2c->state) {
+	case STATE_START:
+		addr_cmd = dc_i2c_addr_cmd(i2c->msg);
+		dc_i2c_write_byte(i2c, addr_cmd);
+		i2c->state = STATE_ADDR;
+		break;
+	case STATE_ADDR:
+		if (i2c->msg->flags & I2C_M_RD) {
+			dc_i2c_next_read(i2c);
+			i2c->state = STATE_READ;
+			break;
+		}
+		i2c->state = STATE_WRITE;
+		/* fall through */
+	case STATE_WRITE:
+		if (i2c->msgbuf_ptr < i2c->msg->len)
+			dc_i2c_write_buf(i2c);
+		else
+			dc_i2c_stop(i2c);
+		break;
+	case STATE_READ:
+		if (i2c->msgbuf_ptr < i2c->msg->len)
+			dc_i2c_read_buf(i2c);
+		else
+			dc_i2c_stop(i2c);
+		break;
+	case STATE_STOP:
+		i2c->state = STATE_IDLE;
+		complete(&i2c->done);
+		break;
+	}
+
+out:
+	spin_unlock_irqrestore(&i2c->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static int dc_i2c_xfer_msg(struct dc_i2c *i2c, struct i2c_msg *msg, int first,
+			   int last)
+{
+	unsigned long timeout = msecs_to_jiffies(TIMEOUT_MS);
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2c->lock, flags);
+	i2c->msg = msg;
+	i2c->msgbuf_ptr = 0;
+	i2c->last = last;
+	i2c->error = 0;
+
+	reinit_completion(&i2c->done);
+	dc_i2c_set_irq(i2c, 1);
+	dc_i2c_start_msg(i2c, first);
+	spin_unlock_irqrestore(&i2c->lock, flags);
+
+	timeout = wait_for_completion_timeout(&i2c->done, timeout);
+	dc_i2c_set_irq(i2c, 0);
+	if (timeout == 0) {
+		i2c->state = STATE_IDLE;
+		return -ETIMEDOUT;
+	}
+
+	if (i2c->error)
+		return i2c->error;
+
+	return 0;
+}
+
+static int dc_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
+{
+	struct dc_i2c *i2c = adap->algo_data;
+	int i, ret;
+
+	for (i = 0; i < num; i++) {
+		ret = dc_i2c_xfer_msg(i2c, &msgs[i], i == 0, i == num - 1);
+		if (ret)
+			return ret;
+	}
+
+	return num;
+}
+
+static int dc_i2c_init_hw(struct dc_i2c *i2c)
+{
+	unsigned long clk_rate = clk_get_rate(i2c->clk);
+	unsigned int clocktime;
+
+	writeb_relaxed(II_CONTROL_LOCAL_RESET, i2c->regs + II_CONTROL);
+	udelay(100);
+	writeb_relaxed(0, i2c->regs + II_CONTROL);
+	udelay(100);
+
+	clocktime = DIV_ROUND_UP(clk_rate, 64 * i2c->frequency);
+	if (clocktime < 1 || clocktime > 0xff) {
+		dev_err(i2c->dev, "can't set bus speed of %u Hz\n",
+			i2c->frequency);
+		return -EINVAL;
+	}
+	writeb_relaxed(clocktime - 1, i2c->regs + II_CLOCKTIME);
+
+	return 0;
+}
+
+static u32 dc_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_NOSTART;
+}
+
+static const struct i2c_algorithm dc_i2c_algorithm = {
+	.master_xfer	= dc_i2c_xfer,
+	.functionality	= dc_i2c_func,
+};
+
+static int dc_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct dc_i2c *i2c;
+	struct resource *r;
+	int ret = 0, irq;
+
+	i2c = devm_kzalloc(&pdev->dev, sizeof(struct dc_i2c), GFP_KERNEL);
+	if (!i2c)
+		return -ENOMEM;
+
+	if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				 &i2c->frequency))
+		i2c->frequency = DEFAULT_FREQ;
+
+	i2c->dev = &pdev->dev;
+	platform_set_drvdata(pdev, i2c);
+
+	spin_lock_init(&i2c->lock);
+	init_completion(&i2c->done);
+
+	i2c->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(i2c->clk))
+		return PTR_ERR(i2c->clk);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	i2c->regs = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(i2c->regs))
+		return PTR_ERR(i2c->regs);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	ret = devm_request_irq(&pdev->dev, irq, dc_i2c_irq, 0,
+			       dev_name(&pdev->dev), i2c);
+	if (ret < 0)
+		return ret;
+
+	strlcpy(i2c->adap.name, "Conexant Digicolor I2C adapter",
+		sizeof(i2c->adap.name));
+	i2c->adap.owner = THIS_MODULE;
+	i2c->adap.algo = &dc_i2c_algorithm;
+	i2c->adap.dev.parent = &pdev->dev;
+	i2c->adap.dev.of_node = np;
+	i2c->adap.algo_data = i2c;
+
+	ret = dc_i2c_init_hw(i2c);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(i2c->clk);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_add_adapter(&i2c->adap);
+	if (ret < 0) {
+		clk_unprepare(i2c->clk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dc_i2c_remove(struct platform_device *pdev)
+{
+	struct dc_i2c *i2c = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&i2c->adap);
+	clk_disable_unprepare(i2c->clk);
+
+	return 0;
+}
+
+static const struct of_device_id dc_i2c_match[] = {
+	{ .compatible = "cnxt,cx92755-i2c" },
+	{ },
+};
+
+static struct platform_driver dc_i2c_driver = {
+	.probe   = dc_i2c_probe,
+	.remove  = dc_i2c_remove,
+	.driver  = {
+		.name  = "digicolor-i2c",
+		.of_match_table = dc_i2c_match,
+	},
+};
+module_platform_driver(dc_i2c_driver);
+
+MODULE_AUTHOR("Baruch Siach <baruch@tkos.co.il>");
+MODULE_DESCRIPTION("Conexant Digicolor I2C master driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-dln2.c b/drivers/i2c/busses/i2c-dln2.c
index b3fb86af4cbb..1600edd57ce9 100644
--- a/drivers/i2c/busses/i2c-dln2.c
+++ b/drivers/i2c/busses/i2c-dln2.c
@@ -144,7 +144,6 @@ static int dln2_i2c_xfer(struct i2c_adapter *adapter,
 {
 	struct dln2_i2c *dln2 = i2c_get_adapdata(adapter);
 	struct i2c_msg *pmsg;
-	struct device *dev = &dln2->adapter.dev;
 	int i;
 
 	for (i = 0; i < num; i++) {
@@ -152,11 +151,6 @@ static int dln2_i2c_xfer(struct i2c_adapter *adapter,
 
 		pmsg = &msgs[i];
 
-		if (pmsg->len > DLN2_I2C_MAX_XFER_SIZE) {
-			dev_warn(dev, "maximum transfer size exceeded\n");
-			return -EOPNOTSUPP;
-		}
-
 		if (pmsg->flags & I2C_M_RD) {
 			ret = dln2_i2c_read(dln2, pmsg->addr, pmsg->buf,
 					    pmsg->len);
@@ -187,6 +181,11 @@ static const struct i2c_algorithm dln2_i2c_usb_algorithm = {
 	.functionality = dln2_i2c_func,
 };
 
+static struct i2c_adapter_quirks dln2_i2c_quirks = {
+	.max_read_len = DLN2_I2C_MAX_XFER_SIZE,
+	.max_write_len = DLN2_I2C_MAX_XFER_SIZE,
+};
+
 static int dln2_i2c_probe(struct platform_device *pdev)
 {
 	int ret;
@@ -209,7 +208,9 @@ static int dln2_i2c_probe(struct platform_device *pdev)
 	dln2->adapter.owner = THIS_MODULE;
 	dln2->adapter.class = I2C_CLASS_HWMON;
 	dln2->adapter.algo = &dln2_i2c_usb_algorithm;
+	dln2->adapter.quirks = &dln2_i2c_quirks;
 	dln2->adapter.dev.parent = dev;
+	dln2->adapter.dev.of_node = dev->of_node;
 	i2c_set_adapdata(&dln2->adapter, dln2);
 	snprintf(dln2->adapter.name, sizeof(dln2->adapter.name), "%s-%s-%d",
 		 "dln2-i2c", dev_name(pdev->dev.parent), dln2->port);
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index 8fafb254e42a..5ecbb3fdc27e 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -223,8 +223,6 @@ struct i801_priv {
 #endif
 };
 
-static struct pci_driver i801_driver;
-
 #define FEATURE_SMBUS_PEC	(1 << 0)
 #define FEATURE_BLOCK_BUFFER	(1 << 1)
 #define FEATURE_BLOCK_PROC	(1 << 2)
@@ -1140,7 +1138,7 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	int err, i;
 	struct i801_priv *priv;
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&dev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
@@ -1182,34 +1180,35 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	}
 	priv->features &= ~disable_features;
 
-	err = pci_enable_device(dev);
+	err = pcim_enable_device(dev);
 	if (err) {
 		dev_err(&dev->dev, "Failed to enable SMBus PCI device (%d)\n",
 			err);
-		goto exit;
+		return err;
 	}
+	pcim_pin_device(dev);
 
 	/* Determine the address of the SMBus area */
 	priv->smba = pci_resource_start(dev, SMBBAR);
 	if (!priv->smba) {
-		dev_err(&dev->dev, "SMBus base address uninitialized, "
-			"upgrade BIOS\n");
-		err = -ENODEV;
-		goto exit;
+		dev_err(&dev->dev,
+			"SMBus base address uninitialized, upgrade BIOS\n");
+		return -ENODEV;
 	}
 
 	err = acpi_check_resource_conflict(&dev->resource[SMBBAR]);
 	if (err) {
-		err = -ENODEV;
-		goto exit;
+		return -ENODEV;
 	}
 
-	err = pci_request_region(dev, SMBBAR, i801_driver.name);
+	err = pcim_iomap_regions(dev, 1 << SMBBAR,
+				 dev_driver_string(&dev->dev));
 	if (err) {
-		dev_err(&dev->dev, "Failed to request SMBus region "
-			"0x%lx-0x%Lx\n", priv->smba,
+		dev_err(&dev->dev,
+			"Failed to request SMBus region 0x%lx-0x%Lx\n",
+			priv->smba,
 			(unsigned long long)pci_resource_end(dev, SMBBAR));
-		goto exit;
+		return err;
 	}
 
 	pci_read_config_byte(priv->pci_dev, SMBHSTCFG, &temp);
@@ -1254,8 +1253,9 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (priv->features & FEATURE_IRQ) {
 		init_waitqueue_head(&priv->waitq);
 
-		err = request_irq(dev->irq, i801_isr, IRQF_SHARED,
-				  i801_driver.name, priv);
+		err = devm_request_irq(&dev->dev, dev->irq, i801_isr,
+				       IRQF_SHARED,
+				       dev_driver_string(&dev->dev), priv);
 		if (err) {
 			dev_err(&dev->dev, "Failed to allocate irq %d: %d\n",
 				dev->irq, err);
@@ -1276,7 +1276,7 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	err = i2c_add_adapter(&priv->adapter);
 	if (err) {
 		dev_err(&dev->dev, "Failed to add SMBus adapter\n");
-		goto exit_free_irq;
+		return err;
 	}
 
 	i801_probe_optional_slaves(priv);
@@ -1286,14 +1286,6 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	pci_set_drvdata(dev, priv);
 
 	return 0;
-
-exit_free_irq:
-	if (priv->features & FEATURE_IRQ)
-		free_irq(dev->irq, priv);
-	pci_release_region(dev, SMBBAR);
-exit:
-	kfree(priv);
-	return err;
 }
 
 static void i801_remove(struct pci_dev *dev)
@@ -1304,11 +1296,6 @@ static void i801_remove(struct pci_dev *dev)
 	i2c_del_adapter(&priv->adapter);
 	pci_write_config_byte(dev, SMBHSTCFG, priv->original_hstcfg);
 
-	if (priv->features & FEATURE_IRQ)
-		free_irq(dev->irq, priv);
-	pci_release_region(dev, SMBBAR);
-
-	kfree(priv);
 	/*
 	 * do not call pci_disable_device(dev) since it can cause hard hangs on
 	 * some systems during power-off (eg. Fujitsu-Siemens Lifebook E8010)
@@ -1330,7 +1317,7 @@ static int i801_resume(struct pci_dev *dev)
 {
 	pci_set_power_state(dev, PCI_D0);
 	pci_restore_state(dev);
-	return pci_enable_device(dev);
+	return 0;
 }
 #else
 #define i801_suspend NULL
diff --git a/drivers/i2c/busses/i2c-img-scb.c b/drivers/i2c/busses/i2c-img-scb.c
index 0fcc1694c607..00ffd6613680 100644
--- a/drivers/i2c/busses/i2c-img-scb.c
+++ b/drivers/i2c/busses/i2c-img-scb.c
@@ -988,15 +988,16 @@ out:
 static int img_i2c_reset_bus(struct img_i2c *i2c)
 {
 	unsigned long flags;
-	int ret;
+	unsigned long time_left;
 
 	spin_lock_irqsave(&i2c->lock, flags);
 	reinit_completion(&i2c->msg_complete);
 	img_i2c_reset_start(i2c);
 	spin_unlock_irqrestore(&i2c->lock, flags);
 
-	ret = wait_for_completion_timeout(&i2c->msg_complete, IMG_I2C_TIMEOUT);
-	if (ret == 0)
+	time_left = wait_for_completion_timeout(&i2c->msg_complete,
+					      IMG_I2C_TIMEOUT);
+	if (time_left == 0)
 		return -ETIMEDOUT;
 	return 0;
 }
@@ -1007,6 +1008,7 @@ static int img_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 	struct img_i2c *i2c = i2c_get_adapdata(adap);
 	bool atomic = false;
 	int i, ret;
+	unsigned long time_left;
 
 	if (i2c->mode == MODE_SUSPEND) {
 		WARN(1, "refusing to service transaction in suspended state\n");
@@ -1068,11 +1070,11 @@ static int img_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 			img_i2c_write(i2c);
 		spin_unlock_irqrestore(&i2c->lock, flags);
 
-		ret = wait_for_completion_timeout(&i2c->msg_complete,
-						  IMG_I2C_TIMEOUT);
+		time_left = wait_for_completion_timeout(&i2c->msg_complete,
+						      IMG_I2C_TIMEOUT);
 		del_timer_sync(&i2c->check_timer);
 
-		if (ret == 0) {
+		if (time_left == 0) {
 			dev_err(adap->dev.parent, "i2c transfer timed out\n");
 			i2c->msg_status = -ETIMEDOUT;
 			break;
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index d7b26fc6f432..a53a7dd66945 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -601,6 +601,7 @@ static int i2c_imx_dma_write(struct imx_i2c_struct *i2c_imx,
 					struct i2c_msg *msgs)
 {
 	int result;
+	unsigned long time_left;
 	unsigned int temp = 0;
 	unsigned long orig_jiffies = jiffies;
 	struct imx_i2c_dma *dma = i2c_imx->dma;
@@ -624,10 +625,10 @@ static int i2c_imx_dma_write(struct imx_i2c_struct *i2c_imx,
 	 */
 	imx_i2c_write_reg(msgs->addr << 1, i2c_imx, IMX_I2C_I2DR);
 	reinit_completion(&i2c_imx->dma->cmd_complete);
-	result = wait_for_completion_timeout(
+	time_left = wait_for_completion_timeout(
 				&i2c_imx->dma->cmd_complete,
 				msecs_to_jiffies(DMA_TIMEOUT));
-	if (result == 0) {
+	if (time_left == 0) {
 		dmaengine_terminate_all(dma->chan_using);
 		return -ETIMEDOUT;
 	}
@@ -663,6 +664,7 @@ static int i2c_imx_dma_read(struct imx_i2c_struct *i2c_imx,
 			struct i2c_msg *msgs, bool is_lastmsg)
 {
 	int result;
+	unsigned long time_left;
 	unsigned int temp;
 	unsigned long orig_jiffies = jiffies;
 	struct imx_i2c_dma *dma = i2c_imx->dma;
@@ -682,10 +684,10 @@ static int i2c_imx_dma_read(struct imx_i2c_struct *i2c_imx,
 		return result;
 
 	reinit_completion(&i2c_imx->dma->cmd_complete);
-	result = wait_for_completion_timeout(
+	time_left = wait_for_completion_timeout(
 				&i2c_imx->dma->cmd_complete,
 				msecs_to_jiffies(DMA_TIMEOUT));
-	if (result == 0) {
+	if (time_left == 0) {
 		dmaengine_terminate_all(dma->chan_using);
 		return -ETIMEDOUT;
 	}
diff --git a/drivers/i2c/busses/i2c-ismt.c b/drivers/i2c/busses/i2c-ismt.c
index f2b0ff011631..f994712d0904 100644
--- a/drivers/i2c/busses/i2c-ismt.c
+++ b/drivers/i2c/busses/i2c-ismt.c
@@ -380,6 +380,7 @@ static int ismt_access(struct i2c_adapter *adap, u16 addr,
 		       int size, union i2c_smbus_data *data)
 {
 	int ret;
+	unsigned long time_left;
 	dma_addr_t dma_addr = 0; /* address of the data buffer */
 	u8 dma_size = 0;
 	enum dma_data_direction dma_direction = 0;
@@ -578,13 +579,13 @@ static int ismt_access(struct i2c_adapter *adap, u16 addr,
 	ismt_submit_desc(priv);
 
 	/* Now we wait for interrupt completion, 1s */
-	ret = wait_for_completion_timeout(&priv->cmp, HZ*1);
+	time_left = wait_for_completion_timeout(&priv->cmp, HZ*1);
 
 	/* unmap the data buffer */
 	if (dma_size != 0)
 		dma_unmap_single(&adap->dev, dma_addr, dma_size, dma_direction);
 
-	if (unlikely(!ret)) {
+	if (unlikely(!time_left)) {
 		dev_err(dev, "completion wait timed out\n");
 		ret = -ETIMEDOUT;
 		goto out;
diff --git a/drivers/i2c/busses/i2c-jz4780.c b/drivers/i2c/busses/i2c-jz4780.c
new file mode 100644
index 000000000000..ce1d69324169
--- /dev/null
+++ b/drivers/i2c/busses/i2c-jz4780.c
@@ -0,0 +1,832 @@
+/*
+ * Ingenic JZ4780 I2C bus driver
+ *
+ * Copyright (C) 2006 - 2009 Ingenic Semiconductor Inc.
+ * Copyright (C) 2015 Imagination Technologies
+ *
+ * This program is free software; 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/bitops.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+
+#define JZ4780_I2C_CTRL		0x00
+#define JZ4780_I2C_TAR		0x04
+#define JZ4780_I2C_SAR		0x08
+#define JZ4780_I2C_DC		0x10
+#define JZ4780_I2C_SHCNT	0x14
+#define JZ4780_I2C_SLCNT	0x18
+#define JZ4780_I2C_FHCNT	0x1C
+#define JZ4780_I2C_FLCNT	0x20
+#define JZ4780_I2C_INTST	0x2C
+#define JZ4780_I2C_INTM		0x30
+#define JZ4780_I2C_RXTL		0x38
+#define JZ4780_I2C_TXTL		0x3C
+#define JZ4780_I2C_CINTR	0x40
+#define JZ4780_I2C_CRXUF	0x44
+#define JZ4780_I2C_CRXOF	0x48
+#define JZ4780_I2C_CTXOF	0x4C
+#define JZ4780_I2C_CRXREQ	0x50
+#define JZ4780_I2C_CTXABRT	0x54
+#define JZ4780_I2C_CRXDONE	0x58
+#define JZ4780_I2C_CACT		0x5C
+#define JZ4780_I2C_CSTP		0x60
+#define JZ4780_I2C_CSTT		0x64
+#define JZ4780_I2C_CGC		0x68
+#define JZ4780_I2C_ENB		0x6C
+#define JZ4780_I2C_STA		0x70
+#define JZ4780_I2C_TXABRT	0x80
+#define JZ4780_I2C_DMACR	0x88
+#define JZ4780_I2C_DMATDLR	0x8C
+#define JZ4780_I2C_DMARDLR	0x90
+#define JZ4780_I2C_SDASU	0x94
+#define JZ4780_I2C_ACKGC	0x98
+#define JZ4780_I2C_ENSTA	0x9C
+#define JZ4780_I2C_SDAHD	0xD0
+
+#define JZ4780_I2C_CTRL_STPHLD		BIT(7)
+#define JZ4780_I2C_CTRL_SLVDIS		BIT(6)
+#define JZ4780_I2C_CTRL_REST		BIT(5)
+#define JZ4780_I2C_CTRL_MATP		BIT(4)
+#define JZ4780_I2C_CTRL_SATP		BIT(3)
+#define JZ4780_I2C_CTRL_SPDF		BIT(2)
+#define JZ4780_I2C_CTRL_SPDS		BIT(1)
+#define JZ4780_I2C_CTRL_MD		BIT(0)
+
+#define JZ4780_I2C_STA_SLVACT		BIT(6)
+#define JZ4780_I2C_STA_MSTACT		BIT(5)
+#define JZ4780_I2C_STA_RFF		BIT(4)
+#define JZ4780_I2C_STA_RFNE		BIT(3)
+#define JZ4780_I2C_STA_TFE		BIT(2)
+#define JZ4780_I2C_STA_TFNF		BIT(1)
+#define JZ4780_I2C_STA_ACT		BIT(0)
+
+static const char * const jz4780_i2c_abrt_src[] = {
+	"ABRT_7B_ADDR_NOACK",
+	"ABRT_10ADDR1_NOACK",
+	"ABRT_10ADDR2_NOACK",
+	"ABRT_XDATA_NOACK",
+	"ABRT_GCALL_NOACK",
+	"ABRT_GCALL_READ",
+	"ABRT_HS_ACKD",
+	"SBYTE_ACKDET",
+	"ABRT_HS_NORSTRT",
+	"SBYTE_NORSTRT",
+	"ABRT_10B_RD_NORSTRT",
+	"ABRT_MASTER_DIS",
+	"ARB_LOST",
+	"SLVFLUSH_TXFIFO",
+	"SLV_ARBLOST",
+	"SLVRD_INTX",
+};
+
+#define JZ4780_I2C_INTST_IGC		BIT(11)
+#define JZ4780_I2C_INTST_ISTT		BIT(10)
+#define JZ4780_I2C_INTST_ISTP		BIT(9)
+#define JZ4780_I2C_INTST_IACT		BIT(8)
+#define JZ4780_I2C_INTST_RXDN		BIT(7)
+#define JZ4780_I2C_INTST_TXABT		BIT(6)
+#define JZ4780_I2C_INTST_RDREQ		BIT(5)
+#define JZ4780_I2C_INTST_TXEMP		BIT(4)
+#define JZ4780_I2C_INTST_TXOF		BIT(3)
+#define JZ4780_I2C_INTST_RXFL		BIT(2)
+#define JZ4780_I2C_INTST_RXOF		BIT(1)
+#define JZ4780_I2C_INTST_RXUF		BIT(0)
+
+#define JZ4780_I2C_INTM_MIGC		BIT(11)
+#define JZ4780_I2C_INTM_MISTT		BIT(10)
+#define JZ4780_I2C_INTM_MISTP		BIT(9)
+#define JZ4780_I2C_INTM_MIACT		BIT(8)
+#define JZ4780_I2C_INTM_MRXDN		BIT(7)
+#define JZ4780_I2C_INTM_MTXABT		BIT(6)
+#define JZ4780_I2C_INTM_MRDREQ		BIT(5)
+#define JZ4780_I2C_INTM_MTXEMP		BIT(4)
+#define JZ4780_I2C_INTM_MTXOF		BIT(3)
+#define JZ4780_I2C_INTM_MRXFL		BIT(2)
+#define JZ4780_I2C_INTM_MRXOF		BIT(1)
+#define JZ4780_I2C_INTM_MRXUF		BIT(0)
+
+#define JZ4780_I2C_DC_READ		BIT(8)
+
+#define JZ4780_I2C_SDAHD_HDENB		BIT(8)
+
+#define JZ4780_I2C_ENB_I2C		BIT(0)
+
+#define JZ4780_I2CSHCNT_ADJUST(n)	(((n) - 8) < 6 ? 6 : ((n) - 8))
+#define JZ4780_I2CSLCNT_ADJUST(n)	(((n) - 1) < 8 ? 8 : ((n) - 1))
+#define JZ4780_I2CFHCNT_ADJUST(n)	(((n) - 8) < 6 ? 6 : ((n) - 8))
+#define JZ4780_I2CFLCNT_ADJUST(n)	(((n) - 1) < 8 ? 8 : ((n) - 1))
+
+#define JZ4780_I2C_FIFO_LEN	16
+#define TX_LEVEL		3
+#define RX_LEVEL		(JZ4780_I2C_FIFO_LEN - TX_LEVEL - 1)
+
+#define JZ4780_I2C_TIMEOUT	300
+
+#define BUFSIZE 200
+
+struct jz4780_i2c {
+	void __iomem		*iomem;
+	int			 irq;
+	struct clk		*clk;
+	struct i2c_adapter	 adap;
+
+	/* lock to protect rbuf and wbuf between xfer_rd/wr and irq handler */
+	spinlock_t		lock;
+
+	/* beginning of lock scope */
+	unsigned char		*rbuf;
+	int			rd_total_len;
+	int			rd_data_xfered;
+	int			rd_cmd_xfered;
+
+	unsigned char		*wbuf;
+	int			wt_len;
+
+	int			is_write;
+	int			stop_hold;
+	int			speed;
+
+	int			data_buf[BUFSIZE];
+	int			cmd_buf[BUFSIZE];
+	int			cmd;
+
+	/* end of lock scope */
+	struct completion	trans_waitq;
+};
+
+static inline unsigned short jz4780_i2c_readw(struct jz4780_i2c *i2c,
+					      unsigned long offset)
+{
+	return readw(i2c->iomem + offset);
+}
+
+static inline void jz4780_i2c_writew(struct jz4780_i2c *i2c,
+				     unsigned long offset, unsigned short val)
+{
+	writew(val, i2c->iomem + offset);
+}
+
+static int jz4780_i2c_disable(struct jz4780_i2c *i2c)
+{
+	unsigned short regval;
+	unsigned long loops = 5;
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_ENB, 0);
+
+	do {
+		regval = jz4780_i2c_readw(i2c, JZ4780_I2C_ENSTA);
+		if (!(regval & JZ4780_I2C_ENB_I2C))
+			return 0;
+
+		usleep_range(5000, 15000);
+	} while (--loops);
+
+	dev_err(&i2c->adap.dev, "disable failed: ENSTA=0x%04x\n", regval);
+	return -ETIMEDOUT;
+}
+
+static int jz4780_i2c_enable(struct jz4780_i2c *i2c)
+{
+	unsigned short regval;
+	unsigned long loops = 5;
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_ENB, 1);
+
+	do {
+		regval = jz4780_i2c_readw(i2c, JZ4780_I2C_ENSTA);
+		if (regval & JZ4780_I2C_ENB_I2C)
+			return 0;
+
+		usleep_range(5000, 15000);
+	} while (--loops);
+
+	dev_err(&i2c->adap.dev, "enable failed: ENSTA=0x%04x\n", regval);
+	return -ETIMEDOUT;
+}
+
+static int jz4780_i2c_set_target(struct jz4780_i2c *i2c, unsigned char address)
+{
+	unsigned short regval;
+	unsigned long loops = 5;
+
+	do {
+		regval = jz4780_i2c_readw(i2c, JZ4780_I2C_STA);
+		if ((regval & JZ4780_I2C_STA_TFE) &&
+		    !(regval & JZ4780_I2C_STA_MSTACT))
+			break;
+
+		usleep_range(5000, 15000);
+	} while (--loops);
+
+	if (loops) {
+		jz4780_i2c_writew(i2c, JZ4780_I2C_TAR, address);
+		return 0;
+	}
+
+	dev_err(&i2c->adap.dev,
+		"set device to address 0x%02x failed, STA=0x%04x\n",
+		address, regval);
+
+	return -ENXIO;
+}
+
+static int jz4780_i2c_set_speed(struct jz4780_i2c *i2c)
+{
+	int dev_clk_khz = clk_get_rate(i2c->clk) / 1000;
+	int cnt_high = 0;	/* HIGH period count of the SCL clock */
+	int cnt_low = 0;	/* LOW period count of the SCL clock */
+	int cnt_period = 0;	/* period count of the SCL clock */
+	int setup_time = 0;
+	int hold_time = 0;
+	unsigned short tmp = 0;
+	int i2c_clk = i2c->speed;
+
+	if (jz4780_i2c_disable(i2c))
+		dev_dbg(&i2c->adap.dev, "i2c not disabled\n");
+
+	/*
+	 * 1 JZ4780_I2C cycle equals to cnt_period PCLK(i2c_clk)
+	 * standard mode, min LOW and HIGH period are 4700 ns and 4000 ns
+	 * fast mode, min LOW and HIGH period are 1300 ns and 600 ns
+	 */
+	cnt_period = dev_clk_khz / i2c_clk;
+
+	if (i2c_clk <= 100)
+		cnt_high = (cnt_period * 4000) / (4700 + 4000);
+	else
+		cnt_high = (cnt_period * 600) / (1300 + 600);
+
+	cnt_low = cnt_period - cnt_high;
+
+	/*
+	 * NOTE: JZ4780_I2C_CTRL_REST can't set when i2c enabled, because
+	 * normal read are 2 messages, we cannot disable i2c controller
+	 * between these two messages, this means that we must always set
+	 * JZ4780_I2C_CTRL_REST when init JZ4780_I2C_CTRL
+	 *
+	 */
+	if (i2c_clk <= 100) {
+		tmp = JZ4780_I2C_CTRL_SPDS | JZ4780_I2C_CTRL_REST
+		      | JZ4780_I2C_CTRL_SLVDIS | JZ4780_I2C_CTRL_MD;
+		jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+
+		jz4780_i2c_writew(i2c, JZ4780_I2C_SHCNT,
+				  JZ4780_I2CSHCNT_ADJUST(cnt_high));
+		jz4780_i2c_writew(i2c, JZ4780_I2C_SLCNT,
+				  JZ4780_I2CSLCNT_ADJUST(cnt_low));
+	} else {
+		tmp = JZ4780_I2C_CTRL_SPDF | JZ4780_I2C_CTRL_REST
+		      | JZ4780_I2C_CTRL_SLVDIS | JZ4780_I2C_CTRL_MD;
+		jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+
+		jz4780_i2c_writew(i2c, JZ4780_I2C_FHCNT,
+				  JZ4780_I2CFHCNT_ADJUST(cnt_high));
+		jz4780_i2c_writew(i2c, JZ4780_I2C_FLCNT,
+				  JZ4780_I2CFLCNT_ADJUST(cnt_low));
+	}
+
+	/*
+	 * a i2c device must internally provide a hold time at least 300ns
+	 * tHD:DAT
+	 *	Standard Mode: min=300ns, max=3450ns
+	 *	Fast Mode: min=0ns, max=900ns
+	 * tSU:DAT
+	 *	Standard Mode: min=250ns, max=infinite
+	 *	Fast Mode: min=100(250ns is recommended), max=infinite
+	 *
+	 * 1i2c_clk = 10^6 / dev_clk_khz
+	 * on FPGA, dev_clk_khz = 12000, so 1i2c_clk = 1000/12 = 83ns
+	 * on Pisces(1008M), dev_clk_khz=126000, so 1i2c_clk = 1000 / 126 = 8ns
+	 *
+	 * The actual hold time is (SDAHD + 1) * (i2c_clk period).
+	 *
+	 * Length of setup time calculated using (SDASU - 1) * (ic_clk_period)
+	 *
+	 */
+	if (i2c_clk <= 100) { /* standard mode */
+		setup_time = 300;
+		hold_time = 400;
+	} else {
+		setup_time = 450;
+		hold_time = 450;
+	}
+
+	hold_time = ((hold_time * dev_clk_khz) / 1000000) - 1;
+	setup_time = ((setup_time * dev_clk_khz) / 1000000)  + 1;
+
+	if (setup_time > 255)
+		setup_time = 255;
+
+	if (setup_time <= 0)
+		setup_time = 1;
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_SDASU, setup_time);
+
+	if (hold_time > 255)
+		hold_time = 255;
+
+	if (hold_time >= 0) {
+		/*i2c hold time enable */
+		hold_time |= JZ4780_I2C_SDAHD_HDENB;
+		jz4780_i2c_writew(i2c, JZ4780_I2C_SDAHD, hold_time);
+	} else {
+		/* disable hold time */
+		jz4780_i2c_writew(i2c, JZ4780_I2C_SDAHD, 0);
+	}
+
+	return 0;
+}
+
+static int jz4780_i2c_cleanup(struct jz4780_i2c *i2c)
+{
+	int ret;
+	unsigned long flags;
+	unsigned short tmp;
+
+	spin_lock_irqsave(&i2c->lock, flags);
+
+	/* can send stop now if need */
+	tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_CTRL);
+	tmp &= ~JZ4780_I2C_CTRL_STPHLD;
+	jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+
+	/* disable all interrupts first */
+	jz4780_i2c_writew(i2c, JZ4780_I2C_INTM, 0);
+
+	/* then clear all interrupts */
+	jz4780_i2c_readw(i2c, JZ4780_I2C_CTXABRT);
+	jz4780_i2c_readw(i2c, JZ4780_I2C_CINTR);
+
+	/* then disable the controller */
+	tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_CTRL);
+	tmp &= ~JZ4780_I2C_ENB_I2C;
+	jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+	udelay(10);
+	tmp |= JZ4780_I2C_ENB_I2C;
+	jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+
+	spin_unlock_irqrestore(&i2c->lock, flags);
+
+	ret = jz4780_i2c_disable(i2c);
+	if (ret)
+		dev_err(&i2c->adap.dev,
+			"unable to disable device during cleanup!\n");
+
+	if (unlikely(jz4780_i2c_readw(i2c, JZ4780_I2C_INTM)
+		     & jz4780_i2c_readw(i2c, JZ4780_I2C_INTST)))
+		dev_err(&i2c->adap.dev,
+			"device has interrupts after a complete cleanup!\n");
+
+	return ret;
+}
+
+static int jz4780_i2c_prepare(struct jz4780_i2c *i2c)
+{
+	jz4780_i2c_set_speed(i2c);
+	return jz4780_i2c_enable(i2c);
+}
+
+static void jz4780_i2c_send_rcmd(struct jz4780_i2c *i2c, int cmd_count)
+{
+	int i;
+
+	for (i = 0; i < cmd_count; i++)
+		jz4780_i2c_writew(i2c, JZ4780_I2C_DC, JZ4780_I2C_DC_READ);
+}
+
+static void jz4780_i2c_trans_done(struct jz4780_i2c *i2c)
+{
+	jz4780_i2c_writew(i2c, JZ4780_I2C_INTM, 0);
+	complete(&i2c->trans_waitq);
+}
+
+static irqreturn_t jz4780_i2c_irq(int irqno, void *dev_id)
+{
+	unsigned short tmp;
+	unsigned short intst;
+	unsigned short intmsk;
+	struct jz4780_i2c *i2c = dev_id;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2c->lock, flags);
+	intmsk = jz4780_i2c_readw(i2c, JZ4780_I2C_INTM);
+	intst = jz4780_i2c_readw(i2c, JZ4780_I2C_INTST);
+
+	intst &= intmsk;
+
+	if (intst & JZ4780_I2C_INTST_TXABT) {
+		jz4780_i2c_trans_done(i2c);
+		goto done;
+	}
+
+	if (intst & JZ4780_I2C_INTST_RXOF) {
+		dev_dbg(&i2c->adap.dev, "received fifo overflow!\n");
+		jz4780_i2c_trans_done(i2c);
+		goto done;
+	}
+
+	/*
+	 * When reading, always drain RX FIFO before we send more Read
+	 * Commands to avoid fifo overrun
+	 */
+	if (i2c->is_write == 0) {
+		int rd_left;
+
+		while ((jz4780_i2c_readw(i2c, JZ4780_I2C_STA)
+				  & JZ4780_I2C_STA_RFNE)) {
+			*(i2c->rbuf++) = jz4780_i2c_readw(i2c, JZ4780_I2C_DC)
+					 & 0xff;
+			i2c->rd_data_xfered++;
+			if (i2c->rd_data_xfered == i2c->rd_total_len) {
+				jz4780_i2c_trans_done(i2c);
+				goto done;
+			}
+		}
+
+		rd_left = i2c->rd_total_len - i2c->rd_data_xfered;
+
+		if (rd_left <= JZ4780_I2C_FIFO_LEN)
+			jz4780_i2c_writew(i2c, JZ4780_I2C_RXTL, rd_left - 1);
+	}
+
+	if (intst & JZ4780_I2C_INTST_TXEMP) {
+		if (i2c->is_write == 0) {
+			int cmd_left = i2c->rd_total_len - i2c->rd_cmd_xfered;
+			int max_send = (JZ4780_I2C_FIFO_LEN - 1)
+					 - (i2c->rd_cmd_xfered
+					 - i2c->rd_data_xfered);
+			int cmd_to_send = min(cmd_left, max_send);
+
+			if (i2c->rd_cmd_xfered != 0)
+				cmd_to_send = min(cmd_to_send,
+						  JZ4780_I2C_FIFO_LEN
+						  - TX_LEVEL - 1);
+
+			if (cmd_to_send) {
+				jz4780_i2c_send_rcmd(i2c, cmd_to_send);
+				i2c->rd_cmd_xfered += cmd_to_send;
+			}
+
+			cmd_left = i2c->rd_total_len - i2c->rd_cmd_xfered;
+			if (cmd_left == 0) {
+				intmsk = jz4780_i2c_readw(i2c, JZ4780_I2C_INTM);
+				intmsk &= ~JZ4780_I2C_INTM_MTXEMP;
+				jz4780_i2c_writew(i2c, JZ4780_I2C_INTM, intmsk);
+
+				tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_CTRL);
+				tmp &= ~JZ4780_I2C_CTRL_STPHLD;
+				jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+			}
+		} else {
+			unsigned short data;
+			unsigned short i2c_sta;
+
+			i2c_sta = jz4780_i2c_readw(i2c, JZ4780_I2C_STA);
+
+			while ((i2c_sta & JZ4780_I2C_STA_TFNF) &&
+			       (i2c->wt_len > 0)) {
+				i2c_sta = jz4780_i2c_readw(i2c, JZ4780_I2C_STA);
+				data = *i2c->wbuf;
+				data &= ~JZ4780_I2C_DC_READ;
+				jz4780_i2c_writew(i2c, JZ4780_I2C_DC,
+						  data);
+				i2c->wbuf++;
+				i2c->wt_len--;
+			}
+
+			if (i2c->wt_len == 0) {
+				if (!i2c->stop_hold) {
+					tmp = jz4780_i2c_readw(i2c,
+							       JZ4780_I2C_CTRL);
+					tmp &= ~JZ4780_I2C_CTRL_STPHLD;
+					jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL,
+							  tmp);
+				}
+
+				jz4780_i2c_trans_done(i2c);
+				goto done;
+			}
+		}
+	}
+
+done:
+	spin_unlock_irqrestore(&i2c->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static void jz4780_i2c_txabrt(struct jz4780_i2c *i2c, int src)
+{
+	int i;
+
+	dev_err(&i2c->adap.dev, "txabrt: 0x%08x\n", src);
+	dev_err(&i2c->adap.dev, "device addr=%x\n",
+		jz4780_i2c_readw(i2c, JZ4780_I2C_TAR));
+	dev_err(&i2c->adap.dev, "send cmd count:%d  %d\n",
+		i2c->cmd, i2c->cmd_buf[i2c->cmd]);
+	dev_err(&i2c->adap.dev, "receive data count:%d  %d\n",
+		i2c->cmd, i2c->data_buf[i2c->cmd]);
+
+	for (i = 0; i < 16; i++) {
+		if (src & BIT(i))
+			dev_dbg(&i2c->adap.dev, "I2C TXABRT[%d]=%s\n",
+				i, jz4780_i2c_abrt_src[i]);
+	}
+}
+
+static inline int jz4780_i2c_xfer_read(struct jz4780_i2c *i2c,
+				       unsigned char *buf, int len, int cnt,
+				       int idx)
+{
+	int ret = 0;
+	long timeout;
+	int wait_time = JZ4780_I2C_TIMEOUT * (len + 5);
+	unsigned short tmp;
+	unsigned long flags;
+
+	memset(buf, 0, len);
+
+	spin_lock_irqsave(&i2c->lock, flags);
+
+	i2c->stop_hold = 0;
+	i2c->is_write = 0;
+	i2c->rbuf = buf;
+	i2c->rd_total_len = len;
+	i2c->rd_data_xfered = 0;
+	i2c->rd_cmd_xfered = 0;
+
+	if (len <= JZ4780_I2C_FIFO_LEN)
+		jz4780_i2c_writew(i2c, JZ4780_I2C_RXTL, len - 1);
+	else
+		jz4780_i2c_writew(i2c, JZ4780_I2C_RXTL, RX_LEVEL);
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_TXTL, TX_LEVEL);
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_INTM,
+			  JZ4780_I2C_INTM_MRXFL | JZ4780_I2C_INTM_MTXEMP
+			  | JZ4780_I2C_INTM_MTXABT | JZ4780_I2C_INTM_MRXOF);
+
+	tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_CTRL);
+	tmp |= JZ4780_I2C_CTRL_STPHLD;
+	jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+
+	spin_unlock_irqrestore(&i2c->lock, flags);
+
+	timeout = wait_for_completion_timeout(&i2c->trans_waitq,
+					      msecs_to_jiffies(wait_time));
+
+	if (!timeout) {
+		dev_err(&i2c->adap.dev, "irq read timeout\n");
+		dev_dbg(&i2c->adap.dev, "send cmd count:%d  %d\n",
+			i2c->cmd, i2c->cmd_buf[i2c->cmd]);
+		dev_dbg(&i2c->adap.dev, "receive data count:%d  %d\n",
+			i2c->cmd, i2c->data_buf[i2c->cmd]);
+		ret = -EIO;
+	}
+
+	tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_TXABRT);
+	if (tmp) {
+		jz4780_i2c_txabrt(i2c, tmp);
+		ret = -EIO;
+	}
+
+	return ret;
+}
+
+static inline int jz4780_i2c_xfer_write(struct jz4780_i2c *i2c,
+					unsigned char *buf, int len,
+					int cnt, int idx)
+{
+	int ret = 0;
+	int wait_time = JZ4780_I2C_TIMEOUT * (len + 5);
+	long timeout;
+	unsigned short tmp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2c->lock, flags);
+
+	if (idx < (cnt - 1))
+		i2c->stop_hold = 1;
+	else
+		i2c->stop_hold = 0;
+
+	i2c->is_write = 1;
+	i2c->wbuf = buf;
+	i2c->wt_len = len;
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_TXTL, TX_LEVEL);
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_INTM, JZ4780_I2C_INTM_MTXEMP
+					| JZ4780_I2C_INTM_MTXABT);
+
+	tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_CTRL);
+	tmp |= JZ4780_I2C_CTRL_STPHLD;
+	jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+
+	spin_unlock_irqrestore(&i2c->lock, flags);
+
+	timeout = wait_for_completion_timeout(&i2c->trans_waitq,
+					      msecs_to_jiffies(wait_time));
+	if (timeout && !i2c->stop_hold) {
+		unsigned short i2c_sta;
+		int write_in_process;
+
+		timeout = JZ4780_I2C_TIMEOUT * 100;
+		for (; timeout > 0; timeout--) {
+			i2c_sta = jz4780_i2c_readw(i2c, JZ4780_I2C_STA);
+
+			write_in_process = (i2c_sta & JZ4780_I2C_STA_MSTACT) ||
+				!(i2c_sta & JZ4780_I2C_STA_TFE);
+			if (!write_in_process)
+				break;
+			udelay(10);
+		}
+	}
+
+	if (!timeout) {
+		dev_err(&i2c->adap.dev, "write wait timeout\n");
+		ret = -EIO;
+	}
+
+	tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_TXABRT);
+	if (tmp) {
+		jz4780_i2c_txabrt(i2c, tmp);
+		ret = -EIO;
+	}
+
+	return ret;
+}
+
+static int jz4780_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msg,
+			   int count)
+{
+	int i = -EIO;
+	int ret = 0;
+	struct jz4780_i2c *i2c = adap->algo_data;
+
+	ret = jz4780_i2c_prepare(i2c);
+	if (ret) {
+		dev_err(&i2c->adap.dev, "I2C prepare failed\n");
+		goto out;
+	}
+
+	if (msg->addr != jz4780_i2c_readw(i2c, JZ4780_I2C_TAR)) {
+		ret = jz4780_i2c_set_target(i2c, msg->addr);
+		if (ret)
+			goto out;
+	}
+	for (i = 0; i < count; i++, msg++) {
+		if (msg->flags & I2C_M_RD)
+			ret = jz4780_i2c_xfer_read(i2c, msg->buf, msg->len,
+						   count, i);
+		else
+			ret = jz4780_i2c_xfer_write(i2c, msg->buf, msg->len,
+						    count, i);
+
+		if (ret)
+			goto out;
+	}
+
+	ret = i;
+
+out:
+	jz4780_i2c_cleanup(i2c);
+	return ret;
+}
+
+static u32 jz4780_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm jz4780_i2c_algorithm = {
+	.master_xfer	= jz4780_i2c_xfer,
+	.functionality	= jz4780_i2c_functionality,
+};
+
+static const struct of_device_id jz4780_i2c_of_matches[] = {
+	{ .compatible = "ingenic,jz4780-i2c", },
+	{ /* sentinel */ }
+};
+
+static int jz4780_i2c_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	unsigned int clk_freq = 0;
+	unsigned short tmp;
+	struct resource *r;
+	struct jz4780_i2c *i2c;
+
+	i2c = devm_kzalloc(&pdev->dev, sizeof(struct jz4780_i2c), GFP_KERNEL);
+	if (!i2c)
+		return -ENOMEM;
+
+	i2c->adap.owner		= THIS_MODULE;
+	i2c->adap.algo		= &jz4780_i2c_algorithm;
+	i2c->adap.algo_data	= i2c;
+	i2c->adap.retries	= 5;
+	i2c->adap.dev.parent	= &pdev->dev;
+	i2c->adap.dev.of_node	= pdev->dev.of_node;
+	sprintf(i2c->adap.name, "%s", pdev->name);
+
+	init_completion(&i2c->trans_waitq);
+	spin_lock_init(&i2c->lock);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	i2c->iomem = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(i2c->iomem))
+		return PTR_ERR(i2c->iomem);
+
+	platform_set_drvdata(pdev, i2c);
+
+	i2c->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(i2c->clk))
+		return PTR_ERR(i2c->clk);
+
+	clk_prepare_enable(i2c->clk);
+
+	if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				 &clk_freq)) {
+		dev_err(&pdev->dev, "clock-frequency not specified in DT");
+		return clk_freq;
+	}
+
+	i2c->speed = clk_freq / 1000;
+	jz4780_i2c_set_speed(i2c);
+
+	dev_info(&pdev->dev, "Bus frequency is %d KHz\n", i2c->speed);
+
+	tmp = jz4780_i2c_readw(i2c, JZ4780_I2C_CTRL);
+	tmp &= ~JZ4780_I2C_CTRL_STPHLD;
+	jz4780_i2c_writew(i2c, JZ4780_I2C_CTRL, tmp);
+
+	jz4780_i2c_writew(i2c, JZ4780_I2C_INTM, 0x0);
+
+	i2c->cmd = 0;
+	memset(i2c->cmd_buf, 0, BUFSIZE);
+	memset(i2c->data_buf, 0, BUFSIZE);
+
+	i2c->irq = platform_get_irq(pdev, 0);
+	ret = devm_request_irq(&pdev->dev, i2c->irq, jz4780_i2c_irq, 0,
+			       dev_name(&pdev->dev), i2c);
+	if (ret) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	ret = i2c_add_adapter(&i2c->adap);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to add bus\n");
+		goto err;
+	}
+
+	return 0;
+
+err:
+	clk_disable_unprepare(i2c->clk);
+	return ret;
+}
+
+static int jz4780_i2c_remove(struct platform_device *pdev)
+{
+	struct jz4780_i2c *i2c = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(i2c->clk);
+	i2c_del_adapter(&i2c->adap);
+	return 0;
+}
+
+static struct platform_driver jz4780_i2c_driver = {
+	.probe		= jz4780_i2c_probe,
+	.remove		= jz4780_i2c_remove,
+	.driver		= {
+		.name	= "jz4780-i2c",
+		.of_match_table = of_match_ptr(jz4780_i2c_of_matches),
+	},
+};
+
+module_platform_driver(jz4780_i2c_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("ztyan<ztyan@ingenic.cn>");
+MODULE_DESCRIPTION("i2c driver for JZ4780 SoCs");
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index c74cc2be613b..48ecffecc0ed 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -29,6 +29,7 @@
 #include <linux/delay.h>
 
 #include <asm/mpc52xx.h>
+#include <asm/mpc85xx.h>
 #include <sysdev/fsl_soc.h>
 
 #define DRV_NAME "mpc-i2c"
@@ -95,8 +96,9 @@ static irqreturn_t mpc_i2c_isr(int irq, void *dev_id)
 		i2c->interrupt = readb(i2c->base + MPC_I2C_SR);
 		writeb(0, i2c->base + MPC_I2C_SR);
 		wake_up(&i2c->queue);
+		return IRQ_HANDLED;
 	}
-	return IRQ_HANDLED;
+	return IRQ_NONE;
 }
 
 /* Sometimes 9th clock pulse isn't generated, and slave doesn't release
@@ -346,6 +348,33 @@ static u32 mpc_i2c_get_sec_cfg_8xxx(void)
 	return val;
 }
 
+static u32 mpc_i2c_get_prescaler_8xxx(void)
+{
+	/* mpc83xx and mpc82xx all have prescaler 1 */
+	u32 prescaler = 1;
+
+	/* mpc85xx */
+	if (pvr_version_is(PVR_VER_E500V1) || pvr_version_is(PVR_VER_E500V2)
+		|| pvr_version_is(PVR_VER_E500MC)
+		|| pvr_version_is(PVR_VER_E5500)
+		|| pvr_version_is(PVR_VER_E6500)) {
+		unsigned int svr = mfspr(SPRN_SVR);
+
+		if ((SVR_SOC_VER(svr) == SVR_8540)
+			|| (SVR_SOC_VER(svr) == SVR_8541)
+			|| (SVR_SOC_VER(svr) == SVR_8560)
+			|| (SVR_SOC_VER(svr) == SVR_8555)
+			|| (SVR_SOC_VER(svr) == SVR_8610))
+			/* the above 85xx SoCs have prescaler 1 */
+			prescaler = 1;
+		else
+			/* all the other 85xx have prescaler 2 */
+			prescaler = 2;
+	}
+
+	return prescaler;
+}
+
 static int mpc_i2c_get_fdr_8xxx(struct device_node *node, u32 clock,
 					  u32 prescaler, u32 *real_clk)
 {
@@ -363,7 +392,7 @@ static int mpc_i2c_get_fdr_8xxx(struct device_node *node, u32 clock,
 	if (of_device_is_compatible(node, "fsl,mpc8544-i2c"))
 		prescaler = mpc_i2c_get_sec_cfg_8xxx() ? 3 : 2;
 	if (!prescaler)
-		prescaler = 1;
+		prescaler = mpc_i2c_get_prescaler_8xxx();
 
 	divider = fsl_get_sys_freq() / clock / prescaler;
 
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index ff8b12c8d25f..56fceff6ba14 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -568,6 +568,7 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 	int ret;
 	int flags;
 	int use_pio = 0;
+	unsigned long time_left;
 
 	flags = stop ? MXS_I2C_CTRL0_POST_SEND_STOP : 0;
 
@@ -599,9 +600,9 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 		if (ret)
 			return ret;
 
-		ret = wait_for_completion_timeout(&i2c->cmd_complete,
+		time_left = wait_for_completion_timeout(&i2c->cmd_complete,
 						msecs_to_jiffies(1000));
-		if (ret == 0)
+		if (!time_left)
 			goto timeout;
 
 		ret = i2c->cmd_err;
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 97998946c4f6..bcd17e8cbcb4 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -446,9 +446,9 @@ static void setup_i2c_controller(struct nmk_i2c_dev *dev)
  */
 static int read_i2c(struct nmk_i2c_dev *dev, u16 flags)
 {
-	u32 status = 0;
+	int status = 0;
 	u32 mcr, irq_mask;
-	int timeout;
+	unsigned long timeout;
 
 	mcr = load_i2c_mcr_reg(dev, flags);
 	writel(mcr, dev->virtbase + I2C_MCR);
@@ -517,7 +517,7 @@ static int write_i2c(struct nmk_i2c_dev *dev, u16 flags)
 {
 	u32 status = 0;
 	u32 mcr, irq_mask;
-	int timeout;
+	unsigned long timeout;
 
 	mcr = load_i2c_mcr_reg(dev, flags);
 
diff --git a/drivers/i2c/busses/i2c-opal.c b/drivers/i2c/busses/i2c-opal.c
index 16f90b1a7508..75dd6d041241 100644
--- a/drivers/i2c/busses/i2c-opal.c
+++ b/drivers/i2c/busses/i2c-opal.c
@@ -104,18 +104,8 @@ static int i2c_opal_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 		req.buffer_ra = cpu_to_be64(__pa(msgs[0].buf));
 		break;
 	case 2:
-		/* For two messages, we basically support only simple
-		 * smbus transactions of a write plus a read. We might
-		 * want to allow also two writes but we'd have to bounce
-		 * the data into a single buffer.
-		 */
-		if ((msgs[0].flags & I2C_M_RD) || !(msgs[1].flags & I2C_M_RD))
-			return -EOPNOTSUPP;
-		if (msgs[0].len > 4)
-			return -EOPNOTSUPP;
-		if (msgs[0].addr != msgs[1].addr)
-			return -EOPNOTSUPP;
-		req.type = OPAL_I2C_SM_READ;
+		req.type = (msgs[1].flags & I2C_M_RD) ?
+			OPAL_I2C_SM_READ : OPAL_I2C_SM_WRITE;
 		req.addr = cpu_to_be16(msgs[0].addr);
 		req.subaddr_sz = msgs[0].len;
 		for (i = 0; i < msgs[0].len; i++)
@@ -210,6 +200,15 @@ static const struct i2c_algorithm i2c_opal_algo = {
 	.functionality	= i2c_opal_func,
 };
 
+/*
+ * For two messages, we basically support simple smbus transactions of a
+ * write-then-anything.
+ */
+static struct i2c_adapter_quirks i2c_opal_quirks = {
+	.flags = I2C_AQ_COMB | I2C_AQ_COMB_WRITE_FIRST | I2C_AQ_COMB_SAME_ADDR,
+	.max_comb_1st_msg_len = 4,
+};
+
 static int i2c_opal_probe(struct platform_device *pdev)
 {
 	struct i2c_adapter	*adapter;
@@ -232,6 +231,7 @@ static int i2c_opal_probe(struct platform_device *pdev)
 
 	adapter->algo = &i2c_opal_algo;
 	adapter->algo_data = (void *)(unsigned long)opal_id;
+	adapter->quirks = &i2c_opal_quirks;
 	adapter->dev.parent = &pdev->dev;
 	adapter->dev.of_node = of_node_get(pdev->dev.of_node);
 	pname = of_get_property(pdev->dev.of_node, "ibm,port-name", NULL);
diff --git a/drivers/i2c/busses/i2c-pmcmsp.c b/drivers/i2c/busses/i2c-pmcmsp.c
index d37d9db6681e..2c40edbf6224 100644
--- a/drivers/i2c/busses/i2c-pmcmsp.c
+++ b/drivers/i2c/busses/i2c-pmcmsp.c
@@ -456,14 +456,6 @@ static enum pmcmsptwi_xfer_result pmcmsptwi_xfer_cmd(
 		return -EINVAL;
 	}
 
-	if (cmd->read_len > MSP_MAX_BYTES_PER_RW ||
-	    cmd->write_len > MSP_MAX_BYTES_PER_RW) {
-		dev_err(&pmcmsptwi_adapter.dev,
-			"%s: Cannot transfer more than %d bytes\n",
-			__func__, MSP_MAX_BYTES_PER_RW);
-		return -EINVAL;
-	}
-
 	mutex_lock(&data->lock);
 	dev_dbg(&pmcmsptwi_adapter.dev,
 		"Setting address to 0x%04x\n", cmd->addr);
@@ -520,25 +512,14 @@ static int pmcmsptwi_master_xfer(struct i2c_adapter *adap,
 	struct pmcmsptwi_cfg oldcfg, newcfg;
 	int ret;
 
-	if (num > 2) {
-		dev_dbg(&adap->dev, "%d messages unsupported\n", num);
-		return -EINVAL;
-	} else if (num == 2) {
-		/* Check for a dual write-then-read command */
+	if (num == 2) {
 		struct i2c_msg *nextmsg = msg + 1;
-		if (!(msg->flags & I2C_M_RD) &&
-		    (nextmsg->flags & I2C_M_RD) &&
-		    msg->addr == nextmsg->addr) {
-			cmd.type = MSP_TWI_CMD_WRITE_READ;
-			cmd.write_len = msg->len;
-			cmd.write_data = msg->buf;
-			cmd.read_len = nextmsg->len;
-			cmd.read_data = nextmsg->buf;
-		} else {
-			dev_dbg(&adap->dev,
-				"Non write-read dual messages unsupported\n");
-			return -EINVAL;
-		}
+
+		cmd.type = MSP_TWI_CMD_WRITE_READ;
+		cmd.write_len = msg->len;
+		cmd.write_data = msg->buf;
+		cmd.read_len = nextmsg->len;
+		cmd.read_data = nextmsg->buf;
 	} else if (msg->flags & I2C_M_RD) {
 		cmd.type = MSP_TWI_CMD_READ;
 		cmd.read_len = msg->len;
@@ -598,6 +579,14 @@ static u32 pmcmsptwi_i2c_func(struct i2c_adapter *adapter)
 		I2C_FUNC_SMBUS_WORD_DATA | I2C_FUNC_SMBUS_PROC_CALL;
 }
 
+static struct i2c_adapter_quirks pmcmsptwi_i2c_quirks = {
+	.flags = I2C_AQ_COMB_WRITE_THEN_READ,
+	.max_write_len = MSP_MAX_BYTES_PER_RW,
+	.max_read_len = MSP_MAX_BYTES_PER_RW,
+	.max_comb_1st_msg_len = MSP_MAX_BYTES_PER_RW,
+	.max_comb_2nd_msg_len = MSP_MAX_BYTES_PER_RW,
+};
+
 /* -- Initialization -- */
 
 static struct i2c_algorithm pmcmsptwi_algo = {
@@ -609,6 +598,7 @@ static struct i2c_adapter pmcmsptwi_adapter = {
 	.owner		= THIS_MODULE,
 	.class		= I2C_CLASS_HWMON | I2C_CLASS_SPD,
 	.algo		= &pmcmsptwi_algo,
+	.quirks		= &pmcmsptwi_i2c_quirks,
 	.name		= DRV_NAME,
 };
 
diff --git a/drivers/i2c/busses/i2c-powermac.c b/drivers/i2c/busses/i2c-powermac.c
index 60a53c169ed2..6abcf696e359 100644
--- a/drivers/i2c/busses/i2c-powermac.c
+++ b/drivers/i2c/busses/i2c-powermac.c
@@ -153,12 +153,6 @@ static int i2c_powermac_master_xfer(	struct i2c_adapter *adap,
 	int			read;
 	int			addrdir;
 
-	if (num != 1) {
-		dev_err(&adap->dev,
-			"Multi-message I2C transactions not supported\n");
-		return -EOPNOTSUPP;
-	}
-
 	if (msgs->flags & I2C_M_TEN)
 		return -EINVAL;
 	read = (msgs->flags & I2C_M_RD) != 0;
@@ -205,6 +199,9 @@ static const struct i2c_algorithm i2c_powermac_algorithm = {
 	.functionality	= i2c_powermac_func,
 };
 
+static struct i2c_adapter_quirks i2c_powermac_quirks = {
+	.max_num_msgs = 1,
+};
 
 static int i2c_powermac_remove(struct platform_device *dev)
 {
@@ -434,6 +431,7 @@ static int i2c_powermac_probe(struct platform_device *dev)
 
 	platform_set_drvdata(dev, adapter);
 	adapter->algo = &i2c_powermac_algorithm;
+	adapter->quirks = &i2c_powermac_quirks;
 	i2c_set_adapdata(adapter, bus);
 	adapter->dev.parent = &dev->dev;
 
diff --git a/drivers/i2c/busses/i2c-qup.c b/drivers/i2c/busses/i2c-qup.c
index 4dad23bdffbe..fdcbdab808e9 100644
--- a/drivers/i2c/busses/i2c-qup.c
+++ b/drivers/i2c/busses/i2c-qup.c
@@ -412,17 +412,6 @@ static int qup_i2c_read_one(struct qup_i2c_dev *qup, struct i2c_msg *msg)
 	unsigned long left;
 	int ret;
 
-	/*
-	 * The QUP block will issue a NACK and STOP on the bus when reaching
-	 * the end of the read, the length of the read is specified as one byte
-	 * which limits the possible read to 256 (QUP_READ_LIMIT) bytes.
-	 */
-	if (msg->len > QUP_READ_LIMIT) {
-		dev_err(qup->dev, "HW not capable of reads over %d bytes\n",
-			QUP_READ_LIMIT);
-		return -EINVAL;
-	}
-
 	qup->msg = msg;
 	qup->pos  = 0;
 
@@ -534,6 +523,15 @@ static const struct i2c_algorithm qup_i2c_algo = {
 	.functionality	= qup_i2c_func,
 };
 
+/*
+ * The QUP block will issue a NACK and STOP on the bus when reaching
+ * the end of the read, the length of the read is specified as one byte
+ * which limits the possible read to 256 (QUP_READ_LIMIT) bytes.
+ */
+static struct i2c_adapter_quirks qup_i2c_quirks = {
+	.max_read_len = QUP_READ_LIMIT,
+};
+
 static void qup_i2c_enable_clocks(struct qup_i2c_dev *qup)
 {
 	clk_prepare_enable(qup->clk);
@@ -670,6 +668,7 @@ static int qup_i2c_probe(struct platform_device *pdev)
 
 	i2c_set_adapdata(&qup->adap, qup);
 	qup->adap.algo = &qup_i2c_algo;
+	qup->adap.quirks = &qup_i2c_quirks;
 	qup->adap.dev.parent = qup->dev;
 	qup->adap.dev.of_node = pdev->dev.of_node;
 	strlcpy(qup->adap.name, "QUP I2C adapter", sizeof(qup->adap.name));
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index 71a6e07eb7ab..5a84bea5b845 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -382,11 +382,11 @@ static bool rcar_i2c_slave_irq(struct rcar_i2c_priv *priv)
 	if (ssr_filtered & SAR) {
 		/* read or write request */
 		if (ssr_raw & STM) {
-			i2c_slave_event(priv->slave, I2C_SLAVE_REQ_READ_START, &value);
+			i2c_slave_event(priv->slave, I2C_SLAVE_READ_REQUESTED, &value);
 			rcar_i2c_write(priv, ICRXTX, value);
 			rcar_i2c_write(priv, ICSIER, SDE | SSR | SAR);
 		} else {
-			i2c_slave_event(priv->slave, I2C_SLAVE_REQ_WRITE_START, &value);
+			i2c_slave_event(priv->slave, I2C_SLAVE_WRITE_REQUESTED, &value);
 			rcar_i2c_read(priv, ICRXTX);	/* dummy read */
 			rcar_i2c_write(priv, ICSIER, SDR | SSR | SAR);
 		}
@@ -406,17 +406,15 @@ static bool rcar_i2c_slave_irq(struct rcar_i2c_priv *priv)
 		int ret;
 
 		value = rcar_i2c_read(priv, ICRXTX);
-		ret = i2c_slave_event(priv->slave, I2C_SLAVE_REQ_WRITE_END, &value);
+		ret = i2c_slave_event(priv->slave, I2C_SLAVE_WRITE_RECEIVED, &value);
 		/* Send NACK in case of error */
 		rcar_i2c_write(priv, ICSCR, SIE | SDBS | (ret < 0 ? FNA : 0));
-		i2c_slave_event(priv->slave, I2C_SLAVE_REQ_WRITE_START, &value);
 		rcar_i2c_write(priv, ICSSR, ~SDR & 0xff);
 	}
 
 	/* master wants to read from us */
 	if (ssr_filtered & SDE) {
-		i2c_slave_event(priv->slave, I2C_SLAVE_REQ_READ_END, &value);
-		i2c_slave_event(priv->slave, I2C_SLAVE_REQ_READ_START, &value);
+		i2c_slave_event(priv->slave, I2C_SLAVE_READ_PROCESSED, &value);
 		rcar_i2c_write(priv, ICRXTX, value);
 		rcar_i2c_write(priv, ICSSR, ~SDE & 0xff);
 	}
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 29f14331dd9d..1bcd75ea0b4c 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -532,7 +532,7 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 {
 	u32 packet_header;
 	u32 int_mask;
-	int ret;
+	unsigned long time_left;
 
 	tegra_i2c_flush_fifos(i2c_dev);
 
@@ -585,18 +585,20 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	dev_dbg(i2c_dev->dev, "unmasked irq: %02x\n",
 		i2c_readl(i2c_dev, I2C_INT_MASK));
 
-	ret = wait_for_completion_timeout(&i2c_dev->msg_complete, TEGRA_I2C_TIMEOUT);
+	time_left = wait_for_completion_timeout(&i2c_dev->msg_complete,
+						TEGRA_I2C_TIMEOUT);
 	tegra_i2c_mask_irq(i2c_dev, int_mask);
 
-	if (ret == 0) {
+	if (time_left == 0) {
 		dev_err(i2c_dev->dev, "i2c transfer timed out\n");
 
 		tegra_i2c_init(i2c_dev);
 		return -ETIMEDOUT;
 	}
 
-	dev_dbg(i2c_dev->dev, "transfer complete: %d %d %d\n",
-		ret, completion_done(&i2c_dev->msg_complete), i2c_dev->msg_err);
+	dev_dbg(i2c_dev->dev, "transfer complete: %lu %d %d\n",
+		time_left, completion_done(&i2c_dev->msg_complete),
+		i2c_dev->msg_err);
 
 	if (likely(i2c_dev->msg_err == I2C_ERR_NONE))
 		return 0;
diff --git a/drivers/i2c/busses/i2c-viperboard.c b/drivers/i2c/busses/i2c-viperboard.c
index 7533fa34d737..47e88adf2011 100644
--- a/drivers/i2c/busses/i2c-viperboard.c
+++ b/drivers/i2c/busses/i2c-viperboard.c
@@ -288,10 +288,6 @@ static int vprbrd_i2c_xfer(struct i2c_adapter *i2c, struct i2c_msg *msgs,
 			i, pmsg->flags & I2C_M_RD ? "read" : "write",
 			pmsg->flags, pmsg->len, pmsg->addr);
 
-		/* msgs longer than 2048 bytes are not supported by adapter */
-		if (pmsg->len > 2048)
-			return -EINVAL;
-
 		mutex_lock(&vb->lock);
 		/* directly send the message */
 		if (pmsg->flags & I2C_M_RD) {
@@ -358,6 +354,11 @@ static const struct i2c_algorithm vprbrd_algorithm = {
 	.functionality	= vprbrd_i2c_func,
 };
 
+static struct i2c_adapter_quirks vprbrd_quirks = {
+	.max_read_len = 2048,
+	.max_write_len = 2048,
+};
+
 static int vprbrd_i2c_probe(struct platform_device *pdev)
 {
 	struct vprbrd *vb = dev_get_drvdata(pdev->dev.parent);
@@ -373,6 +374,7 @@ static int vprbrd_i2c_probe(struct platform_device *pdev)
 	vb_i2c->i2c.owner = THIS_MODULE;
 	vb_i2c->i2c.class = I2C_CLASS_HWMON;
 	vb_i2c->i2c.algo = &vprbrd_algorithm;
+	vb_i2c->i2c.quirks = &vprbrd_quirks;
 	vb_i2c->i2c.algo_data = vb;
 	/* save the param in usb capabable memory */
 	vb_i2c->bus_freq_param = i2c_bus_param;
diff --git a/drivers/i2c/busses/i2c-wmt.c b/drivers/i2c/busses/i2c-wmt.c
index 82ea34925489..e1e3a85596c5 100644
--- a/drivers/i2c/busses/i2c-wmt.c
+++ b/drivers/i2c/busses/i2c-wmt.c
@@ -128,7 +128,8 @@ static int wmt_i2c_write(struct i2c_adapter *adap, struct i2c_msg *pmsg,
 {
 	struct wmt_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
 	u16 val, tcr_val;
-	int ret, wait_result;
+	int ret;
+	unsigned long wait_result;
 	int xfer_len = 0;
 
 	if (!(pmsg->flags & I2C_M_NOSTART)) {
@@ -177,7 +178,7 @@ static int wmt_i2c_write(struct i2c_adapter *adap, struct i2c_msg *pmsg,
 
 	while (xfer_len < pmsg->len) {
 		wait_result = wait_for_completion_timeout(&i2c_dev->complete,
-							  500 * HZ / 1000);
+							msecs_to_jiffies(500));
 
 		if (wait_result == 0)
 			return -ETIMEDOUT;
@@ -218,7 +219,8 @@ static int wmt_i2c_read(struct i2c_adapter *adap, struct i2c_msg *pmsg,
 {
 	struct wmt_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
 	u16 val, tcr_val;
-	int ret, wait_result;
+	int ret;
+	unsigned long wait_result;
 	u32 xfer_len = 0;
 
 	if (!(pmsg->flags & I2C_M_NOSTART)) {
@@ -266,7 +268,7 @@ static int wmt_i2c_read(struct i2c_adapter *adap, struct i2c_msg *pmsg,
 
 	while (xfer_len < pmsg->len) {
 		wait_result = wait_for_completion_timeout(&i2c_dev->complete,
-							  500 * HZ / 1000);
+							msecs_to_jiffies(500));
 
 		if (!wait_result)
 			return -ETIMEDOUT;
diff --git a/drivers/i2c/busses/i2c-xlp9xx.c b/drivers/i2c/busses/i2c-xlp9xx.c
new file mode 100644
index 000000000000..c941418f06f5
--- /dev/null
+++ b/drivers/i2c/busses/i2c-xlp9xx.c
@@ -0,0 +1,445 @@
+/*
+ * Copyright (c) 2003-2015 Broadcom Corporation
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/completion.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#define XLP9XX_I2C_DIV			0x0
+#define XLP9XX_I2C_CTRL			0x1
+#define XLP9XX_I2C_CMD			0x2
+#define XLP9XX_I2C_STATUS		0x3
+#define XLP9XX_I2C_MTXFIFO		0x4
+#define XLP9XX_I2C_MRXFIFO		0x5
+#define XLP9XX_I2C_MFIFOCTRL		0x6
+#define XLP9XX_I2C_STXFIFO		0x7
+#define XLP9XX_I2C_SRXFIFO		0x8
+#define XLP9XX_I2C_SFIFOCTRL		0x9
+#define XLP9XX_I2C_SLAVEADDR		0xA
+#define XLP9XX_I2C_OWNADDR		0xB
+#define XLP9XX_I2C_FIFOWCNT		0xC
+#define XLP9XX_I2C_INTEN		0xD
+#define XLP9XX_I2C_INTST		0xE
+#define XLP9XX_I2C_WAITCNT		0xF
+#define XLP9XX_I2C_TIMEOUT		0X10
+#define XLP9XX_I2C_GENCALLADDR		0x11
+
+#define XLP9XX_I2C_CMD_START		BIT(7)
+#define XLP9XX_I2C_CMD_STOP		BIT(6)
+#define XLP9XX_I2C_CMD_READ		BIT(5)
+#define XLP9XX_I2C_CMD_WRITE		BIT(4)
+#define XLP9XX_I2C_CMD_ACK		BIT(3)
+
+#define XLP9XX_I2C_CTRL_MCTLEN_SHIFT	16
+#define XLP9XX_I2C_CTRL_MCTLEN_MASK	0xffff0000
+#define XLP9XX_I2C_CTRL_RST		BIT(8)
+#define XLP9XX_I2C_CTRL_EN		BIT(6)
+#define XLP9XX_I2C_CTRL_MASTER		BIT(4)
+#define XLP9XX_I2C_CTRL_FIFORD		BIT(1)
+#define XLP9XX_I2C_CTRL_ADDMODE		BIT(0)
+
+#define XLP9XX_I2C_INTEN_NACKADDR	BIT(25)
+#define XLP9XX_I2C_INTEN_SADDR		BIT(13)
+#define XLP9XX_I2C_INTEN_DATADONE	BIT(12)
+#define XLP9XX_I2C_INTEN_ARLOST		BIT(11)
+#define XLP9XX_I2C_INTEN_MFIFOFULL	BIT(4)
+#define XLP9XX_I2C_INTEN_MFIFOEMTY	BIT(3)
+#define XLP9XX_I2C_INTEN_MFIFOHI	BIT(2)
+#define XLP9XX_I2C_INTEN_BUSERR		BIT(0)
+
+#define XLP9XX_I2C_MFIFOCTRL_HITH_SHIFT		8
+#define XLP9XX_I2C_MFIFOCTRL_LOTH_SHIFT		0
+#define XLP9XX_I2C_MFIFOCTRL_RST		BIT(16)
+
+#define XLP9XX_I2C_SLAVEADDR_RW			BIT(0)
+#define XLP9XX_I2C_SLAVEADDR_ADDR_SHIFT		1
+
+#define XLP9XX_I2C_IP_CLK_FREQ		133000000UL
+#define XLP9XX_I2C_DEFAULT_FREQ		100000
+#define XLP9XX_I2C_HIGH_FREQ		400000
+#define XLP9XX_I2C_FIFO_SIZE		0x80U
+#define XLP9XX_I2C_TIMEOUT_MS		1000
+
+#define XLP9XX_I2C_FIFO_WCNT_MASK	0xff
+#define XLP9XX_I2C_STATUS_ERRMASK	(XLP9XX_I2C_INTEN_ARLOST | \
+			XLP9XX_I2C_INTEN_NACKADDR | XLP9XX_I2C_INTEN_BUSERR)
+
+struct xlp9xx_i2c_dev {
+	struct device *dev;
+	struct i2c_adapter adapter;
+	struct completion msg_complete;
+	int irq;
+	bool msg_read;
+	u32 __iomem *base;
+	u32 msg_buf_remaining;
+	u32 msg_len;
+	u32 clk_hz;
+	u32 msg_err;
+	u8 *msg_buf;
+};
+
+static inline void xlp9xx_write_i2c_reg(struct xlp9xx_i2c_dev *priv,
+					unsigned long reg, u32 val)
+{
+	writel(val, priv->base + reg);
+}
+
+static inline u32 xlp9xx_read_i2c_reg(struct xlp9xx_i2c_dev *priv,
+				      unsigned long reg)
+{
+	return readl(priv->base + reg);
+}
+
+static void xlp9xx_i2c_mask_irq(struct xlp9xx_i2c_dev *priv, u32 mask)
+{
+	u32 inten;
+
+	inten = xlp9xx_read_i2c_reg(priv, XLP9XX_I2C_INTEN) & ~mask;
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_INTEN, inten);
+}
+
+static void xlp9xx_i2c_unmask_irq(struct xlp9xx_i2c_dev *priv, u32 mask)
+{
+	u32 inten;
+
+	inten = xlp9xx_read_i2c_reg(priv, XLP9XX_I2C_INTEN) | mask;
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_INTEN, inten);
+}
+
+static void xlp9xx_i2c_update_rx_fifo_thres(struct xlp9xx_i2c_dev *priv)
+{
+	u32 thres;
+
+	thres = min(priv->msg_buf_remaining, XLP9XX_I2C_FIFO_SIZE);
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_MFIFOCTRL,
+			     thres << XLP9XX_I2C_MFIFOCTRL_HITH_SHIFT);
+}
+
+static void xlp9xx_i2c_fill_tx_fifo(struct xlp9xx_i2c_dev *priv)
+{
+	u32 len, i;
+	u8 *buf = priv->msg_buf;
+
+	len = min(priv->msg_buf_remaining, XLP9XX_I2C_FIFO_SIZE);
+	for (i = 0; i < len; i++)
+		xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_MTXFIFO, buf[i]);
+	priv->msg_buf_remaining -= len;
+	priv->msg_buf += len;
+}
+
+static void xlp9xx_i2c_drain_rx_fifo(struct xlp9xx_i2c_dev *priv)
+{
+	u32 len, i;
+	u8 *buf = priv->msg_buf;
+
+	len = xlp9xx_read_i2c_reg(priv, XLP9XX_I2C_FIFOWCNT) &
+				  XLP9XX_I2C_FIFO_WCNT_MASK;
+	len = min(priv->msg_buf_remaining, len);
+	for (i = 0; i < len; i++, buf++)
+		*buf = xlp9xx_read_i2c_reg(priv, XLP9XX_I2C_MRXFIFO);
+
+	priv->msg_buf_remaining -= len;
+	priv->msg_buf = buf;
+
+	if (priv->msg_buf_remaining)
+		xlp9xx_i2c_update_rx_fifo_thres(priv);
+}
+
+static irqreturn_t xlp9xx_i2c_isr(int irq, void *dev_id)
+{
+	struct xlp9xx_i2c_dev *priv = dev_id;
+	u32 status;
+
+	status = xlp9xx_read_i2c_reg(priv, XLP9XX_I2C_INTST);
+	if (status == 0)
+		return IRQ_NONE;
+
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_INTST, status);
+	if (status & XLP9XX_I2C_STATUS_ERRMASK) {
+		priv->msg_err = status;
+		goto xfer_done;
+	}
+
+	/* SADDR ACK for SMBUS_QUICK */
+	if ((status & XLP9XX_I2C_INTEN_SADDR) && (priv->msg_len == 0))
+		goto xfer_done;
+
+	if (!priv->msg_read) {
+		if (status & XLP9XX_I2C_INTEN_MFIFOEMTY) {
+			/* TX FIFO got empty, fill it up again */
+			if (priv->msg_buf_remaining)
+				xlp9xx_i2c_fill_tx_fifo(priv);
+			else
+				xlp9xx_i2c_mask_irq(priv,
+						    XLP9XX_I2C_INTEN_MFIFOEMTY);
+		}
+	} else {
+		if (status & (XLP9XX_I2C_INTEN_DATADONE |
+			      XLP9XX_I2C_INTEN_MFIFOHI)) {
+			/* data is in FIFO, read it */
+			if (priv->msg_buf_remaining)
+				xlp9xx_i2c_drain_rx_fifo(priv);
+		}
+	}
+
+	/* Transfer complete */
+	if (status & XLP9XX_I2C_INTEN_DATADONE)
+		goto xfer_done;
+
+	return IRQ_HANDLED;
+
+xfer_done:
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_INTEN, 0);
+	complete(&priv->msg_complete);
+	return IRQ_HANDLED;
+}
+
+static int xlp9xx_i2c_init(struct xlp9xx_i2c_dev *priv)
+{
+	u32 prescale;
+
+	/*
+	 * The controller uses 5 * SCL clock internally.
+	 * So prescale value should be divided by 5.
+	 */
+	prescale = DIV_ROUND_UP(XLP9XX_I2C_IP_CLK_FREQ, priv->clk_hz);
+	prescale = ((prescale - 8) / 5) - 1;
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_CTRL, XLP9XX_I2C_CTRL_RST);
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_CTRL, XLP9XX_I2C_CTRL_EN |
+			     XLP9XX_I2C_CTRL_MASTER);
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_DIV, prescale);
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_INTEN, 0);
+
+	return 0;
+}
+
+static int xlp9xx_i2c_xfer_msg(struct xlp9xx_i2c_dev *priv, struct i2c_msg *msg,
+			       int last_msg)
+{
+	unsigned long timeleft;
+	u32 intr_mask, cmd, val;
+
+	priv->msg_buf = msg->buf;
+	priv->msg_buf_remaining = priv->msg_len = msg->len;
+	priv->msg_err = 0;
+	priv->msg_read = (msg->flags & I2C_M_RD);
+	reinit_completion(&priv->msg_complete);
+
+	/* Reset FIFO */
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_MFIFOCTRL,
+			     XLP9XX_I2C_MFIFOCTRL_RST);
+
+	/* set FIFO threshold if reading */
+	if (priv->msg_read)
+		xlp9xx_i2c_update_rx_fifo_thres(priv);
+
+	/* set slave addr */
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_SLAVEADDR,
+			     (msg->addr << XLP9XX_I2C_SLAVEADDR_ADDR_SHIFT) |
+			     (priv->msg_read ? XLP9XX_I2C_SLAVEADDR_RW : 0));
+
+	/* Build control word for transfer */
+	val = xlp9xx_read_i2c_reg(priv, XLP9XX_I2C_CTRL);
+	if (!priv->msg_read)
+		val &= ~XLP9XX_I2C_CTRL_FIFORD;
+	else
+		val |= XLP9XX_I2C_CTRL_FIFORD;	/* read */
+
+	if (msg->flags & I2C_M_TEN)
+		val |= XLP9XX_I2C_CTRL_ADDMODE;	/* 10-bit address mode*/
+	else
+		val &= ~XLP9XX_I2C_CTRL_ADDMODE;
+
+	/* set data length to be transferred */
+	val = (val & ~XLP9XX_I2C_CTRL_MCTLEN_MASK) |
+	      (msg->len << XLP9XX_I2C_CTRL_MCTLEN_SHIFT);
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_CTRL, val);
+
+	/* fill fifo during tx */
+	if (!priv->msg_read)
+		xlp9xx_i2c_fill_tx_fifo(priv);
+
+	/* set interrupt mask */
+	intr_mask = (XLP9XX_I2C_INTEN_ARLOST | XLP9XX_I2C_INTEN_BUSERR |
+		     XLP9XX_I2C_INTEN_NACKADDR | XLP9XX_I2C_INTEN_DATADONE);
+
+	if (priv->msg_read) {
+		intr_mask |= XLP9XX_I2C_INTEN_MFIFOHI;
+		if (msg->len == 0)
+			intr_mask |= XLP9XX_I2C_INTEN_SADDR;
+	} else {
+		if (msg->len == 0)
+			intr_mask |= XLP9XX_I2C_INTEN_SADDR;
+		else
+			intr_mask |= XLP9XX_I2C_INTEN_MFIFOEMTY;
+	}
+	xlp9xx_i2c_unmask_irq(priv, intr_mask);
+
+	/* set cmd reg */
+	cmd = XLP9XX_I2C_CMD_START;
+	cmd |= (priv->msg_read ? XLP9XX_I2C_CMD_READ : XLP9XX_I2C_CMD_WRITE);
+	if (last_msg)
+		cmd |= XLP9XX_I2C_CMD_STOP;
+
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_CMD, cmd);
+
+	timeleft = msecs_to_jiffies(XLP9XX_I2C_TIMEOUT_MS);
+	timeleft = wait_for_completion_timeout(&priv->msg_complete, timeleft);
+
+	if (priv->msg_err) {
+		dev_dbg(priv->dev, "transfer error %x!\n", priv->msg_err);
+		if (priv->msg_err & XLP9XX_I2C_INTEN_BUSERR)
+			xlp9xx_i2c_init(priv);
+		return -EIO;
+	}
+
+	if (timeleft == 0) {
+		dev_dbg(priv->dev, "i2c transfer timed out!\n");
+		xlp9xx_i2c_init(priv);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int xlp9xx_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
+			   int num)
+{
+	int i, ret;
+	struct xlp9xx_i2c_dev *priv = i2c_get_adapdata(adap);
+
+	for (i = 0; i < num; i++) {
+		ret = xlp9xx_i2c_xfer_msg(priv, &msgs[i], i == num - 1);
+		if (ret != 0)
+			return ret;
+	}
+
+	return num;
+}
+
+static u32 xlp9xx_i2c_functionality(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_SMBUS_EMUL | I2C_FUNC_I2C |
+		I2C_FUNC_10BIT_ADDR;
+}
+
+static struct i2c_algorithm xlp9xx_i2c_algo = {
+	.master_xfer = xlp9xx_i2c_xfer,
+	.functionality = xlp9xx_i2c_functionality,
+};
+
+static int xlp9xx_i2c_get_frequency(struct platform_device *pdev,
+				    struct xlp9xx_i2c_dev *priv)
+{
+	struct device_node *np = pdev->dev.of_node;
+	u32 freq;
+	int err;
+
+	err = of_property_read_u32(np, "clock-frequency", &freq);
+	if (err) {
+		freq = XLP9XX_I2C_DEFAULT_FREQ;
+		dev_dbg(&pdev->dev, "using default frequency %u\n", freq);
+	} else if (freq == 0 || freq > XLP9XX_I2C_HIGH_FREQ) {
+		dev_warn(&pdev->dev, "invalid frequency %u, using default\n",
+			 freq);
+		freq = XLP9XX_I2C_DEFAULT_FREQ;
+	}
+	priv->clk_hz = freq;
+
+	return 0;
+}
+
+static int xlp9xx_i2c_probe(struct platform_device *pdev)
+{
+	struct xlp9xx_i2c_dev *priv;
+	struct resource *res;
+	int err = 0;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
+
+	priv->irq = platform_get_irq(pdev, 0);
+	if (priv->irq <= 0) {
+		dev_err(&pdev->dev, "invalid irq!\n");
+		return priv->irq;
+	}
+
+	xlp9xx_i2c_get_frequency(pdev, priv);
+	xlp9xx_i2c_init(priv);
+
+	err = devm_request_irq(&pdev->dev, priv->irq, xlp9xx_i2c_isr, 0,
+			       pdev->name, priv);
+	if (err) {
+		dev_err(&pdev->dev, "IRQ request failed!\n");
+		return err;
+	}
+
+	init_completion(&priv->msg_complete);
+	priv->adapter.dev.parent = &pdev->dev;
+	priv->adapter.algo = &xlp9xx_i2c_algo;
+	priv->adapter.dev.of_node = pdev->dev.of_node;
+	priv->dev = &pdev->dev;
+
+	snprintf(priv->adapter.name, sizeof(priv->adapter.name), "xlp9xx-i2c");
+	i2c_set_adapdata(&priv->adapter, priv);
+
+	err = i2c_add_adapter(&priv->adapter);
+	if (err) {
+		dev_err(&pdev->dev, "failed to add I2C adapter!\n");
+		return err;
+	}
+
+	platform_set_drvdata(pdev, priv);
+	dev_dbg(&pdev->dev, "I2C bus:%d added\n", priv->adapter.nr);
+
+	return 0;
+}
+
+static int xlp9xx_i2c_remove(struct platform_device *pdev)
+{
+	struct xlp9xx_i2c_dev *priv;
+
+	priv = platform_get_drvdata(pdev);
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_INTEN, 0);
+	synchronize_irq(priv->irq);
+	i2c_del_adapter(&priv->adapter);
+	xlp9xx_write_i2c_reg(priv, XLP9XX_I2C_CTRL, 0);
+
+	return 0;
+}
+
+static const struct of_device_id xlp9xx_i2c_of_match[] = {
+	{ .compatible = "netlogic,xlp980-i2c", },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver xlp9xx_i2c_driver = {
+	.probe = xlp9xx_i2c_probe,
+	.remove = xlp9xx_i2c_remove,
+	.driver = {
+		.name = "xlp9xx-i2c",
+		.of_match_table = xlp9xx_i2c_of_match,
+	},
+};
+
+module_platform_driver(xlp9xx_i2c_driver);
+
+MODULE_AUTHOR("Subhendu Sekhar Behera <sbehera@broadcom.com>");
+MODULE_DESCRIPTION("XLP9XX/5XX I2C Bus Controller Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index edf274cabe81..1672e6b12774 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -133,7 +133,7 @@ static acpi_status acpi_i2c_add_device(acpi_handle handle, u32 level,
 		return AE_OK;
 
 	memset(&info, 0, sizeof(info));
-	info.acpi_node.companion = adev;
+	info.fwnode = acpi_fwnode_handle(adev);
 	info.irq = -1;
 
 	INIT_LIST_HEAD(&resource_list);
@@ -561,7 +561,7 @@ static int i2c_generic_recovery(struct i2c_adapter *adap)
 	int i = 0, val = 1, ret = 0;
 
 	if (bri->prepare_recovery)
-		bri->prepare_recovery(bri);
+		bri->prepare_recovery(adap);
 
 	/*
 	 * By this time SCL is high, as we need to give 9 falling-rising edges
@@ -586,7 +586,7 @@ static int i2c_generic_recovery(struct i2c_adapter *adap)
 	}
 
 	if (bri->unprepare_recovery)
-		bri->unprepare_recovery(bri);
+		bri->unprepare_recovery(adap);
 
 	return ret;
 }
@@ -971,7 +971,7 @@ i2c_new_device(struct i2c_adapter *adap, struct i2c_board_info const *info)
 	client->dev.bus = &i2c_bus_type;
 	client->dev.type = &i2c_client_type;
 	client->dev.of_node = info->of_node;
-	ACPI_COMPANION_SET(&client->dev, info->acpi_node.companion);
+	client->dev.fwnode = info->fwnode;
 
 	i2c_dev_set_name(adap, client);
 	status = device_register(&client->dev);
@@ -1875,6 +1875,13 @@ static int __init i2c_init(void)
 {
 	int retval;
 
+	retval = of_alias_get_highest_id("i2c");
+
+	down_write(&__i2c_board_lock);
+	if (retval >= __i2c_first_dynamic_bus_num)
+		__i2c_first_dynamic_bus_num = retval + 1;
+	up_write(&__i2c_board_lock);
+
 	retval = bus_register(&i2c_bus_type);
 	if (retval)
 		return retval;
@@ -1926,6 +1933,65 @@ module_exit(i2c_exit);
  * ----------------------------------------------------
  */
 
+/* Check if val is exceeding the quirk IFF quirk is non 0 */
+#define i2c_quirk_exceeded(val, quirk) ((quirk) && ((val) > (quirk)))
+
+static int i2c_quirk_error(struct i2c_adapter *adap, struct i2c_msg *msg, char *err_msg)
+{
+	dev_err_ratelimited(&adap->dev, "adapter quirk: %s (addr 0x%04x, size %u, %s)\n",
+			    err_msg, msg->addr, msg->len,
+			    msg->flags & I2C_M_RD ? "read" : "write");
+	return -EOPNOTSUPP;
+}
+
+static int i2c_check_for_quirks(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
+{
+	const struct i2c_adapter_quirks *q = adap->quirks;
+	int max_num = q->max_num_msgs, i;
+	bool do_len_check = true;
+
+	if (q->flags & I2C_AQ_COMB) {
+		max_num = 2;
+
+		/* special checks for combined messages */
+		if (num == 2) {
+			if (q->flags & I2C_AQ_COMB_WRITE_FIRST && msgs[0].flags & I2C_M_RD)
+				return i2c_quirk_error(adap, &msgs[0], "1st comb msg must be write");
+
+			if (q->flags & I2C_AQ_COMB_READ_SECOND && !(msgs[1].flags & I2C_M_RD))
+				return i2c_quirk_error(adap, &msgs[1], "2nd comb msg must be read");
+
+			if (q->flags & I2C_AQ_COMB_SAME_ADDR && msgs[0].addr != msgs[1].addr)
+				return i2c_quirk_error(adap, &msgs[0], "comb msg only to same addr");
+
+			if (i2c_quirk_exceeded(msgs[0].len, q->max_comb_1st_msg_len))
+				return i2c_quirk_error(adap, &msgs[0], "msg too long");
+
+			if (i2c_quirk_exceeded(msgs[1].len, q->max_comb_2nd_msg_len))
+				return i2c_quirk_error(adap, &msgs[1], "msg too long");
+
+			do_len_check = false;
+		}
+	}
+
+	if (i2c_quirk_exceeded(num, max_num))
+		return i2c_quirk_error(adap, &msgs[0], "too many messages");
+
+	for (i = 0; i < num; i++) {
+		u16 len = msgs[i].len;
+
+		if (msgs[i].flags & I2C_M_RD) {
+			if (do_len_check && i2c_quirk_exceeded(len, q->max_read_len))
+				return i2c_quirk_error(adap, &msgs[i], "msg too long");
+		} else {
+			if (do_len_check && i2c_quirk_exceeded(len, q->max_write_len))
+				return i2c_quirk_error(adap, &msgs[i], "msg too long");
+		}
+	}
+
+	return 0;
+}
+
 /**
  * __i2c_transfer - unlocked flavor of i2c_transfer
  * @adap: Handle to I2C bus
@@ -1943,6 +2009,9 @@ int __i2c_transfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 	unsigned long orig_jiffies;
 	int ret, try;
 
+	if (adap->quirks && i2c_check_for_quirks(adap, msgs, num))
+		return -EOPNOTSUPP;
+
 	/* i2c_trace_msg gets enabled when tracepoint i2c_transfer gets
 	 * enabled.  This is an efficient way of keeping the for-loop from
 	 * being executed when not needed.
diff --git a/drivers/i2c/i2c-slave-eeprom.c b/drivers/i2c/i2c-slave-eeprom.c
index cf9b09db092f..822374654609 100644
--- a/drivers/i2c/i2c-slave-eeprom.c
+++ b/drivers/i2c/i2c-slave-eeprom.c
@@ -36,7 +36,7 @@ static int i2c_slave_eeprom_slave_cb(struct i2c_client *client,
 	struct eeprom_data *eeprom = i2c_get_clientdata(client);
 
 	switch (event) {
-	case I2C_SLAVE_REQ_WRITE_END:
+	case I2C_SLAVE_WRITE_RECEIVED:
 		if (eeprom->first_write) {
 			eeprom->buffer_idx = *val;
 			eeprom->first_write = false;
@@ -47,17 +47,23 @@ static int i2c_slave_eeprom_slave_cb(struct i2c_client *client,
 		}
 		break;
 
-	case I2C_SLAVE_REQ_READ_START:
+	case I2C_SLAVE_READ_PROCESSED:
+		/* The previous byte made it to the bus, get next one */
+		eeprom->buffer_idx++;
+		/* fallthrough */
+	case I2C_SLAVE_READ_REQUESTED:
 		spin_lock(&eeprom->buffer_lock);
 		*val = eeprom->buffer[eeprom->buffer_idx];
 		spin_unlock(&eeprom->buffer_lock);
-		break;
-
-	case I2C_SLAVE_REQ_READ_END:
-		eeprom->buffer_idx++;
+		/*
+		 * Do not increment buffer_idx here, because we don't know if
+		 * this byte will be actually used. Read Linux I2C slave docs
+		 * for details.
+		 */
 		break;
 
 	case I2C_SLAVE_STOP:
+	case I2C_SLAVE_WRITE_REQUESTED:
 		eeprom->first_write = true;
 		break;
 
diff --git a/drivers/i2c/muxes/i2c-mux-gpio.c b/drivers/i2c/muxes/i2c-mux-gpio.c
index f5798eb4076b..70db99264339 100644
--- a/drivers/i2c/muxes/i2c-mux-gpio.c
+++ b/drivers/i2c/muxes/i2c-mux-gpio.c
@@ -76,10 +76,9 @@ static int i2c_mux_gpio_probe_dt(struct gpiomux *mux,
 		return -ENODEV;
 	}
 	adapter = of_find_i2c_adapter_by_node(adapter_np);
-	if (!adapter) {
-		dev_err(&pdev->dev, "Cannot find parent bus\n");
+	if (!adapter)
 		return -EPROBE_DEFER;
-	}
+
 	mux->data.parent = i2c_adapter_id(adapter);
 	put_device(&adapter->dev);
 
@@ -177,11 +176,8 @@ static int i2c_mux_gpio_probe(struct platform_device *pdev)
 	}
 
 	parent = i2c_get_adapter(mux->data.parent);
-	if (!parent) {
-		dev_err(&pdev->dev, "Parent adapter (%d) not found\n",
-			mux->data.parent);
+	if (!parent)
 		return -EPROBE_DEFER;
-	}
 
 	mux->parent = parent;
 	mux->gpio_base = gpio_base;
diff --git a/drivers/i2c/muxes/i2c-mux-pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index 3d8f4fe2e47e..bea0d2de2993 100644
--- a/drivers/i2c/muxes/i2c-mux-pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -204,9 +204,9 @@ static int pca954x_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, data);
 
 	/* Get the mux out of reset if a reset GPIO is specified. */
-	gpio = devm_gpiod_get(&client->dev, "reset");
-	if (!IS_ERR(gpio))
-		gpiod_direction_output(gpio, 0);
+	gpio = devm_gpiod_get_optional(&client->dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(gpio))
+		return PTR_ERR(gpio);
 
 	/* Write the mux register at addr to verify
 	 * that the mux is in fact present. This also
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-pnp.c b/drivers/ide/ide-pnp.c
index e5f3db831373..f5f2b62471da 100644
--- a/drivers/ide/ide-pnp.c
+++ b/drivers/ide/ide-pnp.c
@@ -96,17 +96,5 @@ static struct pnp_driver idepnp_driver = {
 	.remove		= idepnp_remove,
 };
 
-static int __init pnpide_init(void)
-{
-	return pnp_register_driver(&idepnp_driver);
-}
-
-static void __exit pnpide_exit(void)
-{
-	pnp_unregister_driver(&idepnp_driver);
-}
-
-module_init(pnpide_init);
-module_exit(pnpide_exit);
-
+module_pnp_driver(idepnp_driver);
 MODULE_LICENSE("GPL");
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..2a36a95d95cf 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>
@@ -218,18 +218,10 @@ static struct cpuidle_state byt_cstates[] = {
 		.enter = &intel_idle,
 		.enter_freeze = intel_idle_freeze, },
 	{
-		.name = "C1E-BYT",
-		.desc = "MWAIT 0x01",
-		.flags = MWAIT2flg(0x01),
-		.exit_latency = 15,
-		.target_residency = 30,
-		.enter = &intel_idle,
-		.enter_freeze = intel_idle_freeze, },
-	{
 		.name = "C6N-BYT",
 		.desc = "MWAIT 0x58",
 		.flags = MWAIT2flg(0x58) | CPUIDLE_FLAG_TLB_FLUSHED,
-		.exit_latency = 40,
+		.exit_latency = 300,
 		.target_residency = 275,
 		.enter = &intel_idle,
 		.enter_freeze = intel_idle_freeze, },
@@ -237,7 +229,7 @@ static struct cpuidle_state byt_cstates[] = {
 		.name = "C6S-BYT",
 		.desc = "MWAIT 0x52",
 		.flags = MWAIT2flg(0x52) | CPUIDLE_FLAG_TLB_FLUSHED,
-		.exit_latency = 140,
+		.exit_latency = 500,
 		.target_residency = 560,
 		.enter = &intel_idle,
 		.enter_freeze = intel_idle_freeze, },
@@ -246,7 +238,7 @@ static struct cpuidle_state byt_cstates[] = {
 		.desc = "MWAIT 0x60",
 		.flags = MWAIT2flg(0x60) | CPUIDLE_FLAG_TLB_FLUSHED,
 		.exit_latency = 1200,
-		.target_residency = 1500,
+		.target_residency = 4000,
 		.enter = &intel_idle,
 		.enter_freeze = intel_idle_freeze, },
 	{
@@ -261,6 +253,51 @@ static struct cpuidle_state byt_cstates[] = {
 		.enter = NULL }
 };
 
+static struct cpuidle_state cht_cstates[] = {
+	{
+		.name = "C1-CHT",
+		.desc = "MWAIT 0x00",
+		.flags = MWAIT2flg(0x00),
+		.exit_latency = 1,
+		.target_residency = 1,
+		.enter = &intel_idle,
+		.enter_freeze = intel_idle_freeze, },
+	{
+		.name = "C6N-CHT",
+		.desc = "MWAIT 0x58",
+		.flags = MWAIT2flg(0x58) | CPUIDLE_FLAG_TLB_FLUSHED,
+		.exit_latency = 80,
+		.target_residency = 275,
+		.enter = &intel_idle,
+		.enter_freeze = intel_idle_freeze, },
+	{
+		.name = "C6S-CHT",
+		.desc = "MWAIT 0x52",
+		.flags = MWAIT2flg(0x52) | CPUIDLE_FLAG_TLB_FLUSHED,
+		.exit_latency = 200,
+		.target_residency = 560,
+		.enter = &intel_idle,
+		.enter_freeze = intel_idle_freeze, },
+	{
+		.name = "C7-CHT",
+		.desc = "MWAIT 0x60",
+		.flags = MWAIT2flg(0x60) | CPUIDLE_FLAG_TLB_FLUSHED,
+		.exit_latency = 1200,
+		.target_residency = 4000,
+		.enter = &intel_idle,
+		.enter_freeze = intel_idle_freeze, },
+	{
+		.name = "C7S-CHT",
+		.desc = "MWAIT 0x64",
+		.flags = MWAIT2flg(0x64) | CPUIDLE_FLAG_TLB_FLUSHED,
+		.exit_latency = 10000,
+		.target_residency = 20000,
+		.enter = &intel_idle,
+		.enter_freeze = intel_idle_freeze, },
+	{
+		.enter = NULL }
+};
+
 static struct cpuidle_state ivb_cstates[] = {
 	{
 		.name = "C1-IVB",
@@ -638,12 +675,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 +702,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();
+	unsigned long on = (unsigned long)arg;
 
-	reason = reason ?
-		CLOCK_EVT_NOTIFY_BROADCAST_ON : CLOCK_EVT_NOTIFY_BROADCAST_OFF;
-
-	clockevents_notify(reason, &cpu);
+	if (on)
+		tick_broadcast_enable();
+	else
+		tick_broadcast_disable();
 }
 
 static int cpu_hotplug_notify(struct notifier_block *n,
@@ -748,6 +784,12 @@ static const struct idle_cpu idle_cpu_byt = {
 	.byt_auto_demotion_disable_flag = true,
 };
 
+static const struct idle_cpu idle_cpu_cht = {
+	.state_table = cht_cstates,
+	.disable_promotion_to_c1e = true,
+	.byt_auto_demotion_disable_flag = true,
+};
+
 static const struct idle_cpu idle_cpu_ivb = {
 	.state_table = ivb_cstates,
 	.disable_promotion_to_c1e = true,
@@ -776,7 +818,7 @@ static const struct idle_cpu idle_cpu_avn = {
 #define ICPU(model, cpu) \
 	{ X86_VENDOR_INTEL, 6, model, X86_FEATURE_MWAIT, (unsigned long)&cpu }
 
-static const struct x86_cpu_id intel_idle_ids[] = {
+static const struct x86_cpu_id intel_idle_ids[] __initconst = {
 	ICPU(0x1a, idle_cpu_nehalem),
 	ICPU(0x1e, idle_cpu_nehalem),
 	ICPU(0x1f, idle_cpu_nehalem),
@@ -790,6 +832,7 @@ static const struct x86_cpu_id intel_idle_ids[] = {
 	ICPU(0x2d, idle_cpu_snb),
 	ICPU(0x36, idle_cpu_atom),
 	ICPU(0x37, idle_cpu_byt),
+	ICPU(0x4c, idle_cpu_cht),
 	ICPU(0x3a, idle_cpu_ivb),
 	ICPU(0x3e, idle_cpu_ivt),
 	ICPU(0x3c, idle_cpu_hsw),
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/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index df6a593bd4bd..2b4fad6998c1 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -123,7 +123,8 @@ static int accel_3d_read_raw(struct iio_dev *indio_dev,
 			*val = sensor_hub_input_attr_get_raw_value(
 					accel_state->common_attributes.hsdev,
 					HID_USAGE_SENSOR_ACCEL_3D, address,
-					report_id);
+					report_id,
+					SENSOR_HUB_SYNC);
 		else {
 			*val = 0;
 			hid_sensor_power_state(&accel_state->common_attributes,
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/max1027.c b/drivers/iio/adc/max1027.c
index 87ee1c7d0b54..44bf815adb6c 100644
--- a/drivers/iio/adc/max1027.c
+++ b/drivers/iio/adc/max1027.c
@@ -436,7 +436,7 @@ static int max1027_probe(struct spi_device *spi)
 				  indio_dev->num_channels * 2,
 				  GFP_KERNEL);
 	if (st->buffer == NULL) {
-		dev_err(&indio_dev->dev, "Can't allocate bufffer\n");
+		dev_err(&indio_dev->dev, "Can't allocate buffer\n");
 		return -ENOMEM;
 	}
 
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/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
index 25b01e156d82..e81f434760f4 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
@@ -153,8 +153,8 @@ s32 hid_sensor_read_poll_value(struct hid_sensor_common *st)
 	int ret;
 
 	ret = sensor_hub_get_feature(st->hsdev,
-		st->poll.report_id,
-		st->poll.index, &value);
+				     st->poll.report_id,
+				     st->poll.index, sizeof(value), &value);
 
 	if (ret < 0 || value < 0) {
 		return -EINVAL;
@@ -174,8 +174,8 @@ int hid_sensor_read_samp_freq_value(struct hid_sensor_common *st,
 	int ret;
 
 	ret = sensor_hub_get_feature(st->hsdev,
-		st->poll.report_id,
-		st->poll.index, &value);
+				     st->poll.report_id,
+				     st->poll.index, sizeof(value), &value);
 	if (ret < 0 || value < 0) {
 		*val1 = *val2 = 0;
 		return -EINVAL;
@@ -212,9 +212,8 @@ int hid_sensor_write_samp_freq_value(struct hid_sensor_common *st,
 		else
 			value = 0;
 	}
-	ret = sensor_hub_set_feature(st->hsdev,
-		st->poll.report_id,
-		st->poll.index, value);
+	ret = sensor_hub_set_feature(st->hsdev, st->poll.report_id,
+				     st->poll.index, sizeof(value), &value);
 	if (ret < 0 || value < 0)
 		ret = -EINVAL;
 
@@ -229,8 +228,9 @@ int hid_sensor_read_raw_hyst_value(struct hid_sensor_common *st,
 	int ret;
 
 	ret = sensor_hub_get_feature(st->hsdev,
-		st->sensitivity.report_id,
-		st->sensitivity.index, &value);
+				     st->sensitivity.report_id,
+				     st->sensitivity.index, sizeof(value),
+				     &value);
 	if (ret < 0 || value < 0) {
 		*val1 = *val2 = 0;
 		return -EINVAL;
@@ -253,9 +253,9 @@ int hid_sensor_write_raw_hyst_value(struct hid_sensor_common *st,
 	value = convert_to_vtf_format(st->sensitivity.size,
 				st->sensitivity.unit_expo,
 				val1, val2);
-	ret = sensor_hub_set_feature(st->hsdev,
-		st->sensitivity.report_id,
-		st->sensitivity.index, value);
+	ret = sensor_hub_set_feature(st->hsdev, st->sensitivity.report_id,
+				     st->sensitivity.index, sizeof(value),
+				     &value);
 	if (ret < 0 || value < 0)
 		ret = -EINVAL;
 
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index 2f1d535b94c4..610fc98f88ef 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -68,20 +68,21 @@ static int _hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 	if (state_val >= 0) {
 		state_val += st->power_state.logical_minimum;
 		sensor_hub_set_feature(st->hsdev, st->power_state.report_id,
-					st->power_state.index,
-					(s32)state_val);
+				       st->power_state.index, sizeof(state_val),
+				       &state_val);
 	}
 
 	if (report_val >= 0) {
 		report_val += st->report_state.logical_minimum;
 		sensor_hub_set_feature(st->hsdev, st->report_state.report_id,
-					st->report_state.index,
-					(s32)report_val);
+				       st->report_state.index,
+				       sizeof(report_val),
+				       &report_val);
 	}
 
 	sensor_hub_get_feature(st->hsdev, st->power_state.report_id,
-					st->power_state.index,
-					&state_val);
+			       st->power_state.index,
+			       sizeof(state_val), &state_val);
 	if (state && poll_value)
 		msleep_interruptible(poll_value * 2);
 
diff --git a/drivers/iio/common/ssp_sensors/ssp_dev.c b/drivers/iio/common/ssp_sensors/ssp_dev.c
index 55a90082a29b..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,
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/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/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
index a3c3e19de527..b5883b6f4e50 100644
--- a/drivers/iio/gyro/hid-sensor-gyro-3d.c
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -123,7 +123,8 @@ static int gyro_3d_read_raw(struct iio_dev *indio_dev,
 			*val = sensor_hub_input_attr_get_raw_value(
 					gyro_state->common_attributes.hsdev,
 					HID_USAGE_SENSOR_GYRO_3D, address,
-					report_id);
+					report_id,
+					SENSOR_HUB_SYNC);
 		else {
 			*val = 0;
 			hid_sensor_power_state(&gyro_state->common_attributes,
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/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 d8d5bed65e07..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;
@@ -825,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:
@@ -841,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 a224afd6380c..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"
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/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
index 948acfc38b8c..1609ecdd01b0 100644
--- a/drivers/iio/light/hid-sensor-als.c
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -101,7 +101,8 @@ static int als_read_raw(struct iio_dev *indio_dev,
 			*val = sensor_hub_input_attr_get_raw_value(
 					als_state->common_attributes.hsdev,
 					HID_USAGE_SENSOR_ALS, address,
-					report_id);
+					report_id,
+					SENSOR_HUB_SYNC);
 			hid_sensor_power_state(&als_state->common_attributes,
 						false);
 		} else {
diff --git a/drivers/iio/light/hid-sensor-prox.c b/drivers/iio/light/hid-sensor-prox.c
index 3ecf79ed08ac..91ecc46ffeaa 100644
--- a/drivers/iio/light/hid-sensor-prox.c
+++ b/drivers/iio/light/hid-sensor-prox.c
@@ -96,7 +96,8 @@ static int prox_read_raw(struct iio_dev *indio_dev,
 			*val = sensor_hub_input_attr_get_raw_value(
 				prox_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_PROX, address,
-				report_id);
+				report_id,
+				SENSOR_HUB_SYNC);
 			hid_sensor_power_state(&prox_state->common_attributes,
 						false);
 		} else {
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/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index d22993b4066a..4f9c0be24451 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -170,7 +170,8 @@ static int magn_3d_read_raw(struct iio_dev *indio_dev,
 			*val = sensor_hub_input_attr_get_raw_value(
 				magn_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_COMPASS_3D, address,
-				report_id);
+				report_id,
+				SENSOR_HUB_SYNC);
 		else {
 			*val = 0;
 			hid_sensor_power_state(&magn_state->common_attributes,
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/orientation/hid-sensor-incl-3d.c b/drivers/iio/orientation/hid-sensor-incl-3d.c
index 73854460bb2c..5930fa32a2ab 100644
--- a/drivers/iio/orientation/hid-sensor-incl-3d.c
+++ b/drivers/iio/orientation/hid-sensor-incl-3d.c
@@ -124,7 +124,8 @@ static int incl_3d_read_raw(struct iio_dev *indio_dev,
 			*val = sensor_hub_input_attr_get_raw_value(
 				incl_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_INCLINOMETER_3D, address,
-				report_id);
+				report_id,
+				SENSOR_HUB_SYNC);
 		else {
 			hid_sensor_power_state(&incl_state->common_attributes,
 						false);
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/hid-sensor-press.c b/drivers/iio/pressure/hid-sensor-press.c
index 1af314926ebd..7bb8d4c1f7df 100644
--- a/drivers/iio/pressure/hid-sensor-press.c
+++ b/drivers/iio/pressure/hid-sensor-press.c
@@ -100,7 +100,8 @@ static int press_read_raw(struct iio_dev *indio_dev,
 			*val = sensor_hub_input_attr_get_raw_value(
 				press_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_PRESSURE, address,
-				report_id);
+				report_id,
+				SENSOR_HUB_SYNC);
 			hid_sensor_power_state(&press_state->common_attributes,
 						false);
 		} else {
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/ipath/ipath_file_ops.c b/drivers/infiniband/hw/ipath/ipath_file_ops.c
index 6d7f453b4d05..450d15965005 100644
--- a/drivers/infiniband/hw/ipath/ipath_file_ops.c
+++ b/drivers/infiniband/hw/ipath/ipath_file_ops.c
@@ -40,9 +40,9 @@
 #include <linux/slab.h>
 #include <linux/highmem.h>
 #include <linux/io.h>
-#include <linux/aio.h>
 #include <linux/jiffies.h>
 #include <linux/cpu.h>
+#include <linux/uio.h>
 #include <asm/pgtable.h>
 
 #include "ipath_kernel.h"
@@ -53,15 +53,19 @@ static int ipath_open(struct inode *, struct file *);
 static int ipath_close(struct inode *, struct file *);
 static ssize_t ipath_write(struct file *, const char __user *, size_t,
 			   loff_t *);
-static ssize_t ipath_writev(struct kiocb *, const struct iovec *,
-			    unsigned long , loff_t);
+static ssize_t ipath_write_iter(struct kiocb *, struct iov_iter *from);
 static unsigned int ipath_poll(struct file *, struct poll_table_struct *);
 static int ipath_mmap(struct file *, struct vm_area_struct *);
 
+/*
+ * This is really, really weird shit - write() and writev() here
+ * have completely unrelated semantics.  Sucky userland ABI,
+ * film at 11.
+ */
 static const struct file_operations ipath_file_ops = {
 	.owner = THIS_MODULE,
 	.write = ipath_write,
-	.aio_write = ipath_writev,
+	.write_iter = ipath_write_iter,
 	.open = ipath_open,
 	.release = ipath_close,
 	.poll = ipath_poll,
@@ -2414,18 +2418,17 @@ bail:
 	return ret;
 }
 
-static ssize_t ipath_writev(struct kiocb *iocb, const struct iovec *iov,
-			    unsigned long dim, loff_t off)
+static ssize_t ipath_write_iter(struct kiocb *iocb, struct iov_iter *from)
 {
 	struct file *filp = iocb->ki_filp;
 	struct ipath_filedata *fp = filp->private_data;
 	struct ipath_portdata *pd = port_fp(filp);
 	struct ipath_user_sdma_queue *pq = fp->pq;
 
-	if (!dim)
+	if (!iter_is_iovec(from) || !from->nr_segs)
 		return -EINVAL;
 
-	return ipath_user_sdma_writev(pd->port_dd, pq, iov, dim);
+	return ipath_user_sdma_writev(pd->port_dd, pq, from->iov, from->nr_segs);
 }
 
 static struct class *ipath_class;
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..976bea794b5f 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -587,8 +587,9 @@ static int mlx4_ib_SET_PORT(struct mlx4_ib_dev *dev, u8 port, int reset_qkey_vio
 		((__be32 *) mailbox->buf)[1] = cpu_to_be32(cap_mask);
 	}
 
-	err = mlx4_cmd(dev->dev, mailbox->dma, port, 0, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_WRAPPED);
+	err = mlx4_cmd(dev->dev, mailbox->dma, port, MLX4_SET_PORT_IB_OPCODE,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_WRAPPED);
 
 	mlx4_free_cmd_mailbox(dev->dev, mailbox);
 	return err;
@@ -1525,8 +1526,8 @@ static void update_gids_task(struct work_struct *work)
 	memcpy(gids, gw->gids, sizeof gw->gids);
 
 	err = mlx4_cmd(dev, mailbox->dma, MLX4_SET_PORT_GID_TABLE << 8 | gw->port,
-		       1, MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
-		       MLX4_CMD_WRAPPED);
+		       MLX4_SET_PORT_ETH_OPCODE, MLX4_CMD_SET_PORT,
+		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_WRAPPED);
 	if (err)
 		pr_warn("set port command failed\n");
 	else
@@ -1564,7 +1565,7 @@ static void reset_gids_task(struct work_struct *work)
 				    IB_LINK_LAYER_ETHERNET) {
 		err = mlx4_cmd(dev, mailbox->dma,
 			       MLX4_SET_PORT_GID_TABLE << 8 | gw->port,
-			       1, MLX4_CMD_SET_PORT,
+			       MLX4_SET_PORT_ETH_OPCODE, MLX4_CMD_SET_PORT,
 			       MLX4_CMD_TIME_CLASS_B,
 			       MLX4_CMD_WRAPPED);
 		if (err)
@@ -2697,8 +2698,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;
+
+		if (!curr_netdev)
+			continue;
 
-		enum ib_port_state curr_port_state =
+		curr_port_state =
 			(netif_running(curr_netdev) &&
 			 netif_carrier_ok(curr_netdev)) ?
 			IB_PORT_ACTIVE : IB_PORT_DOWN;
diff --git a/drivers/infiniband/hw/mlx5/ah.c b/drivers/infiniband/hw/mlx5/ah.c
index 39ab0caefdf9..66080580e24d 100644
--- a/drivers/infiniband/hw/mlx5/ah.c
+++ b/drivers/infiniband/hw/mlx5/ah.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mlx5/cq.c b/drivers/infiniband/hw/mlx5/cq.c
index c463e7bba5f4..2ee6b1051975 100644
--- a/drivers/infiniband/hw/mlx5/cq.c
+++ b/drivers/infiniband/hw/mlx5/cq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -572,11 +572,15 @@ int mlx5_ib_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc)
 
 int mlx5_ib_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags flags)
 {
+	struct mlx5_core_dev *mdev = to_mdev(ibcq->device)->mdev;
+	void __iomem *uar_page = mdev->priv.uuari.uars[0].map;
+
 	mlx5_cq_arm(&to_mcq(ibcq)->mcq,
 		    (flags & IB_CQ_SOLICITED_MASK) == IB_CQ_SOLICITED ?
 		    MLX5_CQ_DB_REQ_NOT_SOL : MLX5_CQ_DB_REQ_NOT,
-		    to_mdev(ibcq->device)->mdev->priv.uuari.uars[0].map,
-		    MLX5_GET_DOORBELL_LOCK(&to_mdev(ibcq->device)->mdev->priv.cq_uar_lock));
+		    uar_page,
+		    MLX5_GET_DOORBELL_LOCK(&mdev->priv.cq_uar_lock),
+		    to_mcq(ibcq)->mcq.cons_index);
 
 	return 0;
 }
@@ -697,8 +701,6 @@ static int create_cq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_cq *cq,
 
 	cq->mcq.set_ci_db  = cq->db.db;
 	cq->mcq.arm_db     = cq->db.db + 1;
-	*cq->mcq.set_ci_db = 0;
-	*cq->mcq.arm_db    = 0;
 	cq->mcq.cqe_sz = cqe_size;
 
 	err = alloc_cq_buf(dev, &cq->buf, entries, cqe_size);
@@ -782,7 +784,7 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev, int entries,
 	cq->cqe_size = cqe_size;
 	cqb->ctx.cqe_sz_flags = cqe_sz_to_mlx_sz(cqe_size) << 5;
 	cqb->ctx.log_sz_usr_page = cpu_to_be32((ilog2(entries) << 24) | index);
-	err = mlx5_vector2eqn(dev, vector, &eqn, &irqn);
+	err = mlx5_vector2eqn(dev->mdev, vector, &eqn, &irqn);
 	if (err)
 		goto err_cqb;
 
diff --git a/drivers/infiniband/hw/mlx5/doorbell.c b/drivers/infiniband/hw/mlx5/doorbell.c
index ece028fc47d6..a0e4e6ddb71a 100644
--- a/drivers/infiniband/hw/mlx5/doorbell.c
+++ b/drivers/infiniband/hw/mlx5/doorbell.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mlx5/mad.c b/drivers/infiniband/hw/mlx5/mad.c
index 657af9a1167c..9cf9a37bb5ff 100644
--- a/drivers/infiniband/hw/mlx5/mad.c
+++ b/drivers/infiniband/hw/mlx5/mad.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index cc4ac1e583b2..57c9809e8b87 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -62,95 +62,6 @@ static char mlx5_version[] =
 	DRIVER_NAME ": Mellanox Connect-IB Infiniband driver v"
 	DRIVER_VERSION " (" DRIVER_RELDATE ")\n";
 
-int mlx5_vector2eqn(struct mlx5_ib_dev *dev, int vector, int *eqn, int *irqn)
-{
-	struct mlx5_eq_table *table = &dev->mdev->priv.eq_table;
-	struct mlx5_eq *eq, *n;
-	int err = -ENOENT;
-
-	spin_lock(&table->lock);
-	list_for_each_entry_safe(eq, n, &dev->eqs_list, list) {
-		if (eq->index == vector) {
-			*eqn = eq->eqn;
-			*irqn = eq->irqn;
-			err = 0;
-			break;
-		}
-	}
-	spin_unlock(&table->lock);
-
-	return err;
-}
-
-static int alloc_comp_eqs(struct mlx5_ib_dev *dev)
-{
-	struct mlx5_eq_table *table = &dev->mdev->priv.eq_table;
-	char name[MLX5_MAX_EQ_NAME];
-	struct mlx5_eq *eq, *n;
-	int ncomp_vec;
-	int nent;
-	int err;
-	int i;
-
-	INIT_LIST_HEAD(&dev->eqs_list);
-	ncomp_vec = table->num_comp_vectors;
-	nent = MLX5_COMP_EQ_SIZE;
-	for (i = 0; i < ncomp_vec; i++) {
-		eq = kzalloc(sizeof(*eq), GFP_KERNEL);
-		if (!eq) {
-			err = -ENOMEM;
-			goto clean;
-		}
-
-		snprintf(name, MLX5_MAX_EQ_NAME, "mlx5_comp%d", i);
-		err = mlx5_create_map_eq(dev->mdev, eq,
-					 i + MLX5_EQ_VEC_COMP_BASE, nent, 0,
-					 name, &dev->mdev->priv.uuari.uars[0]);
-		if (err) {
-			kfree(eq);
-			goto clean;
-		}
-		mlx5_ib_dbg(dev, "allocated completion EQN %d\n", eq->eqn);
-		eq->index = i;
-		spin_lock(&table->lock);
-		list_add_tail(&eq->list, &dev->eqs_list);
-		spin_unlock(&table->lock);
-	}
-
-	dev->num_comp_vectors = ncomp_vec;
-	return 0;
-
-clean:
-	spin_lock(&table->lock);
-	list_for_each_entry_safe(eq, n, &dev->eqs_list, list) {
-		list_del(&eq->list);
-		spin_unlock(&table->lock);
-		if (mlx5_destroy_unmap_eq(dev->mdev, eq))
-			mlx5_ib_warn(dev, "failed to destroy EQ 0x%x\n", eq->eqn);
-		kfree(eq);
-		spin_lock(&table->lock);
-	}
-	spin_unlock(&table->lock);
-	return err;
-}
-
-static void free_comp_eqs(struct mlx5_ib_dev *dev)
-{
-	struct mlx5_eq_table *table = &dev->mdev->priv.eq_table;
-	struct mlx5_eq *eq, *n;
-
-	spin_lock(&table->lock);
-	list_for_each_entry_safe(eq, n, &dev->eqs_list, list) {
-		list_del(&eq->list);
-		spin_unlock(&table->lock);
-		if (mlx5_destroy_unmap_eq(dev->mdev, eq))
-			mlx5_ib_warn(dev, "failed to destroy EQ 0x%x\n", eq->eqn);
-		kfree(eq);
-		spin_lock(&table->lock);
-	}
-	spin_unlock(&table->lock);
-}
-
 static int mlx5_ib_query_device(struct ib_device *ibdev,
 				struct ib_device_attr *props)
 {
@@ -1291,10 +1202,6 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 
 	get_ext_port_caps(dev);
 
-	err = alloc_comp_eqs(dev);
-	if (err)
-		goto err_dealloc;
-
 	MLX5_INIT_DOORBELL_LOCK(&dev->uar_lock);
 
 	strlcpy(dev->ib_dev.name, "mlx5_%d", IB_DEVICE_NAME_MAX);
@@ -1303,7 +1210,8 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	dev->ib_dev.local_dma_lkey	= mdev->caps.gen.reserved_lkey;
 	dev->num_ports		= mdev->caps.gen.num_ports;
 	dev->ib_dev.phys_port_cnt     = dev->num_ports;
-	dev->ib_dev.num_comp_vectors	= dev->num_comp_vectors;
+	dev->ib_dev.num_comp_vectors    =
+		dev->mdev->priv.eq_table.num_comp_vectors;
 	dev->ib_dev.dma_device	= &mdev->pdev->dev;
 
 	dev->ib_dev.uverbs_abi_ver	= MLX5_IB_UVERBS_ABI_VERSION;
@@ -1390,13 +1298,13 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 
 	err = init_node_data(dev);
 	if (err)
-		goto err_eqs;
+		goto err_dealloc;
 
 	mutex_init(&dev->cap_mask_mutex);
 
 	err = create_dev_resources(&dev->devr);
 	if (err)
-		goto err_eqs;
+		goto err_dealloc;
 
 	err = mlx5_ib_odp_init_one(dev);
 	if (err)
@@ -1433,9 +1341,6 @@ err_odp:
 err_rsrc:
 	destroy_dev_resources(&dev->devr);
 
-err_eqs:
-	free_comp_eqs(dev);
-
 err_dealloc:
 	ib_dealloc_device((struct ib_device *)dev);
 
@@ -1450,7 +1355,6 @@ static void mlx5_ib_remove(struct mlx5_core_dev *mdev, void *context)
 	destroy_umrc_res(dev);
 	mlx5_ib_odp_remove_one(dev);
 	destroy_dev_resources(&dev->devr);
-	free_comp_eqs(dev);
 	ib_dealloc_device(&dev->ib_dev);
 }
 
@@ -1458,6 +1362,7 @@ static struct mlx5_interface mlx5_ib_interface = {
 	.add            = mlx5_ib_add,
 	.remove         = mlx5_ib_remove,
 	.event          = mlx5_ib_event,
+	.protocol	= MLX5_INTERFACE_PROTOCOL_IB,
 };
 
 static int __init mlx5_ib_init(void)
diff --git a/drivers/infiniband/hw/mlx5/mem.c b/drivers/infiniband/hw/mlx5/mem.c
index 611a9fdf2f38..40df2cca0609 100644
--- a/drivers/infiniband/hw/mlx5/mem.c
+++ b/drivers/infiniband/hw/mlx5/mem.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mlx5/mlx5_ib.h b/drivers/infiniband/hw/mlx5/mlx5_ib.h
index 83f22fe297c8..dff1cfcdf476 100644
--- a/drivers/infiniband/hw/mlx5/mlx5_ib.h
+++ b/drivers/infiniband/hw/mlx5/mlx5_ib.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -421,9 +421,7 @@ struct mlx5_ib_dev {
 	struct ib_device		ib_dev;
 	struct mlx5_core_dev		*mdev;
 	MLX5_DECLARE_DOORBELL_LOCK(uar_lock);
-	struct list_head		eqs_list;
 	int				num_ports;
-	int				num_comp_vectors;
 	/* serialize update of capability mask
 	 */
 	struct mutex			cap_mask_mutex;
@@ -594,7 +592,6 @@ struct ib_xrcd *mlx5_ib_alloc_xrcd(struct ib_device *ibdev,
 					  struct ib_ucontext *context,
 					  struct ib_udata *udata);
 int mlx5_ib_dealloc_xrcd(struct ib_xrcd *xrcd);
-int mlx5_vector2eqn(struct mlx5_ib_dev *dev, int vector, int *eqn, int *irqn);
 int mlx5_ib_get_buf_offset(u64 addr, int page_shift, u32 *offset);
 int mlx5_query_ext_port_caps(struct mlx5_ib_dev *dev, u8 port);
 int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
diff --git a/drivers/infiniband/hw/mlx5/mr.c b/drivers/infiniband/hw/mlx5/mr.c
index cd9822eeacae..71c593583864 100644
--- a/drivers/infiniband/hw/mlx5/mr.c
+++ b/drivers/infiniband/hw/mlx5/mr.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mlx5/odp.c b/drivers/infiniband/hw/mlx5/odp.c
index a2c541c4809a..5099db08afd2 100644
--- a/drivers/infiniband/hw/mlx5/odp.c
+++ b/drivers/infiniband/hw/mlx5/odp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index be0cd358b080..4d7024b899cb 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -796,9 +796,6 @@ static int create_kernel_qp(struct mlx5_ib_dev *dev,
 		goto err_free;
 	}
 
-	qp->db.db[0] = 0;
-	qp->db.db[1] = 0;
-
 	qp->sq.wrid = kmalloc(qp->sq.wqe_cnt * sizeof(*qp->sq.wrid), GFP_KERNEL);
 	qp->sq.wr_data = kmalloc(qp->sq.wqe_cnt * sizeof(*qp->sq.wr_data), GFP_KERNEL);
 	qp->rq.wrid = kmalloc(qp->rq.wqe_cnt * sizeof(*qp->rq.wrid), GFP_KERNEL);
@@ -1162,10 +1159,11 @@ static void destroy_qp_common(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp)
 	in = kzalloc(sizeof(*in), GFP_KERNEL);
 	if (!in)
 		return;
+
 	if (qp->state != IB_QPS_RESET) {
 		mlx5_ib_qp_disable_pagefaults(qp);
 		if (mlx5_core_qp_modify(dev->mdev, to_mlx5_state(qp->state),
-					MLX5_QP_STATE_RST, in, sizeof(*in), &qp->mqp))
+					MLX5_QP_STATE_RST, in, 0, &qp->mqp))
 			mlx5_ib_warn(dev, "mlx5_ib: modify QP %06x to RESET failed\n",
 				     qp->mqp.qpn);
 	}
diff --git a/drivers/infiniband/hw/mlx5/srq.c b/drivers/infiniband/hw/mlx5/srq.c
index 41fec66217dd..02d77a29764d 100644
--- a/drivers/infiniband/hw/mlx5/srq.c
+++ b/drivers/infiniband/hw/mlx5/srq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -165,8 +165,6 @@ static int create_srq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_srq *srq,
 		return err;
 	}
 
-	*srq->db.db = 0;
-
 	if (mlx5_buf_alloc(dev->mdev, buf_size, PAGE_SIZE * 2, &srq->buf)) {
 		mlx5_ib_dbg(dev, "buf alloc failed\n");
 		err = -ENOMEM;
diff --git a/drivers/infiniband/hw/mlx5/user.h b/drivers/infiniband/hw/mlx5/user.h
index d0ba264ac1ed..76fb7b927d37 100644
--- a/drivers/infiniband/hw/mlx5/user.h
+++ b/drivers/infiniband/hw/mlx5/user.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/infiniband/hw/qib/qib_file_ops.c b/drivers/infiniband/hw/qib/qib_file_ops.c
index 41937c6f888a..9ea6c440a00c 100644
--- a/drivers/infiniband/hw/qib/qib_file_ops.c
+++ b/drivers/infiniband/hw/qib/qib_file_ops.c
@@ -39,11 +39,11 @@
 #include <linux/vmalloc.h>
 #include <linux/highmem.h>
 #include <linux/io.h>
-#include <linux/aio.h>
 #include <linux/jiffies.h>
 #include <asm/pgtable.h>
 #include <linux/delay.h>
 #include <linux/export.h>
+#include <linux/uio.h>
 
 #include "qib.h"
 #include "qib_common.h"
@@ -55,15 +55,19 @@
 static int qib_open(struct inode *, struct file *);
 static int qib_close(struct inode *, struct file *);
 static ssize_t qib_write(struct file *, const char __user *, size_t, loff_t *);
-static ssize_t qib_aio_write(struct kiocb *, const struct iovec *,
-			     unsigned long, loff_t);
+static ssize_t qib_write_iter(struct kiocb *, struct iov_iter *);
 static unsigned int qib_poll(struct file *, struct poll_table_struct *);
 static int qib_mmapf(struct file *, struct vm_area_struct *);
 
+/*
+ * This is really, really weird shit - write() and writev() here
+ * have completely unrelated semantics.  Sucky userland ABI,
+ * film at 11.
+ */
 static const struct file_operations qib_file_ops = {
 	.owner = THIS_MODULE,
 	.write = qib_write,
-	.aio_write = qib_aio_write,
+	.write_iter = qib_write_iter,
 	.open = qib_open,
 	.release = qib_close,
 	.poll = qib_poll,
@@ -2249,17 +2253,16 @@ bail:
 	return ret;
 }
 
-static ssize_t qib_aio_write(struct kiocb *iocb, const struct iovec *iov,
-			     unsigned long dim, loff_t off)
+static ssize_t qib_write_iter(struct kiocb *iocb, struct iov_iter *from)
 {
 	struct qib_filedata *fp = iocb->ki_filp->private_data;
 	struct qib_ctxtdata *rcd = ctxt_fp(iocb->ki_filp);
 	struct qib_user_sdma_queue *pq = fp->pq;
 
-	if (!dim || !pq)
+	if (!iter_is_iovec(from) || !from->nr_segs || !pq)
 		return -EINVAL;
-
-	return qib_user_sdma_writev(rcd, pq, iov, dim);
+			 
+	return qib_user_sdma_writev(rcd, pq, from->iov, from->nr_segs);
 }
 
 static struct class *qib_class;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 58b5aa3b6f2d..657b89b1d291 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -842,6 +842,13 @@ static void ipoib_set_mcast_list(struct net_device *dev)
 	queue_work(ipoib_workqueue, &priv->restart_task);
 }
 
+static int ipoib_get_iflink(const struct net_device *dev)
+{
+	struct ipoib_dev_priv *priv = netdev_priv(dev);
+
+	return priv->parent->ifindex;
+}
+
 static u32 ipoib_addr_hash(struct ipoib_neigh_hash *htbl, u8 *daddr)
 {
 	/*
@@ -1341,6 +1348,7 @@ static const struct net_device_ops ipoib_netdev_ops = {
 	.ndo_start_xmit	 	 = ipoib_start_xmit,
 	.ndo_tx_timeout		 = ipoib_timeout,
 	.ndo_set_rx_mode	 = ipoib_set_mcast_list,
+	.ndo_get_iflink		 = ipoib_get_iflink,
 };
 
 void ipoib_setup(struct net_device *dev)
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_vlan.c b/drivers/infiniband/ulp/ipoib/ipoib_vlan.c
index 9fad7b5ac8b9..4dd1313056a4 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_vlan.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_vlan.c
@@ -102,7 +102,6 @@ int __ipoib_vlan_add(struct ipoib_dev_priv *ppriv, struct ipoib_dev_priv *priv,
 	}
 
 	priv->child_type  = type;
-	priv->dev->iflink = ppriv->dev->ifindex;
 	list_add_tail(&priv->list, &ppriv->child_intfs);
 
 	return 0;
diff --git a/drivers/input/input-mt.c b/drivers/input/input-mt.c
index cb150a1dbaff..54fce56c8023 100644
--- a/drivers/input/input-mt.c
+++ b/drivers/input/input-mt.c
@@ -88,10 +88,13 @@ int input_mt_init_slots(struct input_dev *dev, unsigned int num_slots,
 			goto err_mem;
 	}
 
-	/* Mark slots as 'unused' */
+	/* Mark slots as 'inactive' */
 	for (i = 0; i < num_slots; i++)
 		input_mt_set_value(&mt->slots[i], ABS_MT_TRACKING_ID, -1);
 
+	/* Mark slots as 'unused' */
+	mt->frame = 1;
+
 	dev->mt = mt;
 	return 0;
 err_mem:
@@ -365,27 +368,35 @@ static void input_mt_set_slots(struct input_mt *mt,
 			       int *slots, int num_pos)
 {
 	struct input_mt_slot *s;
-	int *w = mt->red, *p;
+	int *w = mt->red, j;
 
-	for (p = slots; p != slots + num_pos; p++)
-		*p = -1;
+	for (j = 0; j != num_pos; j++)
+		slots[j] = -1;
 
 	for (s = mt->slots; s != mt->slots + mt->num_slots; s++) {
 		if (!input_mt_is_active(s))
 			continue;
-		for (p = slots; p != slots + num_pos; p++)
-			if (*w++ < 0)
-				*p = s - mt->slots;
+
+		for (j = 0; j != num_pos; j++) {
+			if (w[j] < 0) {
+				slots[j] = s - mt->slots;
+				break;
+			}
+		}
+
+		w += num_pos;
 	}
 
 	for (s = mt->slots; s != mt->slots + mt->num_slots; s++) {
 		if (input_mt_is_active(s))
 			continue;
-		for (p = slots; p != slots + num_pos; p++)
-			if (*p < 0) {
-				*p = s - mt->slots;
+
+		for (j = 0; j != num_pos; j++) {
+			if (slots[j] < 0) {
+				slots[j] = s - mt->slots;
 				break;
 			}
+		}
 	}
 }
 
@@ -439,6 +450,8 @@ EXPORT_SYMBOL(input_mt_assign_slots);
  * set the key on the first unused slot and return.
  *
  * If no available slot can be found, -1 is returned.
+ * Note that for this function to work properly, input_mt_sync_frame() has
+ * to be called at each frame.
  */
 int input_mt_get_slot_by_key(struct input_dev *dev, int key)
 {
@@ -453,7 +466,7 @@ int input_mt_get_slot_by_key(struct input_dev *dev, int key)
 			return s - mt->slots;
 
 	for (s = mt->slots; s != mt->slots + mt->num_slots; s++)
-		if (!input_mt_is_active(s)) {
+		if (!input_mt_is_active(s) && !input_mt_is_used(mt, s)) {
 			s->key = key;
 			return s - mt->slots;
 		}
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index a89ba7cb96f1..106fbac7f8c5 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -588,6 +588,16 @@ config KEYBOARD_DAVINCI
 	  To compile this driver as a module, choose M here: the
 	  module will be called davinci_keyscan.
 
+config KEYBOARD_IPAQ_MICRO
+	tristate "Buttons on Micro SoC (iPaq h3100,h3600,h3700)"
+	depends on MFD_IPAQ_MICRO
+	help
+	  Say Y to enable support for the buttons attached to
+	  Micro peripheral controller on iPAQ h3100/h3600/h3700
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ipaq-micro-keys.
+
 config KEYBOARD_OMAP
 	tristate "TI OMAP keypad support"
 	depends on ARCH_OMAP1
@@ -686,4 +696,15 @@ config KEYBOARD_CAP11XX
 	  To compile this driver as a module, choose M here: the
 	  module will be called cap11xx.
 
+config KEYBOARD_BCM
+	tristate "Broadcom keypad driver"
+	depends on OF && HAVE_CLK
+	select INPUT_MATRIXKMAP
+	default ARCH_BCM_CYGNUS
+	help
+	  Say Y here if you want to use Broadcom keypad.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bcm-keypad.
+
 endif
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 470767884bd8..df28d5553c05 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_KEYBOARD_ADP5589)		+= adp5589-keys.o
 obj-$(CONFIG_KEYBOARD_AMIGA)		+= amikbd.o
 obj-$(CONFIG_KEYBOARD_ATARI)		+= atakbd.o
 obj-$(CONFIG_KEYBOARD_ATKBD)		+= atkbd.o
+obj-$(CONFIG_KEYBOARD_BCM)		+= bcm-keypad.o
 obj-$(CONFIG_KEYBOARD_BFIN)		+= bf54x-keys.o
 obj-$(CONFIG_KEYBOARD_CAP11XX)		+= cap11xx.o
 obj-$(CONFIG_KEYBOARD_CLPS711X)		+= clps711x-keypad.o
@@ -23,6 +24,7 @@ obj-$(CONFIG_KEYBOARD_TCA6416)		+= tca6416-keypad.o
 obj-$(CONFIG_KEYBOARD_TCA8418)		+= tca8418_keypad.o
 obj-$(CONFIG_KEYBOARD_HIL)		+= hil_kbd.o
 obj-$(CONFIG_KEYBOARD_HIL_OLD)		+= hilkbd.o
+obj-$(CONFIG_KEYBOARD_IPAQ_MICRO)	+= ipaq-micro-keys.o
 obj-$(CONFIG_KEYBOARD_IMX)		+= imx_keypad.o
 obj-$(CONFIG_KEYBOARD_HP6XX)		+= jornada680_kbd.o
 obj-$(CONFIG_KEYBOARD_HP7XX)		+= jornada720_kbd.o
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index 387c51f4b4e4..ec876b5b1382 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -1653,6 +1653,12 @@ static int __init atkbd_deactivate_fixup(const struct dmi_system_id *id)
 	return 1;
 }
 
+/*
+ * NOTE: do not add any more "force release" quirks to this table.  The
+ * task of adjusting list of keys that should be "released" automatically
+ * by the driver is now delegated to userspace tools, such as udev, so
+ * submit such quirks there.
+ */
 static const struct dmi_system_id atkbd_dmi_quirk_table[] __initconst = {
 	{
 		.matches = {
diff --git a/drivers/input/keyboard/bcm-keypad.c b/drivers/input/keyboard/bcm-keypad.c
new file mode 100644
index 000000000000..86a8b723ae15
--- /dev/null
+++ b/drivers/input/keyboard/bcm-keypad.c
@@ -0,0 +1,458 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/gfp.h>
+#include <linux/io.h>
+#include <linux/input.h>
+#include <linux/input/matrix_keypad.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/stddef.h>
+#include <linux/types.h>
+
+#define DEFAULT_CLK_HZ			31250
+#define MAX_ROWS			8
+#define MAX_COLS			8
+
+/* Register/field definitions */
+#define KPCR_OFFSET			0x00000080
+#define KPCR_MODE			0x00000002
+#define KPCR_MODE_SHIFT			1
+#define KPCR_MODE_MASK			1
+#define KPCR_ENABLE			0x00000001
+#define KPCR_STATUSFILTERENABLE		0x00008000
+#define KPCR_STATUSFILTERTYPE_SHIFT	12
+#define KPCR_COLFILTERENABLE		0x00000800
+#define KPCR_COLFILTERTYPE_SHIFT	8
+#define KPCR_ROWWIDTH_SHIFT		20
+#define KPCR_COLUMNWIDTH_SHIFT		16
+
+#define KPIOR_OFFSET			0x00000084
+#define KPIOR_ROWOCONTRL_SHIFT		24
+#define KPIOR_ROWOCONTRL_MASK		0xFF000000
+#define KPIOR_COLUMNOCONTRL_SHIFT	16
+#define KPIOR_COLUMNOCONTRL_MASK	0x00FF0000
+#define KPIOR_COLUMN_IO_DATA_SHIFT	0
+
+#define KPEMR0_OFFSET			0x00000090
+#define KPEMR1_OFFSET			0x00000094
+#define KPEMR2_OFFSET			0x00000098
+#define KPEMR3_OFFSET			0x0000009C
+#define KPEMR_EDGETYPE_BOTH		3
+
+#define KPSSR0_OFFSET			0x000000A0
+#define KPSSR1_OFFSET			0x000000A4
+#define KPSSRN_OFFSET(reg_n)		(KPSSR0_OFFSET + 4 * (reg_n))
+#define KPIMR0_OFFSET			0x000000B0
+#define KPIMR1_OFFSET			0x000000B4
+#define KPICR0_OFFSET			0x000000B8
+#define KPICR1_OFFSET			0x000000BC
+#define KPICRN_OFFSET(reg_n)		(KPICR0_OFFSET + 4 * (reg_n))
+#define KPISR0_OFFSET			0x000000C0
+#define KPISR1_OFFSET			0x000000C4
+
+#define KPCR_STATUSFILTERTYPE_MAX	7
+#define KPCR_COLFILTERTYPE_MAX		7
+
+/* Macros to determine the row/column from a bit that is set in SSR0/1. */
+#define BIT_TO_ROW_SSRN(bit_nr, reg_n)	(((bit_nr) >> 3) + 4 * (reg_n))
+#define BIT_TO_COL(bit_nr)		((bit_nr) % 8)
+
+/* Structure representing various run-time entities */
+struct bcm_kp {
+	void __iomem *base;
+	int irq;
+	struct clk *clk;
+	struct input_dev *input_dev;
+	unsigned long last_state[2];
+	unsigned int n_rows;
+	unsigned int n_cols;
+	u32 kpcr;
+	u32 kpior;
+	u32 kpemr;
+	u32 imr0_val;
+	u32 imr1_val;
+};
+
+/*
+ * Returns the keycode from the input device keymap given the row and
+ * column.
+ */
+static int bcm_kp_get_keycode(struct bcm_kp *kp, int row, int col)
+{
+	unsigned int row_shift = get_count_order(kp->n_cols);
+	unsigned short *keymap = kp->input_dev->keycode;
+
+	return keymap[MATRIX_SCAN_CODE(row, col, row_shift)];
+}
+
+static void bcm_kp_report_keys(struct bcm_kp *kp, int reg_num, int pull_mode)
+{
+	unsigned long state, change;
+	int bit_nr;
+	int key_press;
+	int row, col;
+	unsigned int keycode;
+
+	/* Clear interrupts */
+	writel(0xFFFFFFFF, kp->base + KPICRN_OFFSET(reg_num));
+
+	state = readl(kp->base + KPSSRN_OFFSET(reg_num));
+	change = kp->last_state[reg_num] ^ state;
+	kp->last_state[reg_num] = state;
+
+	for_each_set_bit(bit_nr, &change, BITS_PER_LONG) {
+		key_press = state & BIT(bit_nr);
+		/* The meaning of SSR register depends on pull mode. */
+		key_press = pull_mode ? !key_press : key_press;
+		row = BIT_TO_ROW_SSRN(bit_nr, reg_num);
+		col = BIT_TO_COL(bit_nr);
+		keycode = bcm_kp_get_keycode(kp, row, col);
+		input_report_key(kp->input_dev, keycode, key_press);
+	}
+}
+
+static irqreturn_t bcm_kp_isr_thread(int irq, void *dev_id)
+{
+	struct bcm_kp *kp = dev_id;
+	int pull_mode = (kp->kpcr >> KPCR_MODE_SHIFT) & KPCR_MODE_MASK;
+	int reg_num;
+
+	for (reg_num = 0; reg_num <= 1; reg_num++)
+		bcm_kp_report_keys(kp, reg_num, pull_mode);
+
+	input_sync(kp->input_dev);
+
+	return IRQ_HANDLED;
+}
+
+static int bcm_kp_start(struct bcm_kp *kp)
+{
+	int error;
+
+	if (kp->clk) {
+		error = clk_prepare_enable(kp->clk);
+		if (error)
+			return error;
+	}
+
+	writel(kp->kpior, kp->base + KPIOR_OFFSET);
+
+	writel(kp->imr0_val, kp->base + KPIMR0_OFFSET);
+	writel(kp->imr1_val, kp->base + KPIMR1_OFFSET);
+
+	writel(kp->kpemr, kp->base + KPEMR0_OFFSET);
+	writel(kp->kpemr, kp->base + KPEMR1_OFFSET);
+	writel(kp->kpemr, kp->base + KPEMR2_OFFSET);
+	writel(kp->kpemr, kp->base + KPEMR3_OFFSET);
+
+	writel(0xFFFFFFFF, kp->base + KPICR0_OFFSET);
+	writel(0xFFFFFFFF, kp->base + KPICR1_OFFSET);
+
+	kp->last_state[0] = readl(kp->base + KPSSR0_OFFSET);
+	kp->last_state[0] = readl(kp->base + KPSSR1_OFFSET);
+
+	writel(kp->kpcr | KPCR_ENABLE, kp->base + KPCR_OFFSET);
+
+	return 0;
+}
+
+static void bcm_kp_stop(const struct bcm_kp *kp)
+{
+	u32 val;
+
+	val = readl(kp->base + KPCR_OFFSET);
+	val &= ~KPCR_ENABLE;
+	writel(0, kp->base + KPCR_OFFSET);
+	writel(0, kp->base + KPIMR0_OFFSET);
+	writel(0, kp->base + KPIMR1_OFFSET);
+	writel(0xFFFFFFFF, kp->base + KPICR0_OFFSET);
+	writel(0xFFFFFFFF, kp->base + KPICR1_OFFSET);
+
+	if (kp->clk)
+		clk_disable_unprepare(kp->clk);
+}
+
+static int bcm_kp_open(struct input_dev *dev)
+{
+	struct bcm_kp *kp = input_get_drvdata(dev);
+
+	return bcm_kp_start(kp);
+}
+
+static void bcm_kp_close(struct input_dev *dev)
+{
+	struct bcm_kp *kp = input_get_drvdata(dev);
+
+	bcm_kp_stop(kp);
+}
+
+static int bcm_kp_matrix_key_parse_dt(struct bcm_kp *kp)
+{
+	struct device *dev = kp->input_dev->dev.parent;
+	struct device_node *np = dev->of_node;
+	int error;
+	unsigned int dt_val;
+	unsigned int i;
+	unsigned int num_rows, col_mask, rows_set;
+
+	/* Initialize the KPCR Keypad Configuration Register */
+	kp->kpcr = KPCR_STATUSFILTERENABLE | KPCR_COLFILTERENABLE;
+
+	error = matrix_keypad_parse_of_params(dev, &kp->n_rows, &kp->n_cols);
+	if (error) {
+		dev_err(dev, "failed to parse kp params\n");
+		return error;
+	}
+
+	/* Set row width for the ASIC block. */
+	kp->kpcr |= (kp->n_rows - 1) << KPCR_ROWWIDTH_SHIFT;
+
+	/* Set column width for the ASIC block. */
+	kp->kpcr |= (kp->n_cols - 1) << KPCR_COLUMNWIDTH_SHIFT;
+
+	/* Configure the IMR registers */
+
+	/*
+	 * IMR registers contain interrupt enable bits for 8x8 matrix
+	 * IMR0 register format: <row3> <row2> <row1> <row0>
+	 * IMR1 register format: <row7> <row6> <row5> <row4>
+	 */
+	col_mask = (1 << (kp->n_cols)) - 1;
+	num_rows = kp->n_rows;
+
+	/* Set column bits in rows 0 to 3 in IMR0 */
+	kp->imr0_val = col_mask;
+
+	rows_set = 1;
+	while (--num_rows && rows_set++ < 4)
+		kp->imr0_val |= kp->imr0_val << MAX_COLS;
+
+	/* Set column bits in rows 4 to 7 in IMR1 */
+	kp->imr1_val = 0;
+	if (num_rows) {
+		kp->imr1_val = col_mask;
+		while (--num_rows)
+			kp->imr1_val |= kp->imr1_val << MAX_COLS;
+	}
+
+	/* Initialize the KPEMR Keypress Edge Mode Registers */
+	/* Trigger on both edges */
+	kp->kpemr = 0;
+	for (i = 0; i <= 30; i += 2)
+		kp->kpemr |= (KPEMR_EDGETYPE_BOTH << i);
+
+	/*
+	 * Obtain the Status filter debounce value and verify against the
+	 * possible values specified in the DT binding.
+	 */
+	of_property_read_u32(np, "status-debounce-filter-period", &dt_val);
+
+	if (dt_val > KPCR_STATUSFILTERTYPE_MAX) {
+		dev_err(dev, "Invalid Status filter debounce value %d\n",
+			dt_val);
+		return -EINVAL;
+	}
+
+	kp->kpcr |= dt_val << KPCR_STATUSFILTERTYPE_SHIFT;
+
+	/*
+	 * Obtain the Column filter debounce value and verify against the
+	 * possible values specified in the DT binding.
+	 */
+	of_property_read_u32(np, "col-debounce-filter-period", &dt_val);
+
+	if (dt_val > KPCR_COLFILTERTYPE_MAX) {
+		dev_err(dev, "Invalid Column filter debounce value %d\n",
+			dt_val);
+		return -EINVAL;
+	}
+
+	kp->kpcr |= dt_val << KPCR_COLFILTERTYPE_SHIFT;
+
+	/*
+	 * Determine between the row and column,
+	 * which should be configured as output.
+	 */
+	if (of_property_read_bool(np, "row-output-enabled")) {
+		/*
+		* Set RowOContrl or ColumnOContrl in KPIOR
+		* to the number of pins to drive as outputs
+		*/
+		kp->kpior = ((1 << kp->n_rows) - 1) <<
+				KPIOR_ROWOCONTRL_SHIFT;
+	} else {
+		kp->kpior = ((1 << kp->n_cols) - 1) <<
+				KPIOR_COLUMNOCONTRL_SHIFT;
+	}
+
+	/*
+	 * Determine if the scan pull up needs to be enabled
+	 */
+	if (of_property_read_bool(np, "pull-up-enabled"))
+		kp->kpcr |= KPCR_MODE;
+
+	dev_dbg(dev, "n_rows=%d n_col=%d kpcr=%x kpior=%x kpemr=%x\n",
+		kp->n_rows, kp->n_cols,
+		kp->kpcr, kp->kpior, kp->kpemr);
+
+	return 0;
+}
+
+
+static int bcm_kp_probe(struct platform_device *pdev)
+{
+	struct bcm_kp *kp;
+	struct input_dev *input_dev;
+	struct resource *res;
+	int error;
+
+	kp = devm_kzalloc(&pdev->dev, sizeof(*kp), GFP_KERNEL);
+	if (!kp)
+		return -ENOMEM;
+
+	input_dev = devm_input_allocate_device(&pdev->dev);
+	if (!input_dev) {
+		dev_err(&pdev->dev, "failed to allocate the input device\n");
+		return -ENOMEM;
+	}
+
+	__set_bit(EV_KEY, input_dev->evbit);
+
+	/* Enable auto repeat feature of Linux input subsystem */
+	if (of_property_read_bool(pdev->dev.of_node, "autorepeat"))
+		__set_bit(EV_REP, input_dev->evbit);
+
+	input_dev->name = pdev->name;
+	input_dev->phys = "keypad/input0";
+	input_dev->dev.parent = &pdev->dev;
+	input_dev->open = bcm_kp_open;
+	input_dev->close = bcm_kp_close;
+
+	input_dev->id.bustype = BUS_HOST;
+	input_dev->id.vendor = 0x0001;
+	input_dev->id.product = 0x0001;
+	input_dev->id.version = 0x0100;
+
+	input_set_drvdata(input_dev, kp);
+
+	kp->input_dev = input_dev;
+
+	platform_set_drvdata(pdev, kp);
+
+	error = bcm_kp_matrix_key_parse_dt(kp);
+	if (error)
+		return error;
+
+	error = matrix_keypad_build_keymap(NULL, NULL,
+					   kp->n_rows, kp->n_cols,
+					   NULL, input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		return error;
+	}
+
+	/* Get the KEYPAD base address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Missing keypad base address resource\n");
+		return -ENODEV;
+	}
+
+	kp->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(kp->base))
+		return PTR_ERR(kp->base);
+
+	/* Enable clock */
+	kp->clk = devm_clk_get(&pdev->dev, "peri_clk");
+	if (IS_ERR(kp->clk)) {
+		error = PTR_ERR(kp->clk);
+		if (error != -ENOENT) {
+			if (error != -EPROBE_DEFER)
+				dev_err(&pdev->dev, "Failed to get clock\n");
+			return error;
+		}
+		dev_dbg(&pdev->dev,
+			"No clock specified. Assuming it's enabled\n");
+		kp->clk = NULL;
+	} else {
+		unsigned int desired_rate;
+		long actual_rate;
+
+		error = of_property_read_u32(pdev->dev.of_node,
+					     "clock-frequency", &desired_rate);
+		if (error < 0)
+			desired_rate = DEFAULT_CLK_HZ;
+
+		actual_rate = clk_round_rate(kp->clk, desired_rate);
+		if (actual_rate <= 0)
+			return -EINVAL;
+
+		error = clk_set_rate(kp->clk, actual_rate);
+		if (error)
+			return error;
+
+		error = clk_prepare_enable(kp->clk);
+		if (error)
+			return error;
+	}
+
+	/* Put the kp into a known sane state */
+	bcm_kp_stop(kp);
+
+	kp->irq = platform_get_irq(pdev, 0);
+	if (kp->irq < 0) {
+		dev_err(&pdev->dev, "no IRQ specified\n");
+		return -EINVAL;
+	}
+
+	error = devm_request_threaded_irq(&pdev->dev, kp->irq,
+					  NULL, bcm_kp_isr_thread,
+					  IRQF_ONESHOT, pdev->name, kp);
+	if (error) {
+		dev_err(&pdev->dev, "failed to request IRQ\n");
+		return error;
+	}
+
+	error = input_register_device(input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		return error;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id bcm_kp_of_match[] = {
+	{ .compatible = "brcm,bcm-keypad" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, bcm_kp_of_match);
+
+static struct platform_driver bcm_kp_device_driver = {
+	.probe		= bcm_kp_probe,
+	.driver		= {
+		.name	= "bcm-keypad",
+		.of_match_table = of_match_ptr(bcm_kp_of_match),
+	}
+};
+
+module_platform_driver(bcm_kp_device_driver);
+
+MODULE_AUTHOR("Broadcom Corporation");
+MODULE_DESCRIPTION("BCM Keypad Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/keyboard/cros_ec_keyb.c b/drivers/input/keyboard/cros_ec_keyb.c
index ffa989f2c785..64b9b59ad4cb 100644
--- a/drivers/input/keyboard/cros_ec_keyb.c
+++ b/drivers/input/keyboard/cros_ec_keyb.c
@@ -338,7 +338,7 @@ static int cros_ec_keyb_resume(struct device *dev)
 	 * wake source (e.g. the lid is open and the user might press a key to
 	 * wake) then the key scan buffer should be preserved.
 	 */
-	if (ckdev->ec->was_wake_device)
+	if (!ckdev->ec->was_wake_device)
 		cros_ec_keyb_clear_keyboard(ckdev);
 
 	return 0;
diff --git a/drivers/input/keyboard/ipaq-micro-keys.c b/drivers/input/keyboard/ipaq-micro-keys.c
new file mode 100644
index 000000000000..602900d1f937
--- /dev/null
+++ b/drivers/input/keyboard/ipaq-micro-keys.c
@@ -0,0 +1,168 @@
+/*
+ * 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.
+ *
+ * h3600 atmel micro companion support, key subdevice
+ * based on previous kernel 2.4 version
+ * Author : Alessandro Gardich <gremlin@gremlin.it>
+ * Author : Linus Walleij <linus.walleij@linaro.org>
+ *
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/pm.h>
+#include <linux/sysctl.h>
+#include <linux/proc_fs.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/ipaq-micro.h>
+
+struct ipaq_micro_keys {
+	struct ipaq_micro *micro;
+	struct input_dev *input;
+	u16 *codes;
+};
+
+static const u16 micro_keycodes[] = {
+	KEY_RECORD,		/* 1:  Record button			*/
+	KEY_CALENDAR,		/* 2:  Calendar				*/
+	KEY_ADDRESSBOOK,	/* 3:  Contacts (looks like Outlook)	*/
+	KEY_MAIL,		/* 4:  Envelope (Q on older iPAQs)	*/
+	KEY_HOMEPAGE,		/* 5:  Start (looks like swoopy arrow)	*/
+	KEY_UP,			/* 6:  Up				*/
+	KEY_RIGHT,		/* 7:  Right				*/
+	KEY_LEFT,		/* 8:  Left				*/
+	KEY_DOWN,		/* 9:  Down				*/
+};
+
+static void micro_key_receive(void *data, int len, unsigned char *msg)
+{
+	struct ipaq_micro_keys *keys = data;
+	int key, down;
+
+	down = 0x80 & msg[0];
+	key  = 0x7f & msg[0];
+
+	if (key < ARRAY_SIZE(micro_keycodes)) {
+		input_report_key(keys->input, keys->codes[key], down);
+		input_sync(keys->input);
+	}
+}
+
+static void micro_key_start(struct ipaq_micro_keys *keys)
+{
+	spin_lock(&keys->micro->lock);
+	keys->micro->key = micro_key_receive;
+	keys->micro->key_data = keys;
+	spin_unlock(&keys->micro->lock);
+}
+
+static void micro_key_stop(struct ipaq_micro_keys *keys)
+{
+	spin_lock(&keys->micro->lock);
+	keys->micro->key = NULL;
+	keys->micro->key_data = NULL;
+	spin_unlock(&keys->micro->lock);
+}
+
+static int micro_key_open(struct input_dev *input)
+{
+	struct ipaq_micro_keys *keys = input_get_drvdata(input);
+
+	micro_key_start(keys);
+
+	return 0;
+}
+
+static void micro_key_close(struct input_dev *input)
+{
+	struct ipaq_micro_keys *keys = input_get_drvdata(input);
+
+	micro_key_stop(keys);
+}
+
+static int micro_key_probe(struct platform_device *pdev)
+{
+	struct ipaq_micro_keys *keys;
+	int error;
+	int i;
+
+	keys = devm_kzalloc(&pdev->dev, sizeof(*keys), GFP_KERNEL);
+	if (!keys)
+		return -ENOMEM;
+
+	keys->micro = dev_get_drvdata(pdev->dev.parent);
+
+	keys->input = devm_input_allocate_device(&pdev->dev);
+	if (!keys->input)
+		return -ENOMEM;
+
+	keys->input->keycodesize = sizeof(micro_keycodes[0]);
+	keys->input->keycodemax = ARRAY_SIZE(micro_keycodes);
+	keys->codes = devm_kmemdup(&pdev->dev, micro_keycodes,
+			   keys->input->keycodesize * keys->input->keycodemax,
+			   GFP_KERNEL);
+	keys->input->keycode = keys->codes;
+
+	__set_bit(EV_KEY, keys->input->evbit);
+	for (i = 0; i < ARRAY_SIZE(micro_keycodes); i++)
+		__set_bit(micro_keycodes[i], keys->input->keybit);
+
+	keys->input->name = "h3600 micro keys";
+	keys->input->open = micro_key_open;
+	keys->input->close = micro_key_close;
+	input_set_drvdata(keys->input, keys);
+
+	error = input_register_device(keys->input);
+	if (error)
+		return error;
+
+	platform_set_drvdata(pdev, keys);
+	return 0;
+}
+
+static int __maybe_unused micro_key_suspend(struct device *dev)
+{
+	struct ipaq_micro_keys *keys = dev_get_drvdata(dev);
+
+	micro_key_stop(keys);
+
+	return 0;
+}
+
+static int __maybe_unused micro_key_resume(struct device *dev)
+{
+	struct ipaq_micro_keys *keys = dev_get_drvdata(dev);
+	struct input_dev *input = keys->input;
+
+	mutex_lock(&input->mutex);
+
+	if (input->users)
+		micro_key_start(keys);
+
+	mutex_unlock(&input->mutex);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(micro_key_dev_pm_ops,
+			 micro_key_suspend, micro_key_resume);
+
+static struct platform_driver micro_key_device_driver = {
+	.driver = {
+		.name    = "ipaq-micro-keys",
+		.pm	= &micro_key_dev_pm_ops,
+	},
+	.probe   = micro_key_probe,
+};
+module_platform_driver(micro_key_device_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("driver for iPAQ Atmel micro keys");
+MODULE_ALIAS("platform:ipaq-micro-keys");
diff --git a/drivers/input/keyboard/tc3589x-keypad.c b/drivers/input/keyboard/tc3589x-keypad.c
index 563932500ff1..31c606a4dd31 100644
--- a/drivers/input/keyboard/tc3589x-keypad.c
+++ b/drivers/input/keyboard/tc3589x-keypad.c
@@ -70,6 +70,28 @@
 #define TC3589x_KBD_INT_CLR	0x1
 
 /**
+ * struct tc35893_keypad_platform_data - platform specific keypad data
+ * @keymap_data:        matrix scan code table for keycodes
+ * @krow:               mask for available rows, value is 0xFF
+ * @kcol:               mask for available columns, value is 0xFF
+ * @debounce_period:    platform specific debounce time
+ * @settle_time:        platform specific settle down time
+ * @irqtype:            type of interrupt, falling or rising edge
+ * @enable_wakeup:      specifies if keypad event can wake up system from sleep
+ * @no_autorepeat:      flag for auto repetition
+ */
+struct tc3589x_keypad_platform_data {
+	const struct matrix_keymap_data *keymap_data;
+	u8                      krow;
+	u8                      kcol;
+	u8                      debounce_period;
+	u8                      settle_time;
+	unsigned long           irqtype;
+	bool                    enable_wakeup;
+	bool                    no_autorepeat;
+};
+
+/**
  * struct tc_keypad - data structure used by keypad driver
  * @tc3589x:    pointer to tc35893
  * @input:      pointer to input device object
@@ -296,7 +318,6 @@ static void tc3589x_keypad_close(struct input_dev *input)
 	tc3589x_keypad_disable(keypad);
 }
 
-#ifdef CONFIG_OF
 static const struct tc3589x_keypad_platform_data *
 tc3589x_keypad_of_probe(struct device *dev)
 {
@@ -346,14 +367,6 @@ tc3589x_keypad_of_probe(struct device *dev)
 
 	return plat;
 }
-#else
-static inline const struct tc3589x_keypad_platform_data *
-tc3589x_keypad_of_probe(struct device *dev)
-{
-	return ERR_PTR(-ENODEV);
-}
-#endif
-
 
 static int tc3589x_keypad_probe(struct platform_device *pdev)
 {
@@ -363,13 +376,10 @@ static int tc3589x_keypad_probe(struct platform_device *pdev)
 	const struct tc3589x_keypad_platform_data *plat;
 	int error, irq;
 
-	plat = tc3589x->pdata->keypad;
-	if (!plat) {
-		plat = tc3589x_keypad_of_probe(&pdev->dev);
-		if (IS_ERR(plat)) {
-			dev_err(&pdev->dev, "invalid keypad platform data\n");
-			return PTR_ERR(plat);
-		}
+	plat = tc3589x_keypad_of_probe(&pdev->dev);
+	if (IS_ERR(plat)) {
+		dev_err(&pdev->dev, "invalid keypad platform data\n");
+		return PTR_ERR(plat);
 	}
 
 	irq = platform_get_irq(pdev, 0);
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 6deb8dae3205..4436ab1b9735 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -115,6 +115,18 @@ config INPUT_PCSPKR
 	  To compile this driver as a module, choose M here: the
 	  module will be called pcspkr.
 
+config INPUT_PM8941_PWRKEY
+	tristate "Qualcomm PM8941 power key support"
+	depends on MFD_SPMI_PMIC
+	help
+	  Say Y here if you want support for the power key usually found
+	  on boards using a Qualcomm PM8941 compatible PMIC.
+
+	  If unsure, say Y.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pm8941-pwrkey.
+
 config INPUT_PM8XXX_VIBRATOR
 	tristate "Qualcomm PM8XXX vibrator support"
 	depends on MFD_PM8XXX
@@ -165,6 +177,18 @@ config INPUT_MAX77693_HAPTIC
 	  To compile this driver as module, choose M here: the
 	  module will be called max77693-haptic.
 
+config INPUT_MAX77843_HAPTIC
+    tristate "MAXIM MAX77843 haptic controller support"
+    depends on MFD_MAX77843 && REGULATOR
+    select INPUT_FF_MEMLESS
+    help
+      This option enables support for the haptic controller on
+      MAXIM MAX77843 chip. The driver supports ff-memless interface
+      from input framework.
+
+      To compile this driver as module, choose M here: the
+      module will be called max77843-haptic.
+
 config INPUT_MAX8925_ONKEY
 	tristate "MAX8925 ONKEY support"
 	depends on MFD_MAX8925
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index 403a1a54a76c..78ba4c1b8532 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -39,6 +39,7 @@ obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)	+= keyspan_remote.o
 obj-$(CONFIG_INPUT_KXTJ9)		+= kxtj9.o
 obj-$(CONFIG_INPUT_M68K_BEEP)		+= m68kspkr.o
 obj-$(CONFIG_INPUT_MAX77693_HAPTIC)	+= max77693-haptic.o
+obj-$(CONFIG_INPUT_MAX77843_HAPTIC)	+= max77843-haptic.o
 obj-$(CONFIG_INPUT_MAX8925_ONKEY)	+= max8925_onkey.o
 obj-$(CONFIG_INPUT_MAX8997_HAPTIC)	+= max8997_haptic.o
 obj-$(CONFIG_INPUT_MC13783_PWRBUTTON)	+= mc13783-pwrbutton.o
@@ -49,6 +50,7 @@ obj-$(CONFIG_INPUT_PCAP)		+= pcap_keys.o
 obj-$(CONFIG_INPUT_PCF50633_PMU)	+= pcf50633-input.o
 obj-$(CONFIG_INPUT_PCF8574)		+= pcf8574_keypad.o
 obj-$(CONFIG_INPUT_PCSPKR)		+= pcspkr.o
+obj-$(CONFIG_INPUT_PM8941_PWRKEY)	+= pm8941-pwrkey.o
 obj-$(CONFIG_INPUT_PM8XXX_VIBRATOR)	+= pm8xxx-vibrator.o
 obj-$(CONFIG_INPUT_PMIC8XXX_PWRKEY)	+= pmic8xxx-pwrkey.o
 obj-$(CONFIG_INPUT_POWERMATE)		+= powermate.o
diff --git a/drivers/input/misc/max77843-haptic.c b/drivers/input/misc/max77843-haptic.c
new file mode 100644
index 000000000000..dccbb465a055
--- /dev/null
+++ b/drivers/input/misc/max77843-haptic.c
@@ -0,0 +1,358 @@
+/*
+ * MAXIM MAX77693 Haptic device driver
+ *
+ * Copyright (C) 2015 Samsung Electronics
+ * Author: Jaewon Kim <jaewon02.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 <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/mfd/max77843-private.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#define MAX_MAGNITUDE_SHIFT        16
+
+enum max77843_haptic_motor_type {
+	MAX77843_HAPTIC_ERM = 0,
+	MAX77843_HAPTIC_LRA,
+};
+
+enum max77843_haptic_pwm_divisor {
+	MAX77843_HAPTIC_PWM_DIVISOR_32 = 0,
+	MAX77843_HAPTIC_PWM_DIVISOR_64,
+	MAX77843_HAPTIC_PWM_DIVISOR_128,
+	MAX77843_HAPTIC_PWM_DIVISOR_256,
+};
+
+struct max77843_haptic {
+	struct regmap *regmap_haptic;
+	struct device *dev;
+	struct input_dev *input_dev;
+	struct pwm_device *pwm_dev;
+	struct regulator *motor_reg;
+	struct work_struct work;
+	struct mutex mutex;
+
+	unsigned int magnitude;
+	unsigned int pwm_duty;
+
+	bool active;
+	bool suspended;
+
+	enum max77843_haptic_motor_type type;
+	enum max77843_haptic_pwm_divisor pwm_divisor;
+};
+
+static int max77843_haptic_set_duty_cycle(struct max77843_haptic *haptic)
+{
+	int delta = (haptic->pwm_dev->period + haptic->pwm_duty) / 2;
+	int error;
+
+	error = pwm_config(haptic->pwm_dev, delta, haptic->pwm_dev->period);
+	if (error) {
+		dev_err(haptic->dev, "failed to configure pwm: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int max77843_haptic_bias(struct max77843_haptic *haptic, bool on)
+{
+	int error;
+
+	error = regmap_update_bits(haptic->regmap_haptic,
+				   MAX77843_SYS_REG_MAINCTRL1,
+				   MAX77843_MAINCTRL1_BIASEN_MASK,
+				   on << MAINCTRL1_BIASEN_SHIFT);
+	if (error) {
+		dev_err(haptic->dev, "failed to %s bias: %d\n",
+			on ? "enable" : "disable", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int max77843_haptic_config(struct max77843_haptic *haptic, bool enable)
+{
+	unsigned int value;
+	int error;
+
+	value = (haptic->type << MCONFIG_MODE_SHIFT) |
+		(enable << MCONFIG_MEN_SHIFT) |
+		(haptic->pwm_divisor << MCONFIG_PDIV_SHIFT);
+
+	error = regmap_write(haptic->regmap_haptic,
+			     MAX77843_HAP_REG_MCONFIG, value);
+	if (error) {
+		dev_err(haptic->dev,
+			"failed to update haptic config: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int max77843_haptic_enable(struct max77843_haptic *haptic)
+{
+	int error;
+
+	if (haptic->active)
+		return 0;
+
+	error = pwm_enable(haptic->pwm_dev);
+	if (error) {
+		dev_err(haptic->dev,
+			"failed to enable pwm device: %d\n", error);
+		return error;
+	}
+
+	error = max77843_haptic_config(haptic, true);
+	if (error)
+		goto err_config;
+
+	haptic->active = true;
+
+	return 0;
+
+err_config:
+	pwm_disable(haptic->pwm_dev);
+
+	return error;
+}
+
+static int max77843_haptic_disable(struct max77843_haptic *haptic)
+{
+	int error;
+
+	if (!haptic->active)
+		return 0;
+
+	error = max77843_haptic_config(haptic, false);
+	if (error)
+		return error;
+
+	pwm_disable(haptic->pwm_dev);
+
+	haptic->active = false;
+
+	return 0;
+}
+
+static void max77843_haptic_play_work(struct work_struct *work)
+{
+	struct max77843_haptic *haptic =
+			container_of(work, struct max77843_haptic, work);
+	int error;
+
+	mutex_lock(&haptic->mutex);
+
+	if (haptic->suspended)
+		goto out_unlock;
+
+	if (haptic->magnitude) {
+		error = max77843_haptic_set_duty_cycle(haptic);
+		if (error) {
+			dev_err(haptic->dev,
+				"failed to set duty cycle: %d\n", error);
+			goto out_unlock;
+		}
+
+		error = max77843_haptic_enable(haptic);
+		if (error)
+			dev_err(haptic->dev,
+				"cannot enable haptic: %d\n", error);
+	} else {
+		error = max77843_haptic_disable(haptic);
+		if (error)
+			dev_err(haptic->dev,
+				"cannot disable haptic: %d\n", error);
+	}
+
+out_unlock:
+	mutex_unlock(&haptic->mutex);
+}
+
+static int max77843_haptic_play_effect(struct input_dev *dev, void *data,
+		struct ff_effect *effect)
+{
+	struct max77843_haptic *haptic = input_get_drvdata(dev);
+	u64 period_mag_multi;
+
+	haptic->magnitude = effect->u.rumble.strong_magnitude;
+	if (!haptic->magnitude)
+		haptic->magnitude = effect->u.rumble.weak_magnitude;
+
+	period_mag_multi = (u64)haptic->pwm_dev->period * haptic->magnitude;
+	haptic->pwm_duty = (unsigned int)(period_mag_multi >>
+						MAX_MAGNITUDE_SHIFT);
+
+	schedule_work(&haptic->work);
+
+	return 0;
+}
+
+static int max77843_haptic_open(struct input_dev *dev)
+{
+	struct max77843_haptic *haptic = input_get_drvdata(dev);
+	int error;
+
+	error = max77843_haptic_bias(haptic, true);
+	if (error)
+		return error;
+
+	error = regulator_enable(haptic->motor_reg);
+	if (error) {
+		dev_err(haptic->dev,
+			"failed to enable regulator: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static void max77843_haptic_close(struct input_dev *dev)
+{
+	struct max77843_haptic *haptic = input_get_drvdata(dev);
+	int error;
+
+	cancel_work_sync(&haptic->work);
+	max77843_haptic_disable(haptic);
+
+	error = regulator_disable(haptic->motor_reg);
+	if (error)
+		dev_err(haptic->dev,
+			"failed to disable regulator: %d\n", error);
+
+	max77843_haptic_bias(haptic, false);
+}
+
+static int max77843_haptic_probe(struct platform_device *pdev)
+{
+	struct max77843 *max77843 = dev_get_drvdata(pdev->dev.parent);
+	struct max77843_haptic *haptic;
+	int error;
+
+	haptic = devm_kzalloc(&pdev->dev, sizeof(*haptic), GFP_KERNEL);
+	if (!haptic)
+		return -ENOMEM;
+
+	haptic->regmap_haptic = max77843->regmap;
+	haptic->dev = &pdev->dev;
+	haptic->type = MAX77843_HAPTIC_LRA;
+	haptic->pwm_divisor = MAX77843_HAPTIC_PWM_DIVISOR_128;
+
+	INIT_WORK(&haptic->work, max77843_haptic_play_work);
+	mutex_init(&haptic->mutex);
+
+	haptic->pwm_dev = devm_pwm_get(&pdev->dev, NULL);
+	if (IS_ERR(haptic->pwm_dev)) {
+		dev_err(&pdev->dev, "failed to get pwm device\n");
+		return PTR_ERR(haptic->pwm_dev);
+	}
+
+	haptic->motor_reg = devm_regulator_get_exclusive(&pdev->dev, "haptic");
+	if (IS_ERR(haptic->motor_reg)) {
+		dev_err(&pdev->dev, "failed to get regulator\n");
+		return PTR_ERR(haptic->motor_reg);
+	}
+
+	haptic->input_dev = devm_input_allocate_device(&pdev->dev);
+	if (!haptic->input_dev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	haptic->input_dev->name = "max77843-haptic";
+	haptic->input_dev->id.version = 1;
+	haptic->input_dev->dev.parent = &pdev->dev;
+	haptic->input_dev->open = max77843_haptic_open;
+	haptic->input_dev->close = max77843_haptic_close;
+	input_set_drvdata(haptic->input_dev, haptic);
+	input_set_capability(haptic->input_dev, EV_FF, FF_RUMBLE);
+
+	error = input_ff_create_memless(haptic->input_dev, NULL,
+			max77843_haptic_play_effect);
+	if (error) {
+		dev_err(&pdev->dev, "failed to create force-feedback\n");
+		return error;
+	}
+
+	error = input_register_device(haptic->input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		return error;
+	}
+
+	platform_set_drvdata(pdev, haptic);
+
+	return 0;
+}
+
+static int __maybe_unused max77843_haptic_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct max77843_haptic *haptic = platform_get_drvdata(pdev);
+	int error;
+
+	error = mutex_lock_interruptible(&haptic->mutex);
+	if (error)
+		return error;
+
+	max77843_haptic_disable(haptic);
+
+	haptic->suspended = true;
+
+	mutex_unlock(&haptic->mutex);
+
+	return 0;
+}
+
+static int __maybe_unused max77843_haptic_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct max77843_haptic *haptic = platform_get_drvdata(pdev);
+	unsigned int magnitude;
+
+	mutex_lock(&haptic->mutex);
+
+	haptic->suspended = false;
+
+	magnitude = ACCESS_ONCE(haptic->magnitude);
+	if (magnitude)
+		max77843_haptic_enable(haptic);
+
+	mutex_unlock(&haptic->mutex);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(max77843_haptic_pm_ops,
+		max77843_haptic_suspend, max77843_haptic_resume);
+
+static struct platform_driver max77843_haptic_driver = {
+	.driver		= {
+		.name	= "max77843-haptic",
+		.pm	= &max77843_haptic_pm_ops,
+	},
+	.probe		= max77843_haptic_probe,
+};
+module_platform_driver(max77843_haptic_driver);
+
+MODULE_AUTHOR("Jaewon Kim <jaewon02.kim@samsung.com>");
+MODULE_DESCRIPTION("MAXIM MAX77843 Haptic driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/mma8450.c b/drivers/input/misc/mma8450.c
index 98228773a111..19c73574458e 100644
--- a/drivers/input/misc/mma8450.c
+++ b/drivers/input/misc/mma8450.c
@@ -174,12 +174,13 @@ static int mma8450_probe(struct i2c_client *c,
 	struct mma8450 *m;
 	int err;
 
-	m = kzalloc(sizeof(struct mma8450), GFP_KERNEL);
-	idev = input_allocate_polled_device();
-	if (!m || !idev) {
-		err = -ENOMEM;
-		goto err_free_mem;
-	}
+	m = devm_kzalloc(&c->dev, sizeof(*m), GFP_KERNEL);
+	if (!m)
+		return -ENOMEM;
+
+	idev = devm_input_allocate_polled_device(&c->dev);
+	if (!idev)
+		return -ENOMEM;
 
 	m->client = c;
 	m->idev = idev;
@@ -187,7 +188,6 @@ 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;
@@ -202,29 +202,12 @@ static int mma8450_probe(struct i2c_client *c,
 	err = input_register_polled_device(idev);
 	if (err) {
 		dev_err(&c->dev, "failed to register polled input device\n");
-		goto err_free_mem;
+		return err;
 	}
 
 	i2c_set_clientdata(c, m);
 
 	return 0;
-
-err_free_mem:
-	input_free_polled_device(idev);
-	kfree(m);
-	return err;
-}
-
-static int mma8450_remove(struct i2c_client *c)
-{
-	struct mma8450 *m = i2c_get_clientdata(c);
-	struct input_polled_dev *idev = m->idev;
-
-	input_unregister_polled_device(idev);
-	input_free_polled_device(idev);
-	kfree(m);
-
-	return 0;
 }
 
 static const struct i2c_device_id mma8450_id[] = {
@@ -242,11 +225,9 @@ MODULE_DEVICE_TABLE(of, mma8450_dt_ids);
 static struct i2c_driver mma8450_driver = {
 	.driver = {
 		.name	= MMA8450_DRV_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = mma8450_dt_ids,
 	},
 	.probe		= mma8450_probe,
-	.remove		= mma8450_remove,
 	.id_table	= mma8450_id,
 };
 
diff --git a/drivers/input/misc/palmas-pwrbutton.c b/drivers/input/misc/palmas-pwrbutton.c
index 1f9b5ee92746..1e1baed63929 100644
--- a/drivers/input/misc/palmas-pwrbutton.c
+++ b/drivers/input/misc/palmas-pwrbutton.c
@@ -304,7 +304,7 @@ static SIMPLE_DEV_PM_OPS(palmas_pwron_pm,
 			 palmas_pwron_suspend, palmas_pwron_resume);
 
 #ifdef CONFIG_OF
-static struct of_device_id of_palmas_pwr_match[] = {
+static const struct of_device_id of_palmas_pwr_match[] = {
 	{ .compatible = "ti,palmas-pwrbutton" },
 	{ },
 };
diff --git a/drivers/input/misc/pm8941-pwrkey.c b/drivers/input/misc/pm8941-pwrkey.c
new file mode 100644
index 000000000000..867db8a91372
--- /dev/null
+++ b/drivers/input/misc/pm8941-pwrkey.c
@@ -0,0 +1,293 @@
+/*
+ * Copyright (c) 2010-2011, Code Aurora Forum. All rights reserved.
+ * Copyright (c) 2014, Sony Mobile Communications Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/log2.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+
+#define PON_REV2			0x01
+
+#define PON_RT_STS			0x10
+#define  PON_KPDPWR_N_SET		BIT(0)
+
+#define PON_PS_HOLD_RST_CTL		0x5a
+#define PON_PS_HOLD_RST_CTL2		0x5b
+#define  PON_PS_HOLD_ENABLE		BIT(7)
+#define  PON_PS_HOLD_TYPE_MASK		0x0f
+#define  PON_PS_HOLD_TYPE_SHUTDOWN	4
+#define  PON_PS_HOLD_TYPE_HARD_RESET	7
+
+#define PON_PULL_CTL			0x70
+#define  PON_KPDPWR_PULL_UP		BIT(1)
+
+#define PON_DBC_CTL			0x71
+#define  PON_DBC_DELAY_MASK		0x7
+
+
+struct pm8941_pwrkey {
+	struct device *dev;
+	int irq;
+	u32 baseaddr;
+	struct regmap *regmap;
+	struct input_dev *input;
+
+	unsigned int revision;
+	struct notifier_block reboot_notifier;
+};
+
+static int pm8941_reboot_notify(struct notifier_block *nb,
+				unsigned long code, void *unused)
+{
+	struct pm8941_pwrkey *pwrkey = container_of(nb, struct pm8941_pwrkey,
+						    reboot_notifier);
+	unsigned int enable_reg;
+	unsigned int reset_type;
+	int error;
+
+	/* PMICs with revision 0 have the enable bit in same register as ctrl */
+	if (pwrkey->revision == 0)
+		enable_reg = PON_PS_HOLD_RST_CTL;
+	else
+		enable_reg = PON_PS_HOLD_RST_CTL2;
+
+	error = regmap_update_bits(pwrkey->regmap,
+				   pwrkey->baseaddr + enable_reg,
+				   PON_PS_HOLD_ENABLE,
+				   0);
+	if (error)
+		dev_err(pwrkey->dev,
+			"unable to clear ps hold reset enable: %d\n",
+			error);
+
+	/*
+	 * Updates of PON_PS_HOLD_ENABLE requires 3 sleep cycles between
+	 * writes.
+	 */
+	usleep_range(100, 1000);
+
+	switch (code) {
+	case SYS_HALT:
+	case SYS_POWER_OFF:
+		reset_type = PON_PS_HOLD_TYPE_SHUTDOWN;
+		break;
+	case SYS_RESTART:
+	default:
+		reset_type = PON_PS_HOLD_TYPE_HARD_RESET;
+		break;
+	};
+
+	error = regmap_update_bits(pwrkey->regmap,
+				   pwrkey->baseaddr + PON_PS_HOLD_RST_CTL,
+				   PON_PS_HOLD_TYPE_MASK,
+				   reset_type);
+	if (error)
+		dev_err(pwrkey->dev, "unable to set ps hold reset type: %d\n",
+			error);
+
+	error = regmap_update_bits(pwrkey->regmap,
+				   pwrkey->baseaddr + enable_reg,
+				   PON_PS_HOLD_ENABLE,
+				   PON_PS_HOLD_ENABLE);
+	if (error)
+		dev_err(pwrkey->dev, "unable to re-set enable: %d\n", error);
+
+	return NOTIFY_DONE;
+}
+
+static irqreturn_t pm8941_pwrkey_irq(int irq, void *_data)
+{
+	struct pm8941_pwrkey *pwrkey = _data;
+	unsigned int sts;
+	int error;
+
+	error = regmap_read(pwrkey->regmap,
+			    pwrkey->baseaddr + PON_RT_STS, &sts);
+	if (error)
+		return IRQ_HANDLED;
+
+	input_report_key(pwrkey->input, KEY_POWER, !!(sts & PON_KPDPWR_N_SET));
+	input_sync(pwrkey->input);
+
+	return IRQ_HANDLED;
+}
+
+static int __maybe_unused pm8941_pwrkey_suspend(struct device *dev)
+{
+	struct pm8941_pwrkey *pwrkey = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(pwrkey->irq);
+
+	return 0;
+}
+
+static int __maybe_unused pm8941_pwrkey_resume(struct device *dev)
+{
+	struct pm8941_pwrkey *pwrkey = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(pwrkey->irq);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(pm8941_pwr_key_pm_ops,
+			 pm8941_pwrkey_suspend, pm8941_pwrkey_resume);
+
+static int pm8941_pwrkey_probe(struct platform_device *pdev)
+{
+	struct pm8941_pwrkey *pwrkey;
+	bool pull_up;
+	u32 req_delay;
+	int error;
+
+	if (of_property_read_u32(pdev->dev.of_node, "debounce", &req_delay))
+		req_delay = 15625;
+
+	if (req_delay > 2000000 || req_delay == 0) {
+		dev_err(&pdev->dev, "invalid debounce time: %u\n", req_delay);
+		return -EINVAL;
+	}
+
+	pull_up = of_property_read_bool(pdev->dev.of_node, "bias-pull-up");
+
+	pwrkey = devm_kzalloc(&pdev->dev, sizeof(*pwrkey), GFP_KERNEL);
+	if (!pwrkey)
+		return -ENOMEM;
+
+	pwrkey->dev = &pdev->dev;
+
+	pwrkey->regmap = dev_get_regmap(pdev->dev.parent, NULL);
+	if (!pwrkey->regmap) {
+		dev_err(&pdev->dev, "failed to locate regmap\n");
+		return -ENODEV;
+	}
+
+	pwrkey->irq = platform_get_irq(pdev, 0);
+	if (pwrkey->irq < 0) {
+		dev_err(&pdev->dev, "failed to get irq\n");
+		return pwrkey->irq;
+	}
+
+	error = of_property_read_u32(pdev->dev.of_node, "reg",
+				     &pwrkey->baseaddr);
+	if (error)
+		return error;
+
+	error = regmap_read(pwrkey->regmap, pwrkey->baseaddr + PON_REV2,
+			    &pwrkey->revision);
+	if (error) {
+		dev_err(&pdev->dev, "failed to set debounce: %d\n", error);
+		return error;
+	}
+
+	pwrkey->input = devm_input_allocate_device(&pdev->dev);
+	if (!pwrkey->input) {
+		dev_dbg(&pdev->dev, "unable to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	input_set_capability(pwrkey->input, EV_KEY, KEY_POWER);
+
+	pwrkey->input->name = "pm8941_pwrkey";
+	pwrkey->input->phys = "pm8941_pwrkey/input0";
+
+	req_delay = (req_delay << 6) / USEC_PER_SEC;
+	req_delay = ilog2(req_delay);
+
+	error = regmap_update_bits(pwrkey->regmap,
+				   pwrkey->baseaddr + PON_DBC_CTL,
+				   PON_DBC_DELAY_MASK,
+				   req_delay);
+	if (error) {
+		dev_err(&pdev->dev, "failed to set debounce: %d\n", error);
+		return error;
+	}
+
+	error = regmap_update_bits(pwrkey->regmap,
+				   pwrkey->baseaddr + PON_PULL_CTL,
+				   PON_KPDPWR_PULL_UP,
+				   pull_up ? PON_KPDPWR_PULL_UP : 0);
+	if (error) {
+		dev_err(&pdev->dev, "failed to set pull: %d\n", error);
+		return error;
+	}
+
+	error = devm_request_threaded_irq(&pdev->dev, pwrkey->irq,
+					  NULL, pm8941_pwrkey_irq,
+					  IRQF_ONESHOT,
+					  "pm8941_pwrkey", pwrkey);
+	if (error) {
+		dev_err(&pdev->dev, "failed requesting IRQ: %d\n", error);
+		return error;
+	}
+
+	error = input_register_device(pwrkey->input);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register input device: %d\n",
+			error);
+		return error;
+	}
+
+	pwrkey->reboot_notifier.notifier_call = pm8941_reboot_notify,
+	error = register_reboot_notifier(&pwrkey->reboot_notifier);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register reboot notifier: %d\n",
+			error);
+		return error;
+	}
+
+	platform_set_drvdata(pdev, pwrkey);
+	device_init_wakeup(&pdev->dev, 1);
+
+	return 0;
+}
+
+static int pm8941_pwrkey_remove(struct platform_device *pdev)
+{
+	struct pm8941_pwrkey *pwrkey = platform_get_drvdata(pdev);
+
+	device_init_wakeup(&pdev->dev, 0);
+	unregister_reboot_notifier(&pwrkey->reboot_notifier);
+
+	return 0;
+}
+
+static const struct of_device_id pm8941_pwr_key_id_table[] = {
+	{ .compatible = "qcom,pm8941-pwrkey" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, pm8941_pwr_key_id_table);
+
+static struct platform_driver pm8941_pwrkey_driver = {
+	.probe = pm8941_pwrkey_probe,
+	.remove = pm8941_pwrkey_remove,
+	.driver = {
+		.name = "pm8941-pwrkey",
+		.pm = &pm8941_pwr_key_pm_ops,
+		.of_match_table = of_match_ptr(pm8941_pwr_key_id_table),
+	},
+};
+module_platform_driver(pm8941_pwrkey_driver);
+
+MODULE_DESCRIPTION("PM8941 Power Key driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/misc/pwm-beeper.c b/drivers/input/misc/pwm-beeper.c
index a28ee70ff158..e82edf810d1f 100644
--- a/drivers/input/misc/pwm-beeper.c
+++ b/drivers/input/misc/pwm-beeper.c
@@ -50,7 +50,6 @@ static int pwm_beeper_event(struct input_dev *input,
 	}
 
 	if (value == 0) {
-		pwm_config(beeper->pwm, 0, 0);
 		pwm_disable(beeper->pwm);
 	} else {
 		period = HZ_TO_NANOSECONDS(value);
@@ -169,12 +168,6 @@ static int __maybe_unused pwm_beeper_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(pwm_beeper_pm_ops,
 			 pwm_beeper_suspend, pwm_beeper_resume);
 
-#ifdef CONFIG_PM_SLEEP
-#define PWM_BEEPER_PM_OPS (&pwm_beeper_pm_ops)
-#else
-#define PWM_BEEPER_PM_OPS NULL
-#endif
-
 #ifdef CONFIG_OF
 static const struct of_device_id pwm_beeper_match[] = {
 	{ .compatible = "pwm-beeper", },
@@ -187,7 +180,7 @@ static struct platform_driver pwm_beeper_driver = {
 	.remove = pwm_beeper_remove,
 	.driver = {
 		.name	= "pwm-beeper",
-		.pm	= PWM_BEEPER_PM_OPS,
+		.pm	= &pwm_beeper_pm_ops,
 		.of_match_table = of_match_ptr(pwm_beeper_match),
 	},
 };
diff --git a/drivers/input/misc/regulator-haptic.c b/drivers/input/misc/regulator-haptic.c
index 132eb914ea3e..6bf3f1082f71 100644
--- a/drivers/input/misc/regulator-haptic.c
+++ b/drivers/input/misc/regulator-haptic.c
@@ -245,7 +245,7 @@ static int __maybe_unused regulator_haptic_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(regulator_haptic_pm_ops,
 		regulator_haptic_suspend, regulator_haptic_resume);
 
-static struct of_device_id regulator_haptic_dt_match[] = {
+static const struct of_device_id regulator_haptic_dt_match[] = {
 	{ .compatible = "regulator-haptic" },
 	{ /* sentinel */ },
 };
diff --git a/drivers/input/misc/tps65218-pwrbutton.c b/drivers/input/misc/tps65218-pwrbutton.c
index 54508dec4eb3..a39b62651a4b 100644
--- a/drivers/input/misc/tps65218-pwrbutton.c
+++ b/drivers/input/misc/tps65218-pwrbutton.c
@@ -106,7 +106,7 @@ static int tps65218_pwron_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id of_tps65218_pwr_match[] = {
+static const struct of_device_id of_tps65218_pwr_match[] = {
 	{ .compatible = "ti,tps65218-pwrbutton" },
 	{ },
 };
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 1bd15ebc01f2..e6708f6efb4d 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -153,10 +153,18 @@ static const struct alps_protocol_info alps_v7_protocol_data = {
 	ALPS_PROTO_V7, 0x48, 0x48, ALPS_DUALPOINT
 };
 
+static const struct alps_protocol_info alps_v8_protocol_data = {
+	ALPS_PROTO_V8, 0x18, 0x18, 0
+};
+
 static void alps_set_abs_params_st(struct alps_data *priv,
 				   struct input_dev *dev1);
 static void alps_set_abs_params_mt(struct alps_data *priv,
 				   struct input_dev *dev1);
+static void alps_set_abs_params_v7(struct alps_data *priv,
+				   struct input_dev *dev1);
+static void alps_set_abs_params_ss4_v2(struct alps_data *priv,
+				       struct input_dev *dev1);
 
 /* Packet formats are described in Documentation/input/alps.txt */
 
@@ -243,6 +251,14 @@ static void alps_process_packet_v1_v2(struct psmouse *psmouse)
 		return;
 	}
 
+	/* Non interleaved V2 dualpoint has separate stick button bits */
+	if (priv->proto_version == ALPS_PROTO_V2 &&
+	    priv->flags == (ALPS_PASS | ALPS_DUALPOINT)) {
+		left |= packet[0] & 1;
+		right |= packet[0] & 2;
+		middle |= packet[0] & 4;
+	}
+
 	alps_report_buttons(dev, dev2, left, right, middle);
 
 	/* Convert hardware tap to a reasonable Z value */
@@ -1085,6 +1101,176 @@ static void alps_process_packet_v7(struct psmouse *psmouse)
 		alps_process_touchpad_packet_v7(psmouse);
 }
 
+static unsigned char alps_get_pkt_id_ss4_v2(unsigned char *byte)
+{
+	unsigned char pkt_id = SS4_PACKET_ID_IDLE;
+
+	if (byte[0] == 0x18 && byte[1] == 0x10 && byte[2] == 0x00 &&
+	    (byte[3] & 0x88) == 0x08 && byte[4] == 0x10 && byte[5] == 0x00) {
+		pkt_id = SS4_PACKET_ID_IDLE;
+	} else if (!(byte[3] & 0x10)) {
+		pkt_id = SS4_PACKET_ID_ONE;
+	} else if (!(byte[3] & 0x20)) {
+		pkt_id = SS4_PACKET_ID_TWO;
+	} else {
+		pkt_id = SS4_PACKET_ID_MULTI;
+	}
+
+	return pkt_id;
+}
+
+static int alps_decode_ss4_v2(struct alps_fields *f,
+			      unsigned char *p, struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	unsigned char pkt_id;
+	unsigned int no_data_x, no_data_y;
+
+	pkt_id = alps_get_pkt_id_ss4_v2(p);
+
+	/* Current packet is 1Finger coordinate packet */
+	switch (pkt_id) {
+	case SS4_PACKET_ID_ONE:
+		f->mt[0].x = SS4_1F_X_V2(p);
+		f->mt[0].y = SS4_1F_Y_V2(p);
+		f->pressure = ((SS4_1F_Z_V2(p)) * 2) & 0x7f;
+		f->fingers = 1;
+		f->first_mp = 0;
+		f->is_mp = 0;
+		break;
+
+	case SS4_PACKET_ID_TWO:
+		if (priv->flags & ALPS_BUTTONPAD) {
+			f->mt[0].x = SS4_BTL_MF_X_V2(p, 0);
+			f->mt[0].y = SS4_BTL_MF_Y_V2(p, 0);
+			f->mt[1].x = SS4_BTL_MF_X_V2(p, 1);
+			f->mt[1].y = SS4_BTL_MF_Y_V2(p, 1);
+		} else {
+			f->mt[0].x = SS4_STD_MF_X_V2(p, 0);
+			f->mt[0].y = SS4_STD_MF_Y_V2(p, 0);
+			f->mt[1].x = SS4_STD_MF_X_V2(p, 1);
+			f->mt[1].y = SS4_STD_MF_Y_V2(p, 1);
+		}
+		f->pressure = SS4_MF_Z_V2(p, 0) ? 0x30 : 0;
+
+		if (SS4_IS_MF_CONTINUE(p)) {
+			f->first_mp = 1;
+		} else {
+			f->fingers = 2;
+			f->first_mp = 0;
+		}
+		f->is_mp = 0;
+
+		break;
+
+	case SS4_PACKET_ID_MULTI:
+		if (priv->flags & ALPS_BUTTONPAD) {
+			f->mt[2].x = SS4_BTL_MF_X_V2(p, 0);
+			f->mt[2].y = SS4_BTL_MF_Y_V2(p, 0);
+			f->mt[3].x = SS4_BTL_MF_X_V2(p, 1);
+			f->mt[3].y = SS4_BTL_MF_Y_V2(p, 1);
+			no_data_x = SS4_MFPACKET_NO_AX_BL;
+			no_data_y = SS4_MFPACKET_NO_AY_BL;
+		} else {
+			f->mt[2].x = SS4_STD_MF_X_V2(p, 0);
+			f->mt[2].y = SS4_STD_MF_Y_V2(p, 0);
+			f->mt[3].x = SS4_STD_MF_X_V2(p, 1);
+			f->mt[3].y = SS4_STD_MF_Y_V2(p, 1);
+			no_data_x = SS4_MFPACKET_NO_AX;
+			no_data_y = SS4_MFPACKET_NO_AY;
+		}
+
+		f->first_mp = 0;
+		f->is_mp = 1;
+
+		if (SS4_IS_5F_DETECTED(p)) {
+			f->fingers = 5;
+		} else if (f->mt[3].x == no_data_x &&
+			     f->mt[3].y == no_data_y) {
+			f->mt[3].x = 0;
+			f->mt[3].y = 0;
+			f->fingers = 3;
+		} else {
+			f->fingers = 4;
+		}
+		break;
+
+	case SS4_PACKET_ID_IDLE:
+	default:
+		memset(f, 0, sizeof(struct alps_fields));
+		break;
+	}
+
+	f->left = !!(SS4_BTN_V2(p) & 0x01);
+	if (!(priv->flags & ALPS_BUTTONPAD)) {
+		f->right = !!(SS4_BTN_V2(p) & 0x02);
+		f->middle = !!(SS4_BTN_V2(p) & 0x04);
+	}
+
+	return 0;
+}
+
+static void alps_process_packet_ss4_v2(struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	unsigned char *packet = psmouse->packet;
+	struct input_dev *dev = psmouse->dev;
+	struct alps_fields *f = &priv->f;
+
+	memset(f, 0, sizeof(struct alps_fields));
+	priv->decode_fields(f, packet, psmouse);
+	if (priv->multi_packet) {
+		/*
+		 * Sometimes the first packet will indicate a multi-packet
+		 * sequence, but sometimes the next multi-packet would not
+		 * come. Check for this, and when it happens process the
+		 * position packet as usual.
+		 */
+		if (f->is_mp) {
+			/* Now process the 1st packet */
+			priv->decode_fields(f, priv->multi_data, psmouse);
+		} else {
+			priv->multi_packet = 0;
+		}
+	}
+
+	/*
+	 * "f.is_mp" would always be '0' after merging the 1st and 2nd packet.
+	 * When it is set, it means 2nd packet comes without 1st packet come.
+	 */
+	if (f->is_mp)
+		return;
+
+	/* Save the first packet */
+	if (!priv->multi_packet && f->first_mp) {
+		priv->multi_packet = 1;
+		memcpy(priv->multi_data, packet, sizeof(priv->multi_data));
+		return;
+	}
+
+	priv->multi_packet = 0;
+
+	alps_report_mt_data(psmouse, (f->fingers <= 4) ? f->fingers : 4);
+
+	input_mt_report_finger_count(dev, f->fingers);
+
+	input_report_key(dev, BTN_LEFT, f->left);
+	input_report_key(dev, BTN_RIGHT, f->right);
+	input_report_key(dev, BTN_MIDDLE, f->middle);
+
+	input_report_abs(dev, ABS_PRESSURE, f->pressure);
+	input_sync(dev);
+}
+
+static bool alps_is_valid_package_ss4_v2(struct psmouse *psmouse)
+{
+	if (psmouse->pktcnt == 4 && ((psmouse->packet[3] & 0x08) != 0x08))
+		return false;
+	if (psmouse->pktcnt == 6 && ((psmouse->packet[5] & 0x10) != 0x0))
+		return false;
+	return true;
+}
+
 static DEFINE_MUTEX(alps_mutex);
 
 static void alps_register_bare_ps2_mouse(struct work_struct *work)
@@ -1154,12 +1340,31 @@ 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, *dev2 = NULL;
+
+	/* 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;
+		dev2 = psmouse->dev;
+	} 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,
+		alps_report_buttons(dev, dev2,
 				packet[0] & 1, packet[0] & 2, packet[0] & 4);
 
 	input_report_rel(dev, REL_X,
@@ -1232,8 +1437,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,
@@ -1287,20 +1492,15 @@ static psmouse_ret_t alps_process_byte(struct psmouse *psmouse)
 	 * a device connected to the external PS/2 port. Because bare PS/2
 	 * protocol does not have enough constant bits to self-synchronize
 	 * properly we only do this if the device is fully synchronized.
+	 * Can not distinguish V8's first byte from PS/2 packet's
 	 */
-	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 (priv->proto_version != ALPS_PROTO_V8 &&
+	    !psmouse->out_of_sync_cnt &&
+	    (psmouse->packet[0] & 0xc8) == 0x08) {
 
 		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;
@@ -1345,8 +1545,10 @@ static psmouse_ret_t alps_process_byte(struct psmouse *psmouse)
 		return PSMOUSE_BAD_DATA;
 	}
 
-	if (priv->proto_version == ALPS_PROTO_V7 &&
-	    !alps_is_valid_package_v7(psmouse)) {
+	if ((priv->proto_version == ALPS_PROTO_V7 &&
+			!alps_is_valid_package_v7(psmouse)) ||
+	    (priv->proto_version == ALPS_PROTO_V8 &&
+			!alps_is_valid_package_ss4_v2(psmouse))) {
 		psmouse_dbg(psmouse, "refusing packet[%i] = %x\n",
 			    psmouse->pktcnt - 1,
 			    psmouse->packet[psmouse->pktcnt - 1]);
@@ -2121,6 +2323,88 @@ error:
 	return -1;
 }
 
+static int alps_get_otp_values_ss4_v2(struct psmouse *psmouse,
+				      unsigned char index, unsigned char otp[])
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+
+	switch (index) {
+	case 0:
+		if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSTREAM)  ||
+		    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSTREAM)  ||
+		    ps2_command(ps2dev, otp, PSMOUSE_CMD_GETINFO))
+			return -1;
+
+		break;
+
+	case 1:
+		if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETPOLL)  ||
+		    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETPOLL)  ||
+		    ps2_command(ps2dev, otp, PSMOUSE_CMD_GETINFO))
+			return -1;
+
+		break;
+	}
+
+	return 0;
+}
+
+static int alps_update_device_area_ss4_v2(unsigned char otp[][4],
+					  struct alps_data *priv)
+{
+	int num_x_electrode;
+	int num_y_electrode;
+	int x_pitch, y_pitch, x_phys, y_phys;
+
+	num_x_electrode = SS4_NUMSENSOR_XOFFSET + (otp[1][0] & 0x0F);
+	num_y_electrode = SS4_NUMSENSOR_YOFFSET + ((otp[1][0] >> 4) & 0x0F);
+
+	priv->x_max = (num_x_electrode - 1) * SS4_COUNT_PER_ELECTRODE;
+	priv->y_max = (num_y_electrode - 1) * SS4_COUNT_PER_ELECTRODE;
+
+	x_pitch = ((otp[1][2] >> 2) & 0x07) + SS4_MIN_PITCH_MM;
+	y_pitch = ((otp[1][2] >> 5) & 0x07) + SS4_MIN_PITCH_MM;
+
+	x_phys = x_pitch * (num_x_electrode - 1); /* In 0.1 mm units */
+	y_phys = y_pitch * (num_y_electrode - 1); /* In 0.1 mm units */
+
+	priv->x_res = priv->x_max * 10 / x_phys; /* units / mm */
+	priv->y_res = priv->y_max * 10 / y_phys; /* units / mm */
+
+	return 0;
+}
+
+static int alps_update_btn_info_ss4_v2(unsigned char otp[][4],
+				       struct alps_data *priv)
+{
+	unsigned char is_btnless;
+
+	is_btnless = (otp[1][1] >> 3) & 0x01;
+
+	if (is_btnless)
+		priv->flags |= ALPS_BUTTONPAD;
+
+	return 0;
+}
+
+static int alps_set_defaults_ss4_v2(struct psmouse *psmouse,
+				    struct alps_data *priv)
+{
+	unsigned char otp[2][4];
+
+	memset(otp, 0, sizeof(otp));
+
+	if (alps_get_otp_values_ss4_v2(psmouse, 0, &otp[0][0]) ||
+	    alps_get_otp_values_ss4_v2(psmouse, 1, &otp[1][0]))
+		return -1;
+
+	alps_update_device_area_ss4_v2(otp, priv);
+
+	alps_update_btn_info_ss4_v2(otp, priv);
+
+	return 0;
+}
+
 static int alps_dolphin_get_device_area(struct psmouse *psmouse,
 					struct alps_data *priv)
 {
@@ -2213,6 +2497,35 @@ error:
 	return ret;
 }
 
+static int alps_hw_init_ss4_v2(struct psmouse *psmouse)
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	char param[2] = {0x64, 0x28};
+	int ret = -1;
+
+	/* enter absolute mode */
+	if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSTREAM) ||
+	    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSTREAM) ||
+	    ps2_command(ps2dev, &param[0], PSMOUSE_CMD_SETRATE) ||
+	    ps2_command(ps2dev, &param[1], PSMOUSE_CMD_SETRATE)) {
+		goto error;
+	}
+
+	/* T.B.D. Decread noise packet number, delete in the future */
+	if (alps_exit_command_mode(psmouse) ||
+	    alps_enter_command_mode(psmouse) ||
+	    alps_command_mode_write_reg(psmouse, 0x001D, 0x20)) {
+		goto error;
+	}
+	alps_exit_command_mode(psmouse);
+
+	return ps2_command(ps2dev, NULL, PSMOUSE_CMD_ENABLE);
+
+error:
+	alps_exit_command_mode(psmouse);
+	return ret;
+}
+
 static int alps_set_protocol(struct psmouse *psmouse,
 			     struct alps_data *priv,
 			     const struct alps_protocol_info *protocol)
@@ -2281,10 +2594,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:
@@ -2300,17 +2615,29 @@ static int alps_set_protocol(struct psmouse *psmouse,
 		priv->hw_init = alps_hw_init_v7;
 		priv->process_packet = alps_process_packet_v7;
 		priv->decode_fields = alps_decode_packet_v7;
-		priv->set_abs_params = alps_set_abs_params_mt;
+		priv->set_abs_params = alps_set_abs_params_v7;
 		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;
 
 		break;
+
+	case ALPS_PROTO_V8:
+		priv->hw_init = alps_hw_init_ss4_v2;
+		priv->process_packet = alps_process_packet_ss4_v2;
+		priv->decode_fields = alps_decode_ss4_v2;
+		priv->set_abs_params = alps_set_abs_params_ss4_v2;
+		priv->nibble_commands = alps_v3_nibble_commands;
+		priv->addr_command = PSMOUSE_CMD_RESET_WRAP;
+
+		if (alps_set_defaults_ss4_v2(psmouse, priv))
+			return -EIO;
+
+		break;
 	}
 
 	return 0;
@@ -2379,6 +2706,9 @@ static int alps_identify(struct psmouse *psmouse, struct alps_data *priv)
 		} else if (ec[0] == 0x88 && ec[1] == 0x07 &&
 			   ec[2] >= 0x90 && ec[2] <= 0x9d) {
 			protocol = &alps_v3_protocol_data;
+		} else if (e7[0] == 0x73 && e7[1] == 0x03 &&
+			   e7[2] == 0x14 && ec[1] == 0x02) {
+			protocol = &alps_v8_protocol_data;
 		} else {
 			psmouse_dbg(psmouse,
 				    "Likely not an ALPS touchpad: E7=%3ph, EC=%3ph\n", e7, ec);
@@ -2427,10 +2757,11 @@ static void alps_set_abs_params_st(struct alps_data *priv,
 {
 	input_set_abs_params(dev1, ABS_X, 0, priv->x_max, 0, 0);
 	input_set_abs_params(dev1, ABS_Y, 0, priv->y_max, 0, 0);
+	input_set_abs_params(dev1, ABS_PRESSURE, 0, 127, 0, 0);
 }
 
-static void alps_set_abs_params_mt(struct alps_data *priv,
-				   struct input_dev *dev1)
+static void alps_set_abs_params_mt_common(struct alps_data *priv,
+					  struct input_dev *dev1)
 {
 	input_set_abs_params(dev1, ABS_MT_POSITION_X, 0, priv->x_max, 0, 0);
 	input_set_abs_params(dev1, ABS_MT_POSITION_Y, 0, priv->y_max, 0, 0);
@@ -2438,15 +2769,44 @@ static void alps_set_abs_params_mt(struct alps_data *priv,
 	input_abs_set_res(dev1, ABS_MT_POSITION_X, priv->x_res);
 	input_abs_set_res(dev1, ABS_MT_POSITION_Y, priv->y_res);
 
-	input_mt_init_slots(dev1, MAX_TOUCHES, INPUT_MT_POINTER |
-		INPUT_MT_DROP_UNUSED | INPUT_MT_TRACK | INPUT_MT_SEMI_MT);
-
 	set_bit(BTN_TOOL_TRIPLETAP, dev1->keybit);
 	set_bit(BTN_TOOL_QUADTAP, dev1->keybit);
+}
+
+static void alps_set_abs_params_mt(struct alps_data *priv,
+				   struct input_dev *dev1)
+{
+	alps_set_abs_params_mt_common(priv, dev1);
+	input_set_abs_params(dev1, ABS_PRESSURE, 0, 127, 0, 0);
+
+	input_mt_init_slots(dev1, MAX_TOUCHES,
+			    INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED |
+				INPUT_MT_TRACK | INPUT_MT_SEMI_MT);
+}
+
+static void alps_set_abs_params_v7(struct alps_data *priv,
+				   struct input_dev *dev1)
+{
+	alps_set_abs_params_mt_common(priv, dev1);
+	set_bit(BTN_TOOL_QUINTTAP, dev1->keybit);
+
+	input_mt_init_slots(dev1, MAX_TOUCHES,
+			    INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED |
+				INPUT_MT_TRACK);
+
+	set_bit(BTN_TOOL_QUINTTAP, dev1->keybit);
+}
+
+static void alps_set_abs_params_ss4_v2(struct alps_data *priv,
+				       struct input_dev *dev1)
+{
+	alps_set_abs_params_mt_common(priv, dev1);
+	input_set_abs_params(dev1, ABS_PRESSURE, 0, 127, 0, 0);
+	set_bit(BTN_TOOL_QUINTTAP, dev1->keybit);
 
-	/* V7 is real multi-touch */
-	if (priv->proto_version == ALPS_PROTO_V7)
-		clear_bit(INPUT_PROP_SEMI_MT, dev1->propbit);
+	input_mt_init_slots(dev1, MAX_TOUCHES,
+			    INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED |
+				INPUT_MT_TRACK);
 }
 
 int alps_init(struct psmouse *psmouse)
@@ -2479,9 +2839,6 @@ int alps_init(struct psmouse *psmouse)
 	dev1->evbit[BIT_WORD(EV_ABS)] |= BIT_MASK(EV_ABS);
 
 	priv->set_abs_params(priv, dev1);
-	/* No pressure on V7 */
-	if (priv->proto_version != ALPS_PROTO_V7)
-		input_set_abs_params(dev1, ABS_PRESSURE, 0, 127, 0, 0);
 
 	if (priv->flags & ALPS_WHEEL) {
 		dev1->evbit[BIT_WORD(EV_REL)] |= BIT_MASK(EV_REL);
diff --git a/drivers/input/mouse/alps.h b/drivers/input/mouse/alps.h
index 02513c0502fc..6dfdccc3a7c6 100644
--- a/drivers/input/mouse/alps.h
+++ b/drivers/input/mouse/alps.h
@@ -22,14 +22,90 @@
 #define ALPS_PROTO_V5		0x500
 #define ALPS_PROTO_V6		0x600
 #define ALPS_PROTO_V7		0x700	/* t3btl t4s */
+#define ALPS_PROTO_V8		0x800	/* SS4btl SS4s */
 
-#define MAX_TOUCHES	2
+#define MAX_TOUCHES	4
 
 #define DOLPHIN_COUNT_PER_ELECTRODE	64
 #define DOLPHIN_PROFILE_XOFFSET		8	/* x-electrode offset */
 #define DOLPHIN_PROFILE_YOFFSET		1	/* y-electrode offset */
 
 /*
+ * enum SS4_PACKET_ID - defines the packet type for V8
+ * SS4_PACKET_ID_IDLE: There's no finger and no button activity.
+ * SS4_PACKET_ID_ONE: There's one finger on touchpad
+ *  or there's button activities.
+ * SS4_PACKET_ID_TWO: There's two or more fingers on touchpad
+ * SS4_PACKET_ID_MULTI: There's three or more fingers on touchpad
+*/
+enum SS4_PACKET_ID {
+	SS4_PACKET_ID_IDLE = 0,
+	SS4_PACKET_ID_ONE,
+	SS4_PACKET_ID_TWO,
+	SS4_PACKET_ID_MULTI,
+};
+
+#define SS4_COUNT_PER_ELECTRODE		256
+#define SS4_NUMSENSOR_XOFFSET		7
+#define SS4_NUMSENSOR_YOFFSET		7
+#define SS4_MIN_PITCH_MM		50
+
+#define SS4_MASK_NORMAL_BUTTONS		0x07
+
+#define SS4_1F_X_V2(_b)		((_b[0] & 0x0007) |		\
+				 ((_b[1] << 3) & 0x0078) |	\
+				 ((_b[1] << 2) & 0x0380) |	\
+				 ((_b[2] << 5) & 0x1C00)	\
+				)
+
+#define SS4_1F_Y_V2(_b)		(((_b[2]) & 0x000F) |		\
+				 ((_b[3] >> 2) & 0x0030) |	\
+				 ((_b[4] << 6) & 0x03C0) |	\
+				 ((_b[4] << 5) & 0x0C00)	\
+				)
+
+#define SS4_1F_Z_V2(_b)		(((_b[5]) & 0x0F) |		\
+				 ((_b[5] >> 1) & 0x70) |	\
+				 ((_b[4]) & 0x80)		\
+				)
+
+#define SS4_1F_LFB_V2(_b)	(((_b[2] >> 4) & 0x01) == 0x01)
+
+#define SS4_MF_LF_V2(_b, _i)	((_b[1 + (_i) * 3] & 0x0004) == 0x0004)
+
+#define SS4_BTN_V2(_b)		((_b[0] >> 5) & SS4_MASK_NORMAL_BUTTONS)
+
+#define SS4_STD_MF_X_V2(_b, _i)	(((_b[0 + (_i) * 3] << 5) & 0x00E0) |	\
+				 ((_b[1 + _i * 3]  << 5) & 0x1F00)	\
+				)
+
+#define SS4_STD_MF_Y_V2(_b, _i)	(((_b[1 + (_i) * 3] << 3) & 0x0010) |	\
+				 ((_b[2 + (_i) * 3] << 5) & 0x01E0) |	\
+				 ((_b[2 + (_i) * 3] << 4) & 0x0E00)	\
+				)
+
+#define SS4_BTL_MF_X_V2(_b, _i)	(SS4_STD_MF_X_V2(_b, _i) |		\
+				 ((_b[0 + (_i) * 3] >> 3) & 0x0010)	\
+				)
+
+#define SS4_BTL_MF_Y_V2(_b, _i)	(SS4_STD_MF_Y_V2(_b, _i) | \
+				 ((_b[0 + (_i) * 3] >> 3) & 0x0008)	\
+				)
+
+#define SS4_MF_Z_V2(_b, _i)	(((_b[1 + (_i) * 3]) & 0x0001) |	\
+				 ((_b[1 + (_i) * 3] >> 1) & 0x0002)	\
+				)
+
+#define SS4_IS_MF_CONTINUE(_b)	((_b[2] & 0x10) == 0x10)
+#define SS4_IS_5F_DETECTED(_b)	((_b[2] & 0x10) == 0x10)
+
+
+#define SS4_MFPACKET_NO_AX	8160	/* X-Coordinate value */
+#define SS4_MFPACKET_NO_AY	4080	/* Y-Coordinate value */
+#define SS4_MFPACKET_NO_AX_BL	8176	/* Buttonless X-Coordinate value */
+#define SS4_MFPACKET_NO_AY_BL	4088	/* Buttonless Y-Coordinate value */
+
+/*
  * enum V7_PACKET_ID - defines the packet type for V7
  * V7_PACKET_ID_IDLE: There's no finger and no button activity.
  * V7_PACKET_ID_TWO: There's one or two non-resting fingers on touchpad
diff --git a/drivers/input/mouse/elan_i2c.h b/drivers/input/mouse/elan_i2c.h
index e100c1b31597..9b2dc015f20c 100644
--- a/drivers/input/mouse/elan_i2c.h
+++ b/drivers/input/mouse/elan_i2c.h
@@ -17,7 +17,7 @@
  */
 
 #ifndef _ELAN_I2C_H
-#define _ELAN_i2C_H
+#define _ELAN_I2C_H
 
 #include <linux/types.h>
 
diff --git a/drivers/input/mouse/elan_i2c_core.c b/drivers/input/mouse/elan_i2c_core.c
index 7ce8bfe22d7e..375d98f47483 100644
--- a/drivers/input/mouse/elan_i2c_core.c
+++ b/drivers/input/mouse/elan_i2c_core.c
@@ -99,7 +99,7 @@ static int elan_enable_power(struct elan_tp_data *data)
 	error = regulator_enable(data->vcc);
 	if (error) {
 		dev_err(&data->client->dev,
-			"Failed to enable regulator: %d\n", error);
+			"failed to enable regulator: %d\n", error);
 		return error;
 	}
 
@@ -111,6 +111,7 @@ static int elan_enable_power(struct elan_tp_data *data)
 		msleep(30);
 	} while (--repeat > 0);
 
+	dev_err(&data->client->dev, "failed to enable power: %d\n", error);
 	return error;
 }
 
@@ -125,7 +126,7 @@ static int elan_disable_power(struct elan_tp_data *data)
 			error = regulator_disable(data->vcc);
 			if (error) {
 				dev_err(&data->client->dev,
-					"Failed to disable regulator: %d\n",
+					"failed to disable regulator: %d\n",
 					error);
 				/* Attempt to power the chip back up */
 				data->ops->power_control(data->client, true);
@@ -138,6 +139,7 @@ static int elan_disable_power(struct elan_tp_data *data)
 		msleep(30);
 	} while (--repeat > 0);
 
+	dev_err(&data->client->dev, "failed to disable power: %d\n", error);
 	return error;
 }
 
@@ -196,7 +198,6 @@ static int elan_initialize(struct elan_tp_data *data)
 		if (!error)
 			return 0;
 
-		repeat--;
 		msleep(30);
 	} while (--repeat > 0);
 
@@ -1084,16 +1085,18 @@ static int __maybe_unused elan_resume(struct device *dev)
 	}
 
 	error = elan_enable_power(data);
-	if (error)
+	if (error) {
 		dev_err(dev, "power up when resuming failed: %d\n", error);
+		goto err;
+	}
 
 	error = elan_initialize(data);
 	if (error)
 		dev_err(dev, "initialize when resuming failed: %d\n", error);
 
+err:
 	enable_irq(data->client->irq);
-
-	return 0;
+	return error;
 }
 
 static SIMPLE_DEV_PM_OPS(elan_pm_ops, elan_suspend, elan_resume);
diff --git a/drivers/input/mouse/elan_i2c_i2c.c b/drivers/input/mouse/elan_i2c_i2c.c
index 029941f861af..6cf0def6d35e 100644
--- a/drivers/input/mouse/elan_i2c_i2c.c
+++ b/drivers/input/mouse/elan_i2c_i2c.c
@@ -117,7 +117,15 @@ static int elan_i2c_write_cmd(struct i2c_client *client, u16 reg, u16 cmd)
 	int ret;
 
 	ret = i2c_transfer(client->adapter, &msg, 1);
-	return ret == 1 ? 0 : (ret < 0 ? ret : -EIO);
+	if (ret != 1) {
+		if (ret >= 0)
+			ret = -EIO;
+		dev_err(&client->dev, "writing cmd (0x%04x) failed: %d\n",
+			reg, ret);
+		return ret;
+	}
+
+	return 0;
 }
 
 static int elan_i2c_initialize(struct i2c_client *client)
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index 6e22682c8255..991dc6b20a58 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -893,6 +893,21 @@ static psmouse_ret_t elantech_process_byte(struct psmouse *psmouse)
 }
 
 /*
+ * This writes the reg_07 value again to the hardware at the end of every
+ * set_rate call because the register loses its value. reg_07 allows setting
+ * absolute mode on v4 hardware
+ */
+static void elantech_set_rate_restore_reg_07(struct psmouse *psmouse,
+		unsigned int rate)
+{
+	struct elantech_data *etd = psmouse->private;
+
+	etd->original_set_rate(psmouse, rate);
+	if (elantech_write_reg(psmouse, 0x07, etd->reg_07))
+		psmouse_err(psmouse, "restoring reg_07 failed\n");
+}
+
+/*
  * Put the touchpad into absolute mode
  */
 static int elantech_set_absolute_mode(struct psmouse *psmouse)
@@ -1094,6 +1109,8 @@ static int elantech_get_resolution_v4(struct psmouse *psmouse,
  * Asus K53SV              0x450f01        78, 15, 0c      2 hw buttons
  * Asus G46VW              0x460f02        00, 18, 0c      2 hw buttons
  * Asus G750JX             0x360f00        00, 16, 0c      2 hw buttons
+ * Asus TP500LN            0x381f17        10, 14, 0e      clickpad
+ * Asus X750JN             0x381f17        10, 14, 0e      clickpad
  * Asus UX31               0x361f00        20, 15, 0e      clickpad
  * Asus UX32VD             0x361f02        00, 15, 0e      clickpad
  * Avatar AVIU-145A2       0x361f00        ?               clickpad
@@ -1635,6 +1652,11 @@ int elantech_init(struct psmouse *psmouse)
 		goto init_fail;
 	}
 
+	if (etd->fw_version == 0x381f17) {
+		etd->original_set_rate = psmouse->set_rate;
+		psmouse->set_rate = elantech_set_rate_restore_reg_07;
+	}
+
 	if (elantech_set_input_params(psmouse)) {
 		psmouse_err(psmouse, "failed to query touchpad range.\n");
 		goto init_fail;
diff --git a/drivers/input/mouse/elantech.h b/drivers/input/mouse/elantech.h
index 6f3afec02f03..f965d1569cc3 100644
--- a/drivers/input/mouse/elantech.h
+++ b/drivers/input/mouse/elantech.h
@@ -142,6 +142,7 @@ struct elantech_data {
 	struct finger_pos mt[ETP_MAX_FINGERS];
 	unsigned char parity[256];
 	int (*send_cmd)(struct psmouse *psmouse, unsigned char c, unsigned char *param);
+	void (*original_set_rate)(struct psmouse *psmouse, unsigned int rate);
 };
 
 #ifdef CONFIG_MOUSE_PS2_ELANTECH
diff --git a/drivers/input/mouse/lifebook.c b/drivers/input/mouse/lifebook.c
index 23222dd5a66f..e5ed216824e9 100644
--- a/drivers/input/mouse/lifebook.c
+++ b/drivers/input/mouse/lifebook.c
@@ -256,8 +256,8 @@ static void lifebook_disconnect(struct psmouse *psmouse)
 
 int lifebook_detect(struct psmouse *psmouse, bool set_properties)
 {
-        if (!lifebook_present)
-                return -1;
+	if (!lifebook_present)
+		return -1;
 
 	if (desired_serio_phys &&
 	    strcmp(psmouse->ps2dev.serio->phys, desired_serio_phys))
@@ -268,7 +268,7 @@ int lifebook_detect(struct psmouse *psmouse, bool set_properties)
 		psmouse->name = "Lifebook TouchScreen";
 	}
 
-        return 0;
+	return 0;
 }
 
 static int lifebook_create_relative_device(struct psmouse *psmouse)
diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index 8bc61237bc1b..27057df7ba74 100644
--- a/drivers/input/mouse/psmouse-base.c
+++ b/drivers/input/mouse/psmouse-base.c
@@ -474,19 +474,45 @@ static int psmouse_poll(struct psmouse *psmouse)
 			   PSMOUSE_CMD_POLL | (psmouse->pktsize << 8));
 }
 
+static bool psmouse_check_pnp_id(const char *id, const char * const ids[])
+{
+	int i;
+
+	for (i = 0; ids[i]; i++)
+		if (!strcasecmp(id, ids[i]))
+			return true;
+
+	return false;
+}
+
 /*
  * psmouse_matches_pnp_id - check if psmouse matches one of the passed in ids.
  */
 bool psmouse_matches_pnp_id(struct psmouse *psmouse, const char * const ids[])
 {
-	int i;
-
-	if (!strncmp(psmouse->ps2dev.serio->firmware_id, "PNP:", 4))
-		for (i = 0; ids[i]; i++)
-			if (strstr(psmouse->ps2dev.serio->firmware_id, ids[i]))
-				return true;
+	struct serio *serio = psmouse->ps2dev.serio;
+	char *p, *fw_id_copy, *save_ptr;
+	bool found = false;
+
+	if (strncmp(serio->firmware_id, "PNP: ", 5))
+		return false;
+
+	fw_id_copy = kstrndup(&serio->firmware_id[5],
+			      sizeof(serio->firmware_id) - 5,
+			      GFP_KERNEL);
+	if (!fw_id_copy)
+		return false;
+
+	save_ptr = fw_id_copy;
+	while ((p = strsep(&fw_id_copy, " ")) != NULL) {
+		if (psmouse_check_pnp_id(p, ids)) {
+			found = true;
+			break;
+		}
+	}
 
-	return false;
+	kfree(save_ptr);
+	return found;
 }
 
 /*
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index f2cceb6493a0..630af73e98c4 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -123,32 +123,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 +189,7 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN0041",
 	"LEN0042", /* Yoga */
 	"LEN0045",
-	"LEN0046",
 	"LEN0047",
-	"LEN0048",
 	"LEN0049",
 	"LEN2000",
 	"LEN2001", /* Edge E431 */
@@ -185,7 +197,7 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN2003",
 	"LEN2004", /* L440 */
 	"LEN2005",
-	"LEN2006",
+	"LEN2006", /* Edge E440/E540 */
 	"LEN2007",
 	"LEN2008",
 	"LEN2009",
@@ -194,6 +206,13 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	NULL
 };
 
+/* This list has been kindly provided by Synaptics. */
+static const char * const forcepad_pnp_ids[] = {
+	"SYN300D",
+	"SYN3014",
+	NULL
+};
+
 /*****************************************************************************
  *	Synaptics communications functions
  ****************************************************************************/
@@ -235,18 +254,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 +386,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 +397,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 +405,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 +476,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 +592,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,7 +685,17 @@ static void synaptics_parse_agm(const unsigned char buf[],
 	}
 }
 
-static bool is_forcepad;
+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 int synaptics_parse_hw_state(const unsigned char buf[],
 				    struct synaptics_data *priv,
@@ -636,7 +726,7 @@ static int synaptics_parse_hw_state(const unsigned char buf[],
 		hw->left  = (buf[0] & 0x01) ? 1 : 0;
 		hw->right = (buf[0] & 0x02) ? 1 : 0;
 
-		if (is_forcepad) {
+		if (priv->is_forcepad) {
 			/*
 			 * ForcePads, like Clickpads, use middle button
 			 * bits to report primary button clicks.
@@ -691,28 +781,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 +845,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 +905,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,
@@ -1014,7 +1126,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);
 
@@ -1081,7 +1194,7 @@ static void set_input_params(struct psmouse *psmouse,
 					ABS_MT_POSITION_Y);
 		/* Image sensors can report per-contact pressure */
 		input_set_abs_params(dev, ABS_MT_PRESSURE, 0, 255, 0, 0);
-		input_mt_init_slots(dev, 2, INPUT_MT_POINTER | INPUT_MT_TRACK);
+		input_mt_init_slots(dev, 3, INPUT_MT_POINTER | INPUT_MT_TRACK);
 
 		/* Image sensors can signal 4 and 5 finger clicks */
 		__set_bit(BTN_TOOL_QUADTAP, dev->keybit);
@@ -1116,8 +1229,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 +1239,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);
@@ -1311,29 +1426,11 @@ static const struct dmi_system_id __initconst cr48_dmi_table[] = {
 	{ }
 };
 
-static const struct dmi_system_id forcepad_dmi_table[] __initconst = {
-#if defined(CONFIG_DMI) && defined(CONFIG_X86)
-	{
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "HP EliteBook Folio 1040 G1"),
-		},
-	},
-#endif
-	{ }
-};
-
 void __init synaptics_module_init(void)
 {
 	impaired_toshiba_kbc = dmi_check_system(toshiba_dmi_table);
 	broken_olpc_ec = dmi_check_system(olpc_dmi_table);
 	cr48_profile_sensor = dmi_check_system(cr48_dmi_table);
-
-	/*
-	 * Unfortunately ForcePad capability is not exported over PS/2,
-	 * so we have to resort to checking DMI.
-	 */
-	is_forcepad = dmi_check_system(forcepad_dmi_table);
 }
 
 static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
@@ -1368,6 +1465,12 @@ static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
 	if (SYN_ID_DISGEST_SUPPORTED(priv->identity))
 		priv->disable_gesture = true;
 
+	/*
+	 * Unfortunately ForcePad capability is not exported over PS/2,
+	 * so we have to resort to checking PNP IDs.
+	 */
+	priv->is_forcepad = psmouse_matches_pnp_id(psmouse, forcepad_pnp_ids);
+
 	if (synaptics_set_mode(psmouse)) {
 		psmouse_err(psmouse, "Unable to initialize device.\n");
 		goto init_fail;
diff --git a/drivers/input/mouse/synaptics.h b/drivers/input/mouse/synaptics.h
index aedc3299b14e..56faa7ec4434 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
@@ -168,6 +196,7 @@ struct synaptics_data {
 	unsigned long				press_start;
 	bool					press;
 	bool					report_press;
+	bool					is_forcepad;
 };
 
 void synaptics_module_init(void);
diff --git a/drivers/input/serio/gscps2.c b/drivers/input/serio/gscps2.c
index 94ab494a6ade..ecba666afadb 100644
--- a/drivers/input/serio/gscps2.c
+++ b/drivers/input/serio/gscps2.c
@@ -31,7 +31,6 @@
 #include <linux/spinlock.h>
 #include <linux/delay.h>
 #include <linux/ioport.h>
-#include <linux/pci_ids.h>
 
 #include <asm/irq.h>
 #include <asm/io.h>
diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index 986a71c614b0..cb5ece77fd7d 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -1162,13 +1162,32 @@ static int i8042_controller_resume(bool force_reset)
 
 static int i8042_pm_suspend(struct device *dev)
 {
+	int i;
+
 	i8042_controller_reset(true);
 
+	/* Set up serio interrupts for system wakeup. */
+	for (i = 0; i < I8042_NUM_PORTS; i++) {
+		struct serio *serio = i8042_ports[i].serio;
+
+		if (serio && device_may_wakeup(&serio->dev))
+			enable_irq_wake(i8042_ports[i].irq);
+	}
+
 	return 0;
 }
 
 static int i8042_pm_resume(struct device *dev)
 {
+	int i;
+
+	for (i = 0; i < I8042_NUM_PORTS; i++) {
+		struct serio *serio = i8042_ports[i].serio;
+
+		if (serio && device_may_wakeup(&serio->dev))
+			disable_irq_wake(i8042_ports[i].irq);
+	}
+
 	/*
 	 * On resume from S2R we always try to reset the controller
 	 * to bring it in a sane state. (In case of S2D we expect
@@ -1300,13 +1319,16 @@ static void __init i8042_register_ports(void)
 	int i;
 
 	for (i = 0; i < I8042_NUM_PORTS; i++) {
-		if (i8042_ports[i].serio) {
+		struct serio *serio = i8042_ports[i].serio;
+
+		if (serio) {
 			printk(KERN_INFO "serio: %s at %#lx,%#lx irq %d\n",
-				i8042_ports[i].serio->name,
+				serio->name,
 				(unsigned long) I8042_DATA_REG,
 				(unsigned long) I8042_COMMAND_REG,
 				i8042_ports[i].irq);
-			serio_register_port(i8042_ports[i].serio);
+			serio_register_port(serio);
+			device_set_wakeup_capable(&serio->dev, true);
 		}
 	}
 }
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 6261fd6d7c3c..547f67d65372 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -140,6 +140,19 @@ config TOUCHSCREEN_BU21013
 	  To compile this driver as a module, choose M here: the
 	  module will be called bu21013_ts.
 
+config TOUCHSCREEN_CHIPONE_ICN8318
+	tristate "chipone icn8318 touchscreen controller"
+	depends on GPIOLIB
+	depends on I2C
+	depends on OF
+	help
+	  Say Y here if you have a ChipOne icn8318 based I2C touchscreen.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called chipone_icn8318.
+
 config TOUCHSCREEN_CY8CTMG110
 	tristate "cy8ctmg110 touchscreen"
 	depends on I2C
@@ -297,11 +310,12 @@ config TOUCHSCREEN_FUJITSU
 
 config TOUCHSCREEN_GOODIX
 	tristate "Goodix I2C touchscreen"
-	depends on I2C && ACPI
+	depends on I2C
 	help
 	  Say Y here if you have the Goodix touchscreen (such as one
 	  installed in Onda v975w tablets) connected to your
-	  system.
+	  system. It also supports 5-finger chip models, which can be
+	  found on ARM tablets, like Wexler TAB7200 and MSI Primo73.
 
 	  If unsure, say N.
 
@@ -323,6 +337,18 @@ config TOUCHSCREEN_ILI210X
 	  To compile this driver as a module, choose M here: the
 	  module will be called ili210x.
 
+config TOUCHSCREEN_IPROC
+	tristate "IPROC touch panel driver support"
+	depends on ARCH_BCM_IPROC || COMPILE_TEST
+	help
+	  Say Y here if you want to add support for the IPROC touch
+	  controller to your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bcm_iproc_tsc.
+
 config TOUCHSCREEN_S3C2410
 	tristate "Samsung S3C2410/generic touchscreen input driver"
 	depends on ARCH_S3C24XX || SAMSUNG_DEV_TS
@@ -962,6 +988,17 @@ config TOUCHSCREEN_SUR40
 	  To compile this driver as a module, choose M here: the
 	  module will be called sur40.
 
+config TOUCHSCREEN_SX8654
+	tristate "Semtech SX8654 touchscreen"
+	depends on I2C
+	help
+	  Say Y here if you have a Semtech SX8654 touchscreen controller.
+
+	  If unsure, say N
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sx8654.
+
 config TOUCHSCREEN_TPS6507X
 	tristate "TPS6507x based touchscreens"
 	depends on I2C
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 0242fea2102a..44deea743d02 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -17,6 +17,7 @@ obj-$(CONFIG_TOUCHSCREEN_AR1021_I2C)	+= ar1021_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_ATMEL_MXT)	+= atmel_mxt_ts.o
 obj-$(CONFIG_TOUCHSCREEN_AUO_PIXCIR)	+= auo-pixcir-ts.o
 obj-$(CONFIG_TOUCHSCREEN_BU21013)	+= bu21013_ts.o
+obj-$(CONFIG_TOUCHSCREEN_CHIPONE_ICN8318)	+= chipone_icn8318.o
 obj-$(CONFIG_TOUCHSCREEN_CY8CTMG110)	+= cy8ctmg110_ts.o
 obj-$(CONFIG_TOUCHSCREEN_CYTTSP_CORE)	+= cyttsp_core.o
 obj-$(CONFIG_TOUCHSCREEN_CYTTSP_I2C)	+= cyttsp_i2c.o cyttsp_i2c_common.o
@@ -39,6 +40,7 @@ obj-$(CONFIG_TOUCHSCREEN_GOODIX)	+= goodix.o
 obj-$(CONFIG_TOUCHSCREEN_ILI210X)	+= ili210x.o
 obj-$(CONFIG_TOUCHSCREEN_INEXIO)	+= inexio.o
 obj-$(CONFIG_TOUCHSCREEN_INTEL_MID)	+= intel-mid-touch.o
+obj-$(CONFIG_TOUCHSCREEN_IPROC)		+= bcm_iproc_tsc.o
 obj-$(CONFIG_TOUCHSCREEN_LPC32XX)	+= lpc32xx_ts.o
 obj-$(CONFIG_TOUCHSCREEN_MAX11801)	+= max11801_ts.o
 obj-$(CONFIG_TOUCHSCREEN_MC13783)	+= mc13783_ts.o
@@ -79,5 +81,6 @@ obj-$(CONFIG_TOUCHSCREEN_WM97XX_ATMEL)	+= atmel-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX_MAINSTONE)	+= mainstone-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX_ZYLONITE)	+= zylonite-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_W90X900)	+= w90p910_ts.o
+obj-$(CONFIG_TOUCHSCREEN_SX8654)	+= sx8654.o
 obj-$(CONFIG_TOUCHSCREEN_TPS6507X)	+= tps6507x-ts.o
 obj-$(CONFIG_TOUCHSCREEN_ZFORCE)	+= zforce_ts.o
diff --git a/drivers/input/touchscreen/ar1021_i2c.c b/drivers/input/touchscreen/ar1021_i2c.c
index ba30578e296e..f0b954d46a25 100644
--- a/drivers/input/touchscreen/ar1021_i2c.c
+++ b/drivers/input/touchscreen/ar1021_i2c.c
@@ -157,7 +157,7 @@ static const struct i2c_device_id ar1021_i2c_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, ar1021_i2c_id);
 
-static struct of_device_id ar1021_i2c_of_match[] = {
+static const struct of_device_id ar1021_i2c_of_match[] = {
 	{ .compatible = "microchip,ar1021-i2c", },
 	{ }
 };
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 95ee92a91bd2..2875ddf37289 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -25,6 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/of.h>
 #include <linux/slab.h>
+#include <asm/unaligned.h>
 
 /* Version */
 #define MXT_VER_20		20
@@ -79,6 +80,7 @@
 #define MXT_SPT_DIGITIZER_T43		43
 #define MXT_SPT_MESSAGECOUNT_T44	44
 #define MXT_SPT_CTECONFIG_T46		46
+#define MXT_TOUCH_MULTITOUCHSCREEN_T100 100
 
 /* MXT_GEN_MESSAGE_T5 object */
 #define MXT_RPTID_NOMSG		0xff
@@ -185,6 +187,36 @@ struct t9_range {
 #define MXT_RESET_VALUE		0x01
 #define MXT_BACKUP_VALUE	0x55
 
+/* T100 Multiple Touch Touchscreen */
+#define MXT_T100_CTRL		0
+#define MXT_T100_CFG1		1
+#define MXT_T100_TCHAUX		3
+#define MXT_T100_XRANGE		13
+#define MXT_T100_YRANGE		24
+
+#define MXT_T100_CFG_SWITCHXY	BIT(5)
+
+#define MXT_T100_TCHAUX_VECT	BIT(0)
+#define MXT_T100_TCHAUX_AMPL	BIT(1)
+#define MXT_T100_TCHAUX_AREA	BIT(2)
+
+#define MXT_T100_DETECT		BIT(7)
+#define MXT_T100_TYPE_MASK	0x70
+
+enum t100_type {
+	MXT_T100_TYPE_FINGER		= 1,
+	MXT_T100_TYPE_PASSIVE_STYLUS	= 2,
+	MXT_T100_TYPE_HOVERING_FINGER	= 4,
+	MXT_T100_TYPE_GLOVE		= 5,
+	MXT_T100_TYPE_LARGE_TOUCH	= 6,
+};
+
+#define MXT_DISTANCE_ACTIVE_TOUCH	0
+#define MXT_DISTANCE_HOVERING		1
+
+#define MXT_TOUCH_MAJOR_DEFAULT		1
+#define MXT_PRESSURE_DEFAULT		1
+
 /* Delay times */
 #define MXT_BACKUP_TIME		50	/* msec */
 #define MXT_RESET_TIME		200	/* msec */
@@ -244,6 +276,9 @@ struct mxt_data {
 	unsigned int max_y;
 	bool in_bootloader;
 	u16 mem_size;
+	u8 t100_aux_ampl;
+	u8 t100_aux_area;
+	u8 t100_aux_vect;
 	u8 max_reportid;
 	u32 config_crc;
 	u32 info_crc;
@@ -253,6 +288,7 @@ struct mxt_data {
 	bool update_input;
 	u8 last_message_count;
 	u8 num_touchids;
+	u8 multitouch;
 
 	/* Cached parameters from object table */
 	u16 T5_address;
@@ -264,6 +300,8 @@ struct mxt_data {
 	u8 T9_reportid_max;
 	u8 T19_reportid;
 	u16 T44_address;
+	u8 T100_reportid_min;
+	u8 T100_reportid_max;
 
 	/* for fw update in bootloader */
 	struct completion bl_completion;
@@ -771,6 +809,114 @@ static void mxt_proc_t9_message(struct mxt_data *data, u8 *message)
 	data->update_input = true;
 }
 
+static void mxt_proc_t100_message(struct mxt_data *data, u8 *message)
+{
+	struct device *dev = &data->client->dev;
+	struct input_dev *input_dev = data->input_dev;
+	int id;
+	u8 status;
+	u8 type = 0;
+	u16 x;
+	u16 y;
+	int distance = 0;
+	int tool = 0;
+	u8 major = 0;
+	u8 pressure = 0;
+	u8 orientation = 0;
+
+	id = message[0] - data->T100_reportid_min - 2;
+
+	/* ignore SCRSTATUS events */
+	if (id < 0)
+		return;
+
+	status = message[1];
+	x = get_unaligned_le16(&message[2]);
+	y = get_unaligned_le16(&message[4]);
+
+	if (status & MXT_T100_DETECT) {
+		type = (status & MXT_T100_TYPE_MASK) >> 4;
+
+		switch (type) {
+		case MXT_T100_TYPE_HOVERING_FINGER:
+			tool = MT_TOOL_FINGER;
+			distance = MXT_DISTANCE_HOVERING;
+
+			if (data->t100_aux_vect)
+				orientation = message[data->t100_aux_vect];
+
+			break;
+
+		case MXT_T100_TYPE_FINGER:
+		case MXT_T100_TYPE_GLOVE:
+			tool = MT_TOOL_FINGER;
+			distance = MXT_DISTANCE_ACTIVE_TOUCH;
+
+			if (data->t100_aux_area)
+				major = message[data->t100_aux_area];
+
+			if (data->t100_aux_ampl)
+				pressure = message[data->t100_aux_ampl];
+
+			if (data->t100_aux_vect)
+				orientation = message[data->t100_aux_vect];
+
+			break;
+
+		case MXT_T100_TYPE_PASSIVE_STYLUS:
+			tool = MT_TOOL_PEN;
+
+			/*
+			 * Passive stylus is reported with size zero so
+			 * hardcode.
+			 */
+			major = MXT_TOUCH_MAJOR_DEFAULT;
+
+			if (data->t100_aux_ampl)
+				pressure = message[data->t100_aux_ampl];
+
+			break;
+
+		case MXT_T100_TYPE_LARGE_TOUCH:
+			/* Ignore suppressed touch */
+			break;
+
+		default:
+			dev_dbg(dev, "Unexpected T100 type\n");
+			return;
+		}
+	}
+
+	/*
+	 * Values reported should be non-zero if tool is touching the
+	 * device
+	 */
+	if (!pressure && type != MXT_T100_TYPE_HOVERING_FINGER)
+		pressure = MXT_PRESSURE_DEFAULT;
+
+	input_mt_slot(input_dev, id);
+
+	if (status & MXT_T100_DETECT) {
+		dev_dbg(dev, "[%u] type:%u x:%u y:%u a:%02X p:%02X v:%02X\n",
+			id, type, x, y, major, pressure, orientation);
+
+		input_mt_report_slot_state(input_dev, tool, 1);
+		input_report_abs(input_dev, ABS_MT_POSITION_X, x);
+		input_report_abs(input_dev, ABS_MT_POSITION_Y, y);
+		input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR, major);
+		input_report_abs(input_dev, ABS_MT_PRESSURE, pressure);
+		input_report_abs(input_dev, ABS_MT_DISTANCE, distance);
+		input_report_abs(input_dev, ABS_MT_ORIENTATION, orientation);
+	} else {
+		dev_dbg(dev, "[%u] release\n", id);
+
+		/* close out slot */
+		input_mt_report_slot_state(input_dev, 0, 0);
+	}
+
+	data->update_input = true;
+}
+
 static int mxt_proc_message(struct mxt_data *data, u8 *message)
 {
 	u8 report_id = message[0];
@@ -786,9 +932,12 @@ static int mxt_proc_message(struct mxt_data *data, u8 *message)
 		 * is not yet registered.
 		 */
 		mxt_dump_message(data, message);
-	} else if (report_id >= data->T9_reportid_min
-	    && report_id <= data->T9_reportid_max) {
+	} else if (report_id >= data->T9_reportid_min &&
+		   report_id <= data->T9_reportid_max) {
 		mxt_proc_t9_message(data, message);
+	} else if (report_id >= data->T100_reportid_min &&
+		   report_id <= data->T100_reportid_max) {
+		mxt_proc_t100_message(data, message);
 	} else if (report_id == data->T19_reportid) {
 		mxt_input_button(data, message);
 		data->update_input = true;
@@ -1411,6 +1560,8 @@ static void mxt_free_object_table(struct mxt_data *data)
 	data->T9_reportid_max = 0;
 	data->T19_reportid = 0;
 	data->T44_address = 0;
+	data->T100_reportid_min = 0;
+	data->T100_reportid_max = 0;
 	data->max_reportid = 0;
 }
 
@@ -1487,6 +1638,7 @@ static int mxt_get_object_table(struct mxt_data *data)
 			data->T7_address = object->start_address;
 			break;
 		case MXT_TOUCH_MULTI_T9:
+			data->multitouch = MXT_TOUCH_MULTI_T9;
 			data->T9_reportid_min = min_id;
 			data->T9_reportid_max = max_id;
 			data->num_touchids = object->num_report_ids
@@ -1498,6 +1650,13 @@ static int mxt_get_object_table(struct mxt_data *data)
 		case MXT_SPT_GPIOPWM_T19:
 			data->T19_reportid = min_id;
 			break;
+		case MXT_TOUCH_MULTITOUCHSCREEN_T100:
+			data->multitouch = MXT_TOUCH_MULTITOUCHSCREEN_T100;
+			data->T100_reportid_min = min_id;
+			data->T100_reportid_max = max_id;
+			/* first two report IDs reserved */
+			data->num_touchids = object->num_report_ids - 2;
+			break;
 		}
 
 		end_address = object->start_address
@@ -1582,22 +1741,138 @@ static int mxt_read_t9_resolution(struct mxt_data *data)
 	return 0;
 }
 
+static int mxt_read_t100_config(struct mxt_data *data)
+{
+	struct i2c_client *client = data->client;
+	int error;
+	struct mxt_object *object;
+	u16 range_x, range_y;
+	u8 cfg, tchaux;
+	u8 aux;
+
+	object = mxt_get_object(data, MXT_TOUCH_MULTITOUCHSCREEN_T100);
+	if (!object)
+		return -EINVAL;
+
+	error = __mxt_read_reg(client,
+			       object->start_address + MXT_T100_XRANGE,
+			       sizeof(range_x), &range_x);
+	if (error)
+		return error;
+
+	le16_to_cpus(&range_x);
+
+	error = __mxt_read_reg(client,
+			       object->start_address + MXT_T100_YRANGE,
+			       sizeof(range_y), &range_y);
+	if (error)
+		return error;
+
+	le16_to_cpus(&range_y);
+
+	error =  __mxt_read_reg(client,
+				object->start_address + MXT_T100_CFG1,
+				1, &cfg);
+	if (error)
+		return error;
+
+	error =  __mxt_read_reg(client,
+				object->start_address + MXT_T100_TCHAUX,
+				1, &tchaux);
+	if (error)
+		return error;
+
+	/* Handle default values */
+	if (range_x == 0)
+		range_x = 1023;
+
+	if (range_y == 0)
+		range_y = 1023;
+
+	if (cfg & MXT_T100_CFG_SWITCHXY) {
+		data->max_x = range_y;
+		data->max_y = range_x;
+	} else {
+		data->max_x = range_x;
+		data->max_y = range_y;
+	}
+
+	/* allocate aux bytes */
+	aux = 6;
+
+	if (tchaux & MXT_T100_TCHAUX_VECT)
+		data->t100_aux_vect = aux++;
+
+	if (tchaux & MXT_T100_TCHAUX_AMPL)
+		data->t100_aux_ampl = aux++;
+
+	if (tchaux & MXT_T100_TCHAUX_AREA)
+		data->t100_aux_area = aux++;
+
+	dev_dbg(&client->dev,
+		"T100 aux mappings vect:%u ampl:%u area:%u\n",
+		data->t100_aux_vect, data->t100_aux_ampl, data->t100_aux_area);
+
+	dev_info(&client->dev,
+		 "T100 Touchscreen size X%uY%u\n", data->max_x, data->max_y);
+
+	return 0;
+}
+
 static int mxt_input_open(struct input_dev *dev);
 static void mxt_input_close(struct input_dev *dev);
 
-static int mxt_initialize_t9_input_device(struct mxt_data *data)
+static void mxt_set_up_as_touchpad(struct input_dev *input_dev,
+				   struct mxt_data *data)
 {
-	struct device *dev = &data->client->dev;
 	const struct mxt_platform_data *pdata = data->pdata;
+	int i;
+
+	input_dev->name = "Atmel maXTouch Touchpad";
+
+	__set_bit(INPUT_PROP_BUTTONPAD, input_dev->propbit);
+
+	input_abs_set_res(input_dev, ABS_X, MXT_PIXELS_PER_MM);
+	input_abs_set_res(input_dev, ABS_Y, MXT_PIXELS_PER_MM);
+	input_abs_set_res(input_dev, ABS_MT_POSITION_X,
+			  MXT_PIXELS_PER_MM);
+	input_abs_set_res(input_dev, ABS_MT_POSITION_Y,
+			  MXT_PIXELS_PER_MM);
+
+	for (i = 0; i < pdata->t19_num_keys; i++)
+		if (pdata->t19_keymap[i] != KEY_RESERVED)
+			input_set_capability(input_dev, EV_KEY,
+					     pdata->t19_keymap[i]);
+}
+
+static int mxt_initialize_input_device(struct mxt_data *data)
+{
+	const struct mxt_platform_data *pdata = data->pdata;
+	struct device *dev = &data->client->dev;
 	struct input_dev *input_dev;
 	int error;
 	unsigned int num_mt_slots;
 	unsigned int mt_flags = 0;
-	int i;
 
-	error = mxt_read_t9_resolution(data);
-	if (error)
-		dev_warn(dev, "Failed to initialize T9 resolution\n");
+	switch (data->multitouch) {
+	case MXT_TOUCH_MULTI_T9:
+		num_mt_slots = data->T9_reportid_max - data->T9_reportid_min + 1;
+		error = mxt_read_t9_resolution(data);
+		if (error)
+			dev_warn(dev, "Failed to initialize T9 resolution\n");
+		break;
+
+	case MXT_TOUCH_MULTITOUCHSCREEN_T100:
+		num_mt_slots = data->num_touchids;
+		error = mxt_read_t100_config(data);
+		if (error)
+			dev_warn(dev, "Failed to read T100 config\n");
+		break;
+
+	default:
+		dev_err(dev, "Invalid multitouch object\n");
+		return -EINVAL;
+	}
 
 	input_dev = input_allocate_device();
 	if (!input_dev) {
@@ -1612,54 +1887,76 @@ static int mxt_initialize_t9_input_device(struct mxt_data *data)
 	input_dev->open = mxt_input_open;
 	input_dev->close = mxt_input_close;
 
-	__set_bit(EV_ABS, input_dev->evbit);
-	__set_bit(EV_KEY, input_dev->evbit);
-	__set_bit(BTN_TOUCH, input_dev->keybit);
+	input_set_capability(input_dev, EV_KEY, BTN_TOUCH);
 
-	if (pdata->t19_num_keys) {
-		__set_bit(INPUT_PROP_BUTTONPAD, input_dev->propbit);
+	/* For single touch */
+	input_set_abs_params(input_dev, ABS_X, 0, data->max_x, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, data->max_y, 0, 0);
 
-		for (i = 0; i < pdata->t19_num_keys; i++)
-			if (pdata->t19_keymap[i] != KEY_RESERVED)
-				input_set_capability(input_dev, EV_KEY,
-						     pdata->t19_keymap[i]);
+	if (data->multitouch == MXT_TOUCH_MULTI_T9 ||
+	    (data->multitouch == MXT_TOUCH_MULTITOUCHSCREEN_T100 &&
+	     data->t100_aux_ampl)) {
+		input_set_abs_params(input_dev, ABS_PRESSURE, 0, 255, 0, 0);
+	}
 
+	/* If device has buttons we assume it is a touchpad */
+	if (pdata->t19_num_keys) {
+		mxt_set_up_as_touchpad(input_dev, data);
 		mt_flags |= INPUT_MT_POINTER;
-
-		input_abs_set_res(input_dev, ABS_X, MXT_PIXELS_PER_MM);
-		input_abs_set_res(input_dev, ABS_Y, MXT_PIXELS_PER_MM);
-		input_abs_set_res(input_dev, ABS_MT_POSITION_X,
-				  MXT_PIXELS_PER_MM);
-		input_abs_set_res(input_dev, ABS_MT_POSITION_Y,
-				  MXT_PIXELS_PER_MM);
-
-		input_dev->name = "Atmel maXTouch Touchpad";
 	}
 
-	/* For single touch */
-	input_set_abs_params(input_dev, ABS_X,
-			     0, data->max_x, 0, 0);
-	input_set_abs_params(input_dev, ABS_Y,
-			     0, data->max_y, 0, 0);
-	input_set_abs_params(input_dev, ABS_PRESSURE,
-			     0, 255, 0, 0);
-
 	/* For multi touch */
-	num_mt_slots = data->T9_reportid_max - data->T9_reportid_min + 1;
 	error = input_mt_init_slots(input_dev, num_mt_slots, mt_flags);
 	if (error) {
 		dev_err(dev, "Error %d initialising slots\n", error);
 		goto err_free_mem;
 	}
 
-	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
-			     0, MXT_MAX_AREA, 0, 0);
+	if (data->multitouch == MXT_TOUCH_MULTITOUCHSCREEN_T100) {
+		input_set_abs_params(input_dev, ABS_MT_TOOL_TYPE,
+				     0, MT_TOOL_MAX, 0, 0);
+		input_set_abs_params(input_dev, ABS_MT_DISTANCE,
+				     MXT_DISTANCE_ACTIVE_TOUCH,
+				     MXT_DISTANCE_HOVERING,
+				     0, 0);
+	}
+
 	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
 			     0, data->max_x, 0, 0);
 	input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
 			     0, data->max_y, 0, 0);
-	input_set_abs_params(input_dev, ABS_MT_PRESSURE,
-			     0, 255, 0, 0);
+
+	if (data->multitouch == MXT_TOUCH_MULTI_T9 ||
+	    (data->multitouch == MXT_TOUCH_MULTITOUCHSCREEN_T100 &&
+	     data->t100_aux_area)) {
+		input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
+				     0, MXT_MAX_AREA, 0, 0);
+	}
+
+	if (data->multitouch == MXT_TOUCH_MULTI_T9 ||
+	    (data->multitouch == MXT_TOUCH_MULTITOUCHSCREEN_T100 &&
+	     data->t100_aux_ampl)) {
+		input_set_abs_params(input_dev, ABS_MT_PRESSURE,
+				     0, 255, 0, 0);
+	}
+
+	if (data->multitouch == MXT_TOUCH_MULTITOUCHSCREEN_T100 &&
+	    data->t100_aux_vect) {
+		input_set_abs_params(input_dev, ABS_MT_ORIENTATION,
+				     0, 255, 0, 0);
+	}
+
+	if (data->multitouch == MXT_TOUCH_MULTITOUCHSCREEN_T100 &&
+	    data->t100_aux_ampl) {
+		input_set_abs_params(input_dev, ABS_MT_PRESSURE,
+				     0, 255, 0, 0);
+	}
+
+	if (data->multitouch == MXT_TOUCH_MULTITOUCHSCREEN_T100 &&
+	    data->t100_aux_vect) {
+		input_set_abs_params(input_dev, ABS_MT_ORIENTATION,
+				     0, 255, 0, 0);
+	}
 
 	input_set_drvdata(input_dev, data);
 
@@ -1765,9 +2062,13 @@ static int mxt_configure_objects(struct mxt_data *data,
 			dev_warn(dev, "Error %d updating config\n", error);
 	}
 
-	error = mxt_initialize_t9_input_device(data);
-	if (error)
-		return error;
+	if (data->multitouch) {
+		error = mxt_initialize_input_device(data);
+		if (error)
+			return error;
+	} else {
+		dev_warn(dev, "No touch object detected\n");
+	}
 
 	dev_info(dev,
 		 "Family: %u Variant: %u Firmware V%u.%u.%02X Objects: %u\n",
@@ -2044,15 +2345,13 @@ static const struct attribute_group mxt_attr_group = {
 static void mxt_start(struct mxt_data *data)
 {
 	/* Touch enable */
-	mxt_write_object(data,
-			MXT_TOUCH_MULTI_T9, MXT_TOUCH_CTRL, 0x83);
+	mxt_write_object(data, data->multitouch, MXT_TOUCH_CTRL, 0x83);
 }
 
 static void mxt_stop(struct mxt_data *data)
 {
 	/* Touch disable */
-	mxt_write_object(data,
-			MXT_TOUCH_MULTI_T9, MXT_TOUCH_CTRL, 0);
+	mxt_write_object(data, data->multitouch, MXT_TOUCH_CTRL, 0);
 }
 
 static int mxt_input_open(struct input_dev *dev)
diff --git a/drivers/input/touchscreen/bcm_iproc_tsc.c b/drivers/input/touchscreen/bcm_iproc_tsc.c
new file mode 100644
index 000000000000..ae460a5c93d5
--- /dev/null
+++ b/drivers/input/touchscreen/bcm_iproc_tsc.c
@@ -0,0 +1,522 @@
+/*
+* 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/module.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/keyboard.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <asm/irq.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/serio.h>
+
+#define IPROC_TS_NAME "iproc-ts"
+
+#define PEN_DOWN_STATUS     1
+#define PEN_UP_STATUS       0
+
+#define X_MIN               0
+#define Y_MIN               0
+#define X_MAX               0xFFF
+#define Y_MAX               0xFFF
+
+/* Value given by controller for invalid coordinate. */
+#define INVALID_COORD       0xFFFFFFFF
+
+/* Register offsets */
+#define REGCTL1             0x00
+#define REGCTL2             0x04
+#define INTERRUPT_THRES     0x08
+#define INTERRUPT_MASK      0x0c
+
+#define INTERRUPT_STATUS    0x10
+#define CONTROLLER_STATUS   0x14
+#define FIFO_DATA           0x18
+#define FIFO_DATA_X_Y_MASK  0xFFFF
+#define ANALOG_CONTROL      0x1c
+
+#define AUX_DATA            0x20
+#define DEBOUNCE_CNTR_STAT  0x24
+#define SCAN_CNTR_STAT      0x28
+#define REM_CNTR_STAT       0x2c
+
+#define SETTLING_TIMER_STAT 0x30
+#define SPARE_REG           0x34
+#define SOFT_BYPASS_CONTROL 0x38
+#define SOFT_BYPASS_DATA    0x3c
+
+
+/* Bit values for INTERRUPT_MASK and INTERRUPT_STATUS regs */
+#define TS_PEN_INTR_MASK        BIT(0)
+#define TS_FIFO_INTR_MASK       BIT(2)
+
+/* Bit values for CONTROLLER_STATUS reg1 */
+#define TS_PEN_DOWN             BIT(0)
+
+/* Shift values for control reg1 */
+#define SCANNING_PERIOD_SHIFT   24
+#define DEBOUNCE_TIMEOUT_SHIFT  16
+#define SETTLING_TIMEOUT_SHIFT  8
+#define TOUCH_TIMEOUT_SHIFT     0
+
+/* Shift values for coordinates from fifo */
+#define X_COORD_SHIFT  0
+#define Y_COORD_SHIFT  16
+
+/* Bit values for REGCTL2 */
+#define TS_CONTROLLER_EN_BIT    BIT(16)
+#define TS_CONTROLLER_AVGDATA_SHIFT 8
+#define TS_CONTROLLER_AVGDATA_MASK (0x7 << TS_CONTROLLER_AVGDATA_SHIFT)
+#define TS_CONTROLLER_PWR_LDO   BIT(5)
+#define TS_CONTROLLER_PWR_ADC   BIT(4)
+#define TS_CONTROLLER_PWR_BGP   BIT(3)
+#define TS_CONTROLLER_PWR_TS    BIT(2)
+#define TS_WIRE_MODE_BIT        BIT(1)
+
+#define dbg_reg(dev, priv, reg) \
+	dev_dbg(dev, "%20s= 0x%08x\n", #reg, readl((priv)->regs + reg))
+
+struct tsc_param {
+	/* Each step is 1024 us.  Valid 1-256 */
+	u32 scanning_period;
+
+	/*  Each step is 512 us.  Valid 0-255 */
+	u32 debounce_timeout;
+
+	/*
+	 * The settling duration (in ms) is the amount of time the tsc
+	 * waits to allow the voltage to settle after turning on the
+	 * drivers in detection mode. Valid values: 0-11
+	 *   0 =  0.008 ms
+	 *   1 =  0.01 ms
+	 *   2 =  0.02 ms
+	 *   3 =  0.04 ms
+	 *   4 =  0.08 ms
+	 *   5 =  0.16 ms
+	 *   6 =  0.32 ms
+	 *   7 =  0.64 ms
+	 *   8 =  1.28 ms
+	 *   9 =  2.56 ms
+	 *   10 = 5.12 ms
+	 *   11 = 10.24 ms
+	 */
+	u32 settling_timeout;
+
+	/* touch timeout in sample counts */
+	u32 touch_timeout;
+
+	/*
+	 * Number of data samples which are averaged before a final data point
+	 * is placed into the FIFO
+	 */
+	u32 average_data;
+
+	/* FIFO threshold */
+	u32 fifo_threshold;
+
+	/* Optional standard touchscreen properties. */
+	u32 max_x;
+	u32 max_y;
+	u32 fuzz_x;
+	u32 fuzz_y;
+	bool invert_x;
+	bool invert_y;
+};
+
+struct iproc_ts_priv {
+	struct platform_device *pdev;
+	struct input_dev *idev;
+
+	void __iomem *regs;
+	struct clk *tsc_clk;
+
+	int  pen_status;
+	struct tsc_param cfg_params;
+};
+
+/*
+ * Set default values the same as hardware reset values
+ * except for fifo_threshold with is set to 1.
+ */
+static const struct tsc_param iproc_default_config = {
+	.scanning_period  = 0x5,  /* 1 to 256 */
+	.debounce_timeout = 0x28, /* 0 to 255 */
+	.settling_timeout = 0x7,  /* 0 to 11 */
+	.touch_timeout    = 0xa,  /* 0 to 255 */
+	.average_data     = 5,    /* entry 5 = 32 pts */
+	.fifo_threshold   = 1,    /* 0 to 31 */
+	.max_x            = X_MAX,
+	.max_y            = Y_MAX,
+};
+
+static void ts_reg_dump(struct iproc_ts_priv *priv)
+{
+	struct device *dev = &priv->pdev->dev;
+
+	dbg_reg(dev, priv, REGCTL1);
+	dbg_reg(dev, priv, REGCTL2);
+	dbg_reg(dev, priv, INTERRUPT_THRES);
+	dbg_reg(dev, priv, INTERRUPT_MASK);
+	dbg_reg(dev, priv, INTERRUPT_STATUS);
+	dbg_reg(dev, priv, CONTROLLER_STATUS);
+	dbg_reg(dev, priv, FIFO_DATA);
+	dbg_reg(dev, priv, ANALOG_CONTROL);
+	dbg_reg(dev, priv, AUX_DATA);
+	dbg_reg(dev, priv, DEBOUNCE_CNTR_STAT);
+	dbg_reg(dev, priv, SCAN_CNTR_STAT);
+	dbg_reg(dev, priv, REM_CNTR_STAT);
+	dbg_reg(dev, priv, SETTLING_TIMER_STAT);
+	dbg_reg(dev, priv, SPARE_REG);
+	dbg_reg(dev, priv, SOFT_BYPASS_CONTROL);
+	dbg_reg(dev, priv, SOFT_BYPASS_DATA);
+}
+
+static irqreturn_t iproc_touchscreen_interrupt(int irq, void *data)
+{
+	struct platform_device *pdev = data;
+	struct iproc_ts_priv *priv = platform_get_drvdata(pdev);
+	u32 intr_status;
+	u32 raw_coordinate;
+	u16 x;
+	u16 y;
+	int i;
+	bool needs_sync = false;
+
+	intr_status = readl(priv->regs + INTERRUPT_STATUS);
+	intr_status &= TS_PEN_INTR_MASK | TS_FIFO_INTR_MASK;
+	if (intr_status == 0)
+		return IRQ_NONE;
+
+	/* Clear all interrupt status bits, write-1-clear */
+	writel(intr_status, priv->regs + INTERRUPT_STATUS);
+
+	/* Pen up/down */
+	if (intr_status & TS_PEN_INTR_MASK) {
+		if (readl(priv->regs + CONTROLLER_STATUS) & TS_PEN_DOWN)
+			priv->pen_status = PEN_DOWN_STATUS;
+		else
+			priv->pen_status = PEN_UP_STATUS;
+
+		input_report_key(priv->idev, BTN_TOUCH, priv->pen_status);
+		needs_sync = true;
+
+		dev_dbg(&priv->pdev->dev,
+			"pen up-down (%d)\n", priv->pen_status);
+	}
+
+	/* coordinates in FIFO exceed the theshold */
+	if (intr_status & TS_FIFO_INTR_MASK) {
+		for (i = 0; i < priv->cfg_params.fifo_threshold; i++) {
+			raw_coordinate = readl(priv->regs + FIFO_DATA);
+			if (raw_coordinate == INVALID_COORD)
+				continue;
+
+			/*
+			 * The x and y coordinate are 16 bits each
+			 * with the x in the lower 16 bits and y in the
+			 * upper 16 bits.
+			 */
+			x = (raw_coordinate >> X_COORD_SHIFT) &
+				FIFO_DATA_X_Y_MASK;
+			y = (raw_coordinate >> Y_COORD_SHIFT) &
+				FIFO_DATA_X_Y_MASK;
+
+			/* We only want to retain the 12 msb of the 16 */
+			x = (x >> 4) & 0x0FFF;
+			y = (y >> 4) & 0x0FFF;
+
+			/* adjust x y according to lcd tsc mount angle */
+			if (priv->cfg_params.invert_x)
+				x = priv->cfg_params.max_x - x;
+
+			if (priv->cfg_params.invert_y)
+				y = priv->cfg_params.max_y - y;
+
+			input_report_abs(priv->idev, ABS_X, x);
+			input_report_abs(priv->idev, ABS_Y, y);
+			needs_sync = true;
+
+			dev_dbg(&priv->pdev->dev, "xy (0x%x 0x%x)\n", x, y);
+		}
+	}
+
+	if (needs_sync)
+		input_sync(priv->idev);
+
+	return IRQ_HANDLED;
+}
+
+static int iproc_ts_start(struct input_dev *idev)
+{
+	struct iproc_ts_priv *priv = input_get_drvdata(idev);
+	u32 val;
+	int error;
+
+	/* Enable clock */
+	error = clk_prepare_enable(priv->tsc_clk);
+	if (error) {
+		dev_err(&priv->pdev->dev, "%s clk_prepare_enable failed %d\n",
+			__func__, error);
+		return error;
+	}
+
+	/*
+	 * Interrupt is generated when:
+	 *  FIFO reaches the int_th value, and pen event(up/down)
+	 */
+	val = TS_PEN_INTR_MASK | TS_FIFO_INTR_MASK;
+	writel(val, priv->regs + INTERRUPT_MASK);
+
+	writel(priv->cfg_params.fifo_threshold, priv->regs + INTERRUPT_THRES);
+
+	/* Initialize control reg1 */
+	val = 0;
+	val |= priv->cfg_params.scanning_period << SCANNING_PERIOD_SHIFT;
+	val |= priv->cfg_params.debounce_timeout << DEBOUNCE_TIMEOUT_SHIFT;
+	val |= priv->cfg_params.settling_timeout << SETTLING_TIMEOUT_SHIFT;
+	val |= priv->cfg_params.touch_timeout << TOUCH_TIMEOUT_SHIFT;
+	writel(val, priv->regs + REGCTL1);
+
+	/* Try to clear all interrupt status */
+	val = readl(priv->regs + INTERRUPT_STATUS);
+	val |= TS_FIFO_INTR_MASK | TS_PEN_INTR_MASK;
+	writel(val, priv->regs + INTERRUPT_STATUS);
+
+	/* Initialize control reg2 */
+	val = readl(priv->regs + REGCTL2);
+	val |= TS_CONTROLLER_EN_BIT | TS_WIRE_MODE_BIT;
+
+	val &= ~TS_CONTROLLER_AVGDATA_MASK;
+	val |= priv->cfg_params.average_data << TS_CONTROLLER_AVGDATA_SHIFT;
+
+	val &= ~(TS_CONTROLLER_PWR_LDO |	/* PWR up LDO */
+		   TS_CONTROLLER_PWR_ADC |	/* PWR up ADC */
+		   TS_CONTROLLER_PWR_BGP |	/* PWR up BGP */
+		   TS_CONTROLLER_PWR_TS);	/* PWR up TS */
+
+	writel(val, priv->regs + REGCTL2);
+
+	ts_reg_dump(priv);
+
+	return 0;
+}
+
+static void iproc_ts_stop(struct input_dev *dev)
+{
+	u32 val;
+	struct iproc_ts_priv *priv = input_get_drvdata(dev);
+
+	writel(0, priv->regs + INTERRUPT_MASK); /* Disable all interrupts */
+
+	/* Only power down touch screen controller */
+	val = readl(priv->regs + REGCTL2);
+	val |= TS_CONTROLLER_PWR_TS;
+	writel(val, priv->regs + REGCTL2);
+
+	clk_disable(priv->tsc_clk);
+}
+
+static int iproc_get_tsc_config(struct device *dev, struct iproc_ts_priv *priv)
+{
+	struct device_node *np = dev->of_node;
+	u32 val;
+
+	priv->cfg_params = iproc_default_config;
+
+	if (!np)
+		return 0;
+
+	if (of_property_read_u32(np, "scanning_period", &val) >= 0) {
+		if (val < 1 || val > 256) {
+			dev_err(dev, "scanning_period (%u) must be [1-256]\n",
+				val);
+			return -EINVAL;
+		}
+		priv->cfg_params.scanning_period = val;
+	}
+
+	if (of_property_read_u32(np, "debounce_timeout", &val) >= 0) {
+		if (val > 255) {
+			dev_err(dev, "debounce_timeout (%u) must be [0-255]\n",
+				val);
+			return -EINVAL;
+		}
+		priv->cfg_params.debounce_timeout = val;
+	}
+
+	if (of_property_read_u32(np, "settling_timeout", &val) >= 0) {
+		if (val > 11) {
+			dev_err(dev, "settling_timeout (%u) must be [0-11]\n",
+				val);
+			return -EINVAL;
+		}
+		priv->cfg_params.settling_timeout = val;
+	}
+
+	if (of_property_read_u32(np, "touch_timeout", &val) >= 0) {
+		if (val > 255) {
+			dev_err(dev, "touch_timeout (%u) must be [0-255]\n",
+				val);
+			return -EINVAL;
+		}
+		priv->cfg_params.touch_timeout = val;
+	}
+
+	if (of_property_read_u32(np, "average_data", &val) >= 0) {
+		if (val > 8) {
+			dev_err(dev, "average_data (%u) must be [0-8]\n", val);
+			return -EINVAL;
+		}
+		priv->cfg_params.average_data = val;
+	}
+
+	if (of_property_read_u32(np, "fifo_threshold", &val) >= 0) {
+		if (val > 31) {
+			dev_err(dev, "fifo_threshold (%u)) must be [0-31]\n",
+				val);
+			return -EINVAL;
+		}
+		priv->cfg_params.fifo_threshold = val;
+	}
+
+	/* Parse optional properties. */
+	of_property_read_u32(np, "touchscreen-size-x", &priv->cfg_params.max_x);
+	of_property_read_u32(np, "touchscreen-size-y", &priv->cfg_params.max_y);
+
+	of_property_read_u32(np, "touchscreen-fuzz-x",
+			     &priv->cfg_params.fuzz_x);
+	of_property_read_u32(np, "touchscreen-fuzz-y",
+			     &priv->cfg_params.fuzz_y);
+
+	priv->cfg_params.invert_x =
+		of_property_read_bool(np, "touchscreen-inverted-x");
+	priv->cfg_params.invert_y =
+		of_property_read_bool(np, "touchscreen-inverted-y");
+
+	return 0;
+}
+
+static int iproc_ts_probe(struct platform_device *pdev)
+{
+	struct iproc_ts_priv *priv;
+	struct input_dev *idev;
+	struct resource *res;
+	int irq;
+	int error;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	/* touchscreen controller memory mapped regs */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->regs)) {
+		error = PTR_ERR(priv->regs);
+		dev_err(&pdev->dev, "unable to map I/O memory: %d\n", error);
+		return error;
+	}
+
+	priv->tsc_clk = devm_clk_get(&pdev->dev, "tsc_clk");
+	if (IS_ERR(priv->tsc_clk)) {
+		error = PTR_ERR(priv->tsc_clk);
+		dev_err(&pdev->dev,
+			"failed getting clock tsc_clk: %d\n", error);
+		return error;
+	}
+
+	priv->pdev = pdev;
+	error = iproc_get_tsc_config(&pdev->dev, priv);
+	if (error) {
+		dev_err(&pdev->dev, "get_tsc_config failed: %d\n", error);
+		return error;
+	}
+
+	idev = devm_input_allocate_device(&pdev->dev);
+	if (!idev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	priv->idev = idev;
+	priv->pen_status = PEN_UP_STATUS;
+
+	/* Set input device info  */
+	idev->name = IPROC_TS_NAME;
+	idev->dev.parent = &pdev->dev;
+
+	idev->id.bustype = BUS_HOST;
+	idev->id.vendor = SERIO_UNKNOWN;
+	idev->id.product = 0;
+	idev->id.version = 0;
+
+	idev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	__set_bit(BTN_TOUCH, idev->keybit);
+
+	input_set_abs_params(idev, ABS_X, X_MIN, priv->cfg_params.max_x,
+			     priv->cfg_params.fuzz_x, 0);
+	input_set_abs_params(idev, ABS_Y, Y_MIN, priv->cfg_params.max_y,
+			     priv->cfg_params.fuzz_y, 0);
+
+	idev->open = iproc_ts_start;
+	idev->close = iproc_ts_stop;
+
+	input_set_drvdata(idev, priv);
+	platform_set_drvdata(pdev, priv);
+
+	/* get interrupt */
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "platform_get_irq failed: %d\n", irq);
+		return irq;
+	}
+
+	error = devm_request_irq(&pdev->dev, irq,
+				 iproc_touchscreen_interrupt,
+				 IRQF_SHARED, IPROC_TS_NAME, pdev);
+	if (error)
+		return error;
+
+	error = input_register_device(priv->idev);
+	if (error) {
+		dev_err(&pdev->dev,
+			"failed to register input device: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id iproc_ts_of_match[] = {
+	{.compatible = "brcm,iproc-touchscreen", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, iproc_ts_of_match);
+
+static struct platform_driver iproc_ts_driver = {
+	.probe = iproc_ts_probe,
+	.driver = {
+		.name	= IPROC_TS_NAME,
+		.of_match_table = of_match_ptr(iproc_ts_of_match),
+	},
+};
+
+module_platform_driver(iproc_ts_driver);
+
+MODULE_DESCRIPTION("IPROC Touchscreen driver");
+MODULE_AUTHOR("Broadcom");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/touchscreen/chipone_icn8318.c b/drivers/input/touchscreen/chipone_icn8318.c
new file mode 100644
index 000000000000..32e9db0e04bf
--- /dev/null
+++ b/drivers/input/touchscreen/chipone_icn8318.c
@@ -0,0 +1,316 @@
+/*
+ * Driver for ChipOne icn8318 i2c touchscreen controller
+ *
+ * Copyright (c) 2015 Red Hat Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Red Hat authors:
+ * Hans de Goede <hdegoede@redhat.com>
+ */
+
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/input/mt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+
+#define ICN8318_REG_POWER		4
+#define ICN8318_REG_TOUCHDATA		16
+
+#define ICN8318_POWER_ACTIVE		0
+#define ICN8318_POWER_MONITOR		1
+#define ICN8318_POWER_HIBERNATE		2
+
+#define ICN8318_MAX_TOUCHES		5
+
+struct icn8318_touch {
+	__u8 slot;
+	__be16 x;
+	__be16 y;
+	__u8 pressure;	/* Seems more like finger width then pressure really */
+	__u8 event;
+/* The difference between 2 and 3 is unclear */
+#define ICN8318_EVENT_NO_DATA	1 /* No finger seen yet since wakeup */
+#define ICN8318_EVENT_UPDATE1	2 /* New or updated coordinates */
+#define ICN8318_EVENT_UPDATE2	3 /* New or updated coordinates */
+#define ICN8318_EVENT_END	4 /* Finger lifted */
+} __packed;
+
+struct icn8318_touch_data {
+	__u8 softbutton;
+	__u8 touch_count;
+	struct icn8318_touch touches[ICN8318_MAX_TOUCHES];
+} __packed;
+
+struct icn8318_data {
+	struct i2c_client *client;
+	struct input_dev *input;
+	struct gpio_desc *wake_gpio;
+	u32 max_x;
+	u32 max_y;
+	bool invert_x;
+	bool invert_y;
+	bool swap_x_y;
+};
+
+static int icn8318_read_touch_data(struct i2c_client *client,
+				   struct icn8318_touch_data *touch_data)
+{
+	u8 reg = ICN8318_REG_TOUCHDATA;
+	struct i2c_msg msg[2] = {
+		{
+			.addr = client->addr,
+			.len = 1,
+			.buf = &reg
+		},
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = sizeof(struct icn8318_touch_data),
+			.buf = (u8 *)touch_data
+		}
+	};
+
+	return i2c_transfer(client->adapter, msg, 2);
+}
+
+static inline bool icn8318_touch_active(u8 event)
+{
+	return (event == ICN8318_EVENT_UPDATE1) ||
+	       (event == ICN8318_EVENT_UPDATE2);
+}
+
+static irqreturn_t icn8318_irq(int irq, void *dev_id)
+{
+	struct icn8318_data *data = dev_id;
+	struct device *dev = &data->client->dev;
+	struct icn8318_touch_data touch_data;
+	int i, ret, x, y;
+
+	ret = icn8318_read_touch_data(data->client, &touch_data);
+	if (ret < 0) {
+		dev_err(dev, "Error reading touch data: %d\n", ret);
+		return IRQ_HANDLED;
+	}
+
+	if (touch_data.softbutton) {
+		/*
+		 * Other data is invalid when a softbutton is pressed.
+		 * This needs some extra devicetree bindings to map the icn8318
+		 * softbutton codes to evdev codes. Currently no known devices
+		 * use this.
+		 */
+		return IRQ_HANDLED;
+	}
+
+	if (touch_data.touch_count > ICN8318_MAX_TOUCHES) {
+		dev_warn(dev, "Too much touches %d > %d\n",
+			 touch_data.touch_count, ICN8318_MAX_TOUCHES);
+		touch_data.touch_count = ICN8318_MAX_TOUCHES;
+	}
+
+	for (i = 0; i < touch_data.touch_count; i++) {
+		struct icn8318_touch *touch = &touch_data.touches[i];
+		bool act = icn8318_touch_active(touch->event);
+
+		input_mt_slot(data->input, touch->slot);
+		input_mt_report_slot_state(data->input, MT_TOOL_FINGER, act);
+		if (!act)
+			continue;
+
+		x = be16_to_cpu(touch->x);
+		y = be16_to_cpu(touch->y);
+
+		if (data->invert_x)
+			x = data->max_x - x;
+
+		if (data->invert_y)
+			y = data->max_y - y;
+
+		if (!data->swap_x_y) {
+			input_event(data->input, EV_ABS, ABS_MT_POSITION_X, x);
+			input_event(data->input, EV_ABS, ABS_MT_POSITION_Y, y);
+		} else {
+			input_event(data->input, EV_ABS, ABS_MT_POSITION_X, y);
+			input_event(data->input, EV_ABS, ABS_MT_POSITION_Y, x);
+		}
+	}
+
+	input_mt_sync_frame(data->input);
+	input_sync(data->input);
+
+	return IRQ_HANDLED;
+}
+
+static int icn8318_start(struct input_dev *dev)
+{
+	struct icn8318_data *data = input_get_drvdata(dev);
+
+	enable_irq(data->client->irq);
+	gpiod_set_value_cansleep(data->wake_gpio, 1);
+
+	return 0;
+}
+
+static void icn8318_stop(struct input_dev *dev)
+{
+	struct icn8318_data *data = input_get_drvdata(dev);
+
+	disable_irq(data->client->irq);
+	i2c_smbus_write_byte_data(data->client, ICN8318_REG_POWER,
+				  ICN8318_POWER_HIBERNATE);
+	gpiod_set_value_cansleep(data->wake_gpio, 0);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int icn8318_suspend(struct device *dev)
+{
+	struct icn8318_data *data = i2c_get_clientdata(to_i2c_client(dev));
+
+	mutex_lock(&data->input->mutex);
+	if (data->input->users)
+		icn8318_stop(data->input);
+	mutex_unlock(&data->input->mutex);
+
+	return 0;
+}
+
+static int icn8318_resume(struct device *dev)
+{
+	struct icn8318_data *data = i2c_get_clientdata(to_i2c_client(dev));
+
+	mutex_lock(&data->input->mutex);
+	if (data->input->users)
+		icn8318_start(data->input);
+	mutex_unlock(&data->input->mutex);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(icn8318_pm_ops, icn8318_suspend, icn8318_resume);
+
+static int icn8318_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct device_node *np = dev->of_node;
+	struct icn8318_data *data;
+	struct input_dev *input;
+	u32 fuzz_x = 0, fuzz_y = 0;
+	int error;
+
+	if (!client->irq) {
+		dev_err(dev, "Error no irq specified\n");
+		return -EINVAL;
+	}
+
+	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->wake_gpio = devm_gpiod_get(dev, "wake", GPIOD_OUT_LOW);
+	if (IS_ERR(data->wake_gpio)) {
+		error = PTR_ERR(data->wake_gpio);
+		if (error != -EPROBE_DEFER)
+			dev_err(dev, "Error getting wake gpio: %d\n", error);
+		return error;
+	}
+
+	if (of_property_read_u32(np, "touchscreen-size-x", &data->max_x) ||
+	    of_property_read_u32(np, "touchscreen-size-y", &data->max_y)) {
+		dev_err(dev, "Error touchscreen-size-x and/or -y missing\n");
+		return -EINVAL;
+	}
+
+	/* Optional */
+	of_property_read_u32(np, "touchscreen-fuzz-x", &fuzz_x);
+	of_property_read_u32(np, "touchscreen-fuzz-y", &fuzz_y);
+	data->invert_x = of_property_read_bool(np, "touchscreen-inverted-x");
+	data->invert_y = of_property_read_bool(np, "touchscreen-inverted-y");
+	data->swap_x_y = of_property_read_bool(np, "touchscreen-swapped-x-y");
+
+	input = devm_input_allocate_device(dev);
+	if (!input)
+		return -ENOMEM;
+
+	input->name = client->name;
+	input->id.bustype = BUS_I2C;
+	input->open = icn8318_start;
+	input->close = icn8318_stop;
+	input->dev.parent = dev;
+
+	if (!data->swap_x_y) {
+		input_set_abs_params(input, ABS_MT_POSITION_X, 0,
+				     data->max_x, fuzz_x, 0);
+		input_set_abs_params(input, ABS_MT_POSITION_Y, 0,
+				     data->max_y, fuzz_y, 0);
+	} else {
+		input_set_abs_params(input, ABS_MT_POSITION_X, 0,
+				     data->max_y, fuzz_y, 0);
+		input_set_abs_params(input, ABS_MT_POSITION_Y, 0,
+				     data->max_x, fuzz_x, 0);
+	}
+
+	error = input_mt_init_slots(input, ICN8318_MAX_TOUCHES,
+				    INPUT_MT_DIRECT | INPUT_MT_DROP_UNUSED);
+	if (error)
+		return error;
+
+	data->client = client;
+	data->input = input;
+	input_set_drvdata(input, data);
+
+	error = devm_request_threaded_irq(dev, client->irq, NULL, icn8318_irq,
+					  IRQF_ONESHOT, client->name, data);
+	if (error) {
+		dev_err(dev, "Error requesting irq: %d\n", error);
+		return error;
+	}
+
+	/* Stop device till opened */
+	icn8318_stop(data->input);
+
+	error = input_register_device(input);
+	if (error)
+		return error;
+
+	i2c_set_clientdata(client, data);
+
+	return 0;
+}
+
+static const struct of_device_id icn8318_of_match[] = {
+	{ .compatible = "chipone,icn8318" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, icn8318_of_match);
+
+/* This is useless for OF-enabled devices, but it is needed by I2C subsystem */
+static const struct i2c_device_id icn8318_i2c_id[] = {
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, icn8318_i2c_id);
+
+static struct i2c_driver icn8318_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "chipone_icn8318",
+		.pm	= &icn8318_pm_ops,
+		.of_match_table = icn8318_of_match,
+	},
+	.probe = icn8318_probe,
+	.id_table = icn8318_i2c_id,
+};
+
+module_i2c_driver(icn8318_driver);
+
+MODULE_DESCRIPTION("ChipOne icn8318 I2C Touchscreen Driver");
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index d4c24fb7704f..e6aef3e48bd9 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -37,6 +37,7 @@
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
 #include <linux/input/mt.h>
+#include <linux/input/touchscreen.h>
 #include <linux/input/edt-ft5x06.h>
 
 #define MAX_SUPPORT_POINTS		5
@@ -1034,7 +1035,6 @@ static int edt_ft5x06_ts_probe(struct i2c_client *client,
 	input->id.bustype = BUS_I2C;
 	input->dev.parent = &client->dev;
 
-	__set_bit(EV_SYN, input->evbit);
 	__set_bit(EV_KEY, input->evbit);
 	__set_bit(EV_ABS, input->evbit);
 	__set_bit(BTN_TOUCH, input->keybit);
@@ -1044,6 +1044,10 @@ static int edt_ft5x06_ts_probe(struct i2c_client *client,
 			     0, tsdata->num_x * 64 - 1, 0, 0);
 	input_set_abs_params(input, ABS_MT_POSITION_Y,
 			     0, tsdata->num_y * 64 - 1, 0, 0);
+
+	if (!pdata)
+		touchscreen_parse_of_params(input);
+
 	error = input_mt_init_slots(input, MAX_SUPPORT_POINTS, 0);
 	if (error) {
 		dev_err(&client->dev, "Unable to init MT slots.\n");
diff --git a/drivers/input/touchscreen/elants_i2c.c b/drivers/input/touchscreen/elants_i2c.c
index 926c58e540c0..43b3c9c2d788 100644
--- a/drivers/input/touchscreen/elants_i2c.c
+++ b/drivers/input/touchscreen/elants_i2c.c
@@ -98,7 +98,6 @@
 #define MAX_FW_UPDATE_RETRIES	30
 
 #define ELAN_FW_PAGESIZE	132
-#define ELAN_FW_FILENAME	"elants_i2c.bin"
 
 /* calibration timeout definition */
 #define ELAN_CALI_TIMEOUT_MSEC	10000
@@ -697,12 +696,19 @@ static int elants_i2c_fw_update(struct elants_data *ts)
 {
 	struct i2c_client *client = ts->client;
 	const struct firmware *fw;
+	char *fw_name;
 	int error;
 
-	error = request_firmware(&fw, ELAN_FW_FILENAME, &client->dev);
+	fw_name = kasprintf(GFP_KERNEL, "elants_i2c_%4x.bin", ts->hw_version);
+	if (!fw_name)
+		return -ENOMEM;
+
+	dev_info(&client->dev, "requesting fw name = %s\n", fw_name);
+	error = request_firmware(&fw, fw_name, &client->dev);
+	kfree(fw_name);
 	if (error) {
-		dev_err(&client->dev, "failed to request firmware %s: %d\n",
-			ELAN_FW_FILENAME, error);
+		dev_err(&client->dev, "failed to request firmware: %d\n",
+			error);
 		return error;
 	}
 
diff --git a/drivers/input/touchscreen/goodix.c b/drivers/input/touchscreen/goodix.c
index ca196689f025..3af16984d57c 100644
--- a/drivers/input/touchscreen/goodix.c
+++ b/drivers/input/touchscreen/goodix.c
@@ -23,6 +23,8 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/of.h>
 #include <asm/unaligned.h>
 
 struct goodix_ts_data {
@@ -48,6 +50,7 @@ struct goodix_ts_data {
 #define GOODIX_REG_VERSION		0x8140
 
 #define RESOLUTION_LOC		1
+#define MAX_CONTACTS_LOC	5
 #define TRIGGER_LOC		6
 
 static const unsigned long goodix_irq_flags[] = {
@@ -99,7 +102,7 @@ static int goodix_ts_read_input_report(struct goodix_ts_data *ts, u8 *data)
 	}
 
 	touch_num = data[0] & 0x0f;
-	if (touch_num > GOODIX_MAX_CONTACTS)
+	if (touch_num > ts->max_touch_num)
 		return -EPROTO;
 
 	if (touch_num > 1) {
@@ -141,7 +144,7 @@ static void goodix_ts_report_touch(struct goodix_ts_data *ts, u8 *coor_data)
  */
 static void goodix_process_events(struct goodix_ts_data *ts)
 {
-	u8  point_data[1 + GOODIX_CONTACT_SIZE * GOODIX_MAX_CONTACTS];
+	u8  point_data[1 + GOODIX_CONTACT_SIZE * ts->max_touch_num];
 	int touch_num;
 	int i;
 
@@ -202,21 +205,23 @@ static void goodix_read_config(struct goodix_ts_data *ts)
 		ts->abs_x_max = GOODIX_MAX_WIDTH;
 		ts->abs_y_max = GOODIX_MAX_HEIGHT;
 		ts->int_trigger_type = GOODIX_INT_TRIGGER;
+		ts->max_touch_num = GOODIX_MAX_CONTACTS;
 		return;
 	}
 
 	ts->abs_x_max = get_unaligned_le16(&config[RESOLUTION_LOC]);
 	ts->abs_y_max = get_unaligned_le16(&config[RESOLUTION_LOC + 2]);
-	ts->int_trigger_type = (config[TRIGGER_LOC]) & 0x03;
-	if (!ts->abs_x_max || !ts->abs_y_max) {
+	ts->int_trigger_type = config[TRIGGER_LOC] & 0x03;
+	ts->max_touch_num = config[MAX_CONTACTS_LOC] & 0x0f;
+	if (!ts->abs_x_max || !ts->abs_y_max || !ts->max_touch_num) {
 		dev_err(&ts->client->dev,
 			"Invalid config, using defaults\n");
 		ts->abs_x_max = GOODIX_MAX_WIDTH;
 		ts->abs_y_max = GOODIX_MAX_HEIGHT;
+		ts->max_touch_num = GOODIX_MAX_CONTACTS;
 	}
 }
 
-
 /**
  * goodix_read_version - Read goodix touchscreen version
  *
@@ -295,7 +300,7 @@ static int goodix_request_input_dev(struct goodix_ts_data *ts)
 	input_set_abs_params(ts->input_dev, ABS_MT_WIDTH_MAJOR, 0, 255, 0, 0);
 	input_set_abs_params(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0, 255, 0, 0);
 
-	input_mt_init_slots(ts->input_dev, GOODIX_MAX_CONTACTS,
+	input_mt_init_slots(ts->input_dev, ts->max_touch_num,
 			    INPUT_MT_DIRECT | INPUT_MT_DROP_UNUSED);
 
 	ts->input_dev->name = "Goodix Capacitive TouchScreen";
@@ -372,11 +377,27 @@ static const struct i2c_device_id goodix_ts_id[] = {
 	{ }
 };
 
+#ifdef CONFIG_ACPI
 static const struct acpi_device_id goodix_acpi_match[] = {
 	{ "GDIX1001", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, goodix_acpi_match);
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id goodix_of_match[] = {
+	{ .compatible = "goodix,gt911" },
+	{ .compatible = "goodix,gt9110" },
+	{ .compatible = "goodix,gt912" },
+	{ .compatible = "goodix,gt927" },
+	{ .compatible = "goodix,gt9271" },
+	{ .compatible = "goodix,gt928" },
+	{ .compatible = "goodix,gt967" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, goodix_of_match);
+#endif
 
 static struct i2c_driver goodix_ts_driver = {
 	.probe = goodix_ts_probe,
@@ -384,7 +405,8 @@ static struct i2c_driver goodix_ts_driver = {
 	.driver = {
 		.name = "Goodix-TS",
 		.owner = THIS_MODULE,
-		.acpi_match_table = goodix_acpi_match,
+		.acpi_match_table = ACPI_PTR(goodix_acpi_match),
+		.of_match_table = of_match_ptr(goodix_of_match),
 	},
 };
 module_i2c_driver(goodix_ts_driver);
diff --git a/drivers/input/touchscreen/of_touchscreen.c b/drivers/input/touchscreen/of_touchscreen.c
index f8f9b84230b1..b82b5207c78b 100644
--- a/drivers/input/touchscreen/of_touchscreen.c
+++ b/drivers/input/touchscreen/of_touchscreen.c
@@ -11,8 +11,41 @@
 
 #include <linux/of.h>
 #include <linux/input.h>
+#include <linux/input/mt.h>
 #include <linux/input/touchscreen.h>
 
+static u32 of_get_optional_u32(struct device_node *np,
+			       const char *property)
+{
+	u32 val = 0;
+
+	of_property_read_u32(np, property, &val);
+
+	return val;
+}
+
+static void touchscreen_set_params(struct input_dev *dev,
+				   unsigned long axis,
+				   int max, int fuzz)
+{
+	struct input_absinfo *absinfo;
+
+	if (!test_bit(axis, dev->absbit)) {
+		/*
+		 * Emit a warning only if the axis is not a multitouch
+		 * axis, which might not be set by the driver.
+		 */
+		if (!input_is_mt_axis(axis))
+			dev_warn(&dev->dev,
+				 "DT specifies parameters but the axis is not set up\n");
+		return;
+	}
+
+	absinfo = &dev->absinfo[axis];
+	absinfo->maximum = max;
+	absinfo->fuzz = fuzz;
+}
+
 /**
  * touchscreen_parse_of_params - parse common touchscreen DT properties
  * @dev: device that should be parsed
@@ -24,22 +57,31 @@
 void touchscreen_parse_of_params(struct input_dev *dev)
 {
 	struct device_node *np = dev->dev.parent->of_node;
-	struct input_absinfo *absinfo;
+	u32 maximum, fuzz;
 
 	input_alloc_absinfo(dev);
 	if (!dev->absinfo)
 		return;
 
-	absinfo = &dev->absinfo[ABS_X];
-	of_property_read_u32(np, "touchscreen-size-x", &absinfo->maximum);
-	of_property_read_u32(np, "touchscreen-fuzz-x", &absinfo->fuzz);
+	maximum = of_get_optional_u32(np, "touchscreen-size-x");
+	fuzz = of_get_optional_u32(np, "touchscreen-fuzz-x");
+	if (maximum || fuzz) {
+		touchscreen_set_params(dev, ABS_X, maximum, fuzz);
+		touchscreen_set_params(dev, ABS_MT_POSITION_X, maximum, fuzz);
+	}
 
-	absinfo = &dev->absinfo[ABS_Y];
-	of_property_read_u32(np, "touchscreen-size-y", &absinfo->maximum);
-	of_property_read_u32(np, "touchscreen-fuzz-y", &absinfo->fuzz);
+	maximum = of_get_optional_u32(np, "touchscreen-size-y");
+	fuzz = of_get_optional_u32(np, "touchscreen-fuzz-y");
+	if (maximum || fuzz) {
+		touchscreen_set_params(dev, ABS_Y, maximum, fuzz);
+		touchscreen_set_params(dev, ABS_MT_POSITION_Y, maximum, fuzz);
+	}
 
-	absinfo = &dev->absinfo[ABS_PRESSURE];
-	of_property_read_u32(np, "touchscreen-max-pressure", &absinfo->maximum);
-	of_property_read_u32(np, "touchscreen-fuzz-pressure", &absinfo->fuzz);
+	maximum = of_get_optional_u32(np, "touchscreen-max-pressure");
+	fuzz = of_get_optional_u32(np, "touchscreen-fuzz-pressure");
+	if (maximum || fuzz) {
+		touchscreen_set_params(dev, ABS_PRESSURE, maximum, fuzz);
+		touchscreen_set_params(dev, ABS_MT_PRESSURE, maximum, fuzz);
+	}
 }
 EXPORT_SYMBOL(touchscreen_parse_of_params);
diff --git a/drivers/input/touchscreen/sun4i-ts.c b/drivers/input/touchscreen/sun4i-ts.c
index b93a28b955fd..c0116994067d 100644
--- a/drivers/input/touchscreen/sun4i-ts.c
+++ b/drivers/input/touchscreen/sun4i-ts.c
@@ -30,6 +30,10 @@
  * These kinds of heuristics are just asking for trouble (and don't belong
  * in the kernel). So this driver offers straight forward, reliable single
  * touch functionality only.
+ *
+ * s.a. A20 User Manual "1.15 TP" (Documentation/arm/sunxi/README)
+ * (looks like the description in the A20 User Manual v1.3 is better
+ * than the one in the A10 User Manual v.1.5)
  */
 
 #include <linux/err.h>
@@ -193,7 +197,7 @@ static int sun4i_get_temp(const struct sun4i_ts_data *ts, long *temp)
 	if (ts->temp_data == -1)
 		return -EAGAIN;
 
-	*temp = (ts->temp_data - ts->temp_offset) * ts->temp_step;
+	*temp = ts->temp_data * ts->temp_step - ts->temp_offset;
 
 	return 0;
 }
@@ -246,6 +250,8 @@ static int sun4i_ts_probe(struct platform_device *pdev)
 	int error;
 	u32 reg;
 	bool ts_attached;
+	u32 tp_sensitive_adjust = 15;
+	u32 filter_type = 1;
 
 	ts = devm_kzalloc(dev, sizeof(struct sun4i_ts_data), GFP_KERNEL);
 	if (!ts)
@@ -255,22 +261,31 @@ static int sun4i_ts_probe(struct platform_device *pdev)
 	ts->ignore_fifo_data = true;
 	ts->temp_data = -1;
 	if (of_device_is_compatible(np, "allwinner,sun6i-a31-ts")) {
-		/* Allwinner SDK has temperature = -271 + (value / 6) (C) */
-		ts->temp_offset = 1626;
+		/* Allwinner SDK has temperature (C) = (value / 6) - 271 */
+		ts->temp_offset = 271000;
 		ts->temp_step = 167;
+	} else if (of_device_is_compatible(np, "allwinner,sun4i-a10-ts")) {
+		/*
+		 * The A10 temperature sensor has quite a wide spread, these
+		 * parameters are based on the averaging of the calibration
+		 * results of 4 completely different boards, with a spread of
+		 * temp_step from 0.096 - 0.170 and temp_offset from 176 - 331.
+		 */
+		ts->temp_offset = 257000;
+		ts->temp_step = 133;
 	} else {
 		/*
 		 * The user manuals do not contain the formula for calculating
 		 * the temperature. The formula used here is from the AXP209,
 		 * which is designed by X-Powers, an affiliate of Allwinner:
 		 *
-		 *     temperature = -144.7 + (value * 0.1)
+		 *     temperature (C) = (value * 0.1) - 144.7
 		 *
 		 * Allwinner does not have any documentation whatsoever for
 		 * this hardware. Moreover, it is claimed that the sensor
 		 * is inaccurate and cannot work properly.
 		 */
-		ts->temp_offset = 1447;
+		ts->temp_offset = 144700;
 		ts->temp_step = 100;
 	}
 
@@ -313,14 +328,20 @@ static int sun4i_ts_probe(struct platform_device *pdev)
 	       ts->base + TP_CTRL0);
 
 	/*
-	 * sensitive_adjust = 15 : max, which is not all that sensitive,
+	 * tp_sensitive_adjust is an optional property
 	 * tp_mode = 0 : only x and y coordinates, as we don't use dual touch
 	 */
-	writel(TP_SENSITIVE_ADJUST(15) | TP_MODE_SELECT(0),
+	of_property_read_u32(np, "allwinner,tp-sensitive-adjust",
+			     &tp_sensitive_adjust);
+	writel(TP_SENSITIVE_ADJUST(tp_sensitive_adjust) | TP_MODE_SELECT(0),
 	       ts->base + TP_CTRL2);
 
-	/* Enable median filter, type 1 : 5/3 */
-	writel(FILTER_EN(1) | FILTER_TYPE(1), ts->base + TP_CTRL3);
+	/*
+	 * Enable median and averaging filter, optional property for
+	 * filter type.
+	 */
+	of_property_read_u32(np, "allwinner,filter-type", &filter_type);
+	writel(FILTER_EN(1) | FILTER_TYPE(filter_type), ts->base + TP_CTRL3);
 
 	/* Enable temperature measurement, period 1953 (2 seconds) */
 	writel(TEMP_ENABLE(1) | TEMP_PERIOD(1953), ts->base + TP_TPR);
@@ -330,10 +351,10 @@ static int sun4i_ts_probe(struct platform_device *pdev)
 	 * finally enable tp mode.
 	 */
 	reg = STYLUS_UP_DEBOUN(5) | STYLUS_UP_DEBOUN_EN(1);
-	if (of_device_is_compatible(np, "allwinner,sun4i-a10-ts"))
-		reg |= TP_MODE_EN(1);
-	else
+	if (of_device_is_compatible(np, "allwinner,sun6i-a31-ts"))
 		reg |= SUN6I_TP_MODE_EN(1);
+	else
+		reg |= TP_MODE_EN(1);
 	writel(reg, ts->base + TP_CTRL1);
 
 	/*
@@ -383,6 +404,7 @@ static int sun4i_ts_remove(struct platform_device *pdev)
 
 static const struct of_device_id sun4i_ts_of_match[] = {
 	{ .compatible = "allwinner,sun4i-a10-ts", },
+	{ .compatible = "allwinner,sun5i-a13-ts", },
 	{ .compatible = "allwinner,sun6i-a31-ts", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/input/touchscreen/sx8654.c b/drivers/input/touchscreen/sx8654.c
new file mode 100644
index 000000000000..aecb9ad2e701
--- /dev/null
+++ b/drivers/input/touchscreen/sx8654.c
@@ -0,0 +1,286 @@
+/*
+ * Driver for Semtech SX8654 I2C touchscreen controller.
+ *
+ * Copyright (c) 2015 Armadeus Systems
+ *	Sébastien Szymanski <sebastien.szymanski@armadeus.com>
+ *
+ * Using code from:
+ *  - sx865x.c
+ *	Copyright (c) 2013 U-MoBo Srl
+ *	Pierluigi Passaro <p.passaro@u-mobo.com>
+ *  - sx8650.c
+ *      Copyright (c) 2009 Wayne Roberts
+ *  - tsc2007.c
+ *      Copyright (c) 2008 Kwangwoo Lee
+ *  - ads7846.c
+ *      Copyright (c) 2005 David Brownell
+ *      Copyright (c) 2006 Nokia Corporation
+ *  - corgi_ts.c
+ *      Copyright (C) 2004-2005 Richard Purdie
+ *  - omap_ts.[hc], ads7846.h, ts_osk.c
+ *      Copyright (C) 2002 MontaVista Software
+ *      Copyright (C) 2004 Texas Instruments
+ *      Copyright (C) 2005 Dirk Behme
+ *
+ *  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/input.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+
+/* register addresses */
+#define I2C_REG_TOUCH0			0x00
+#define I2C_REG_TOUCH1			0x01
+#define I2C_REG_CHANMASK		0x04
+#define I2C_REG_IRQMASK			0x22
+#define I2C_REG_IRQSRC			0x23
+#define I2C_REG_SOFTRESET		0x3f
+
+/* commands */
+#define CMD_READ_REGISTER		0x40
+#define CMD_MANUAL			0xc0
+#define CMD_PENTRG			0xe0
+
+/* value for I2C_REG_SOFTRESET */
+#define SOFTRESET_VALUE			0xde
+
+/* bits for I2C_REG_IRQSRC */
+#define IRQ_PENTOUCH_TOUCHCONVDONE	0x08
+#define IRQ_PENRELEASE			0x04
+
+/* bits for RegTouch1 */
+#define CONDIRQ				0x20
+#define FILT_7SA			0x03
+
+/* bits for I2C_REG_CHANMASK */
+#define CONV_X				0x80
+#define CONV_Y				0x40
+
+/* coordinates rate: higher nibble of CTRL0 register */
+#define RATE_MANUAL			0x00
+#define RATE_5000CPS			0xf0
+
+/* power delay: lower nibble of CTRL0 register */
+#define POWDLY_1_1MS			0x0b
+
+#define MAX_12BIT			((1 << 12) - 1)
+
+struct sx8654 {
+	struct input_dev *input;
+	struct i2c_client *client;
+};
+
+static irqreturn_t sx8654_irq(int irq, void *handle)
+{
+	struct sx8654 *sx8654 = handle;
+	int irqsrc;
+	u8 data[4];
+	unsigned int x, y;
+	int retval;
+
+	irqsrc = i2c_smbus_read_byte_data(sx8654->client,
+					  CMD_READ_REGISTER | I2C_REG_IRQSRC);
+	dev_dbg(&sx8654->client->dev, "irqsrc = 0x%x", irqsrc);
+
+	if (irqsrc < 0)
+		goto out;
+
+	if (irqsrc & IRQ_PENRELEASE) {
+		dev_dbg(&sx8654->client->dev, "pen release interrupt");
+
+		input_report_key(sx8654->input, BTN_TOUCH, 0);
+		input_sync(sx8654->input);
+	}
+
+	if (irqsrc & IRQ_PENTOUCH_TOUCHCONVDONE) {
+		dev_dbg(&sx8654->client->dev, "pen touch interrupt");
+
+		retval = i2c_master_recv(sx8654->client, data, sizeof(data));
+		if (retval != sizeof(data))
+			goto out;
+
+		/* invalid data */
+		if (unlikely(data[0] & 0x80 || data[2] & 0x80))
+			goto out;
+
+		x = ((data[0] & 0xf) << 8) | (data[1]);
+		y = ((data[2] & 0xf) << 8) | (data[3]);
+
+		input_report_abs(sx8654->input, ABS_X, x);
+		input_report_abs(sx8654->input, ABS_Y, y);
+		input_report_key(sx8654->input, BTN_TOUCH, 1);
+		input_sync(sx8654->input);
+
+		dev_dbg(&sx8654->client->dev, "point(%4d,%4d)\n", x, y);
+	}
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int sx8654_open(struct input_dev *dev)
+{
+	struct sx8654 *sx8654 = input_get_drvdata(dev);
+	struct i2c_client *client = sx8654->client;
+	int error;
+
+	/* enable pen trigger mode */
+	error = i2c_smbus_write_byte_data(client, I2C_REG_TOUCH0,
+					  RATE_5000CPS | POWDLY_1_1MS);
+	if (error) {
+		dev_err(&client->dev, "writing to I2C_REG_TOUCH0 failed");
+		return error;
+	}
+
+	error = i2c_smbus_write_byte(client, CMD_PENTRG);
+	if (error) {
+		dev_err(&client->dev, "writing command CMD_PENTRG failed");
+		return error;
+	}
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static void sx8654_close(struct input_dev *dev)
+{
+	struct sx8654 *sx8654 = input_get_drvdata(dev);
+	struct i2c_client *client = sx8654->client;
+	int error;
+
+	disable_irq(client->irq);
+
+	/* enable manual mode mode */
+	error = i2c_smbus_write_byte(client, CMD_MANUAL);
+	if (error) {
+		dev_err(&client->dev, "writing command CMD_MANUAL failed");
+		return;
+	}
+
+	error = i2c_smbus_write_byte_data(client, I2C_REG_TOUCH0, 0);
+	if (error) {
+		dev_err(&client->dev, "writing to I2C_REG_TOUCH0 failed");
+		return;
+	}
+}
+
+static int sx8654_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct sx8654 *sx8654;
+	struct input_dev *input;
+	int error;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_READ_WORD_DATA))
+		return -ENXIO;
+
+	sx8654 = devm_kzalloc(&client->dev, sizeof(*sx8654), GFP_KERNEL);
+	if (!sx8654)
+		return -ENOMEM;
+
+	input = devm_input_allocate_device(&client->dev);
+	if (!sx8654)
+		return -ENOMEM;
+
+	input->name = "SX8654 I2C Touchscreen";
+	input->id.bustype = BUS_I2C;
+	input->dev.parent = &client->dev;
+	input->open = sx8654_open;
+	input->close = sx8654_close;
+
+	__set_bit(INPUT_PROP_DIRECT, input->propbit);
+	input_set_capability(input, EV_KEY, BTN_TOUCH);
+	input_set_abs_params(input, ABS_X, 0, MAX_12BIT, 0, 0);
+	input_set_abs_params(input, ABS_Y, 0, MAX_12BIT, 0, 0);
+
+	sx8654->client = client;
+	sx8654->input = input;
+
+	input_set_drvdata(sx8654->input, sx8654);
+
+	error = i2c_smbus_write_byte_data(client, I2C_REG_SOFTRESET,
+					  SOFTRESET_VALUE);
+	if (error) {
+		dev_err(&client->dev, "writing softreset value failed");
+		return error;
+	}
+
+	error = i2c_smbus_write_byte_data(client, I2C_REG_CHANMASK,
+					  CONV_X | CONV_Y);
+	if (error) {
+		dev_err(&client->dev, "writing to I2C_REG_CHANMASK failed");
+		return error;
+	}
+
+	error = i2c_smbus_write_byte_data(client, I2C_REG_IRQMASK,
+					  IRQ_PENTOUCH_TOUCHCONVDONE |
+						IRQ_PENRELEASE);
+	if (error) {
+		dev_err(&client->dev, "writing to I2C_REG_IRQMASK failed");
+		return error;
+	}
+
+	error = i2c_smbus_write_byte_data(client, I2C_REG_TOUCH1,
+					  CONDIRQ | FILT_7SA);
+	if (error) {
+		dev_err(&client->dev, "writing to I2C_REG_TOUCH1 failed");
+		return error;
+	}
+
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, sx8654_irq,
+					  IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					  client->name, sx8654);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to enable IRQ %d, error: %d\n",
+			client->irq, error);
+		return error;
+	}
+
+	/* Disable the IRQ, we'll enable it in sx8654_open() */
+	disable_irq(client->irq);
+
+	error = input_register_device(sx8654->input);
+	if (error)
+		return error;
+
+	i2c_set_clientdata(client, sx8654);
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id sx8654_of_match[] = {
+	{ .compatible = "semtech,sx8654", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, sx8654_of_match);
+#endif
+
+static const struct i2c_device_id sx8654_id_table[] = {
+	{ "semtech_sx8654", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, sx8654_id_table);
+
+static struct i2c_driver sx8654_driver = {
+	.driver = {
+		.name = "sx8654",
+		.of_match_table = of_match_ptr(sx8654_of_match),
+	},
+	.id_table = sx8654_id_table,
+	.probe = sx8654_probe,
+};
+module_i2c_driver(sx8654_driver);
+
+MODULE_AUTHOR("Sébastien Szymanski <sebastien.szymanski@armadeus.com>");
+MODULE_DESCRIPTION("Semtech SX8654 I2C Touchscreen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/tsc2007.c b/drivers/input/touchscreen/tsc2007.c
index 1bf9906b5a3f..ccc8aa615709 100644
--- a/drivers/input/touchscreen/tsc2007.c
+++ b/drivers/input/touchscreen/tsc2007.c
@@ -75,7 +75,7 @@ struct tsc2007 {
 	u16			model;
 	u16			x_plate_ohms;
 	u16			max_rt;
-	unsigned long		poll_period;
+	unsigned long		poll_period; /* in jiffies */
 	int			fuzzx;
 	int			fuzzy;
 	int			fuzzz;
@@ -214,8 +214,7 @@ static irqreturn_t tsc2007_soft_irq(int irq, void *handle)
 			dev_dbg(&ts->client->dev, "ignored pressure %d\n", rt);
 		}
 
-		wait_event_timeout(ts->wait, ts->stopped,
-				   msecs_to_jiffies(ts->poll_period));
+		wait_event_timeout(ts->wait, ts->stopped, ts->poll_period);
 	}
 
 	dev_dbg(&ts->client->dev, "UP\n");
@@ -314,9 +313,9 @@ static int tsc2007_probe_dt(struct i2c_client *client, struct tsc2007 *ts)
 		ts->fuzzz = val32;
 
 	if (!of_property_read_u64(np, "ti,poll-period", &val64))
-		ts->poll_period = val64;
+		ts->poll_period = msecs_to_jiffies(val64);
 	else
-		ts->poll_period = 1;
+		ts->poll_period = msecs_to_jiffies(1);
 
 	if (!of_property_read_u32(np, "ti,x-plate-ohms", &val32)) {
 		ts->x_plate_ohms = val32;
@@ -350,7 +349,7 @@ static int tsc2007_probe_pdev(struct i2c_client *client, struct tsc2007 *ts,
 	ts->model             = pdata->model;
 	ts->x_plate_ohms      = pdata->x_plate_ohms;
 	ts->max_rt            = pdata->max_rt ? : MAX_12BIT;
-	ts->poll_period       = pdata->poll_period ? : 1;
+	ts->poll_period       = msecs_to_jiffies(pdata->poll_period ? : 1);
 	ts->get_pendown_state = pdata->get_pendown_state;
 	ts->clear_penirq      = pdata->clear_penirq;
 	ts->fuzzx             = pdata->fuzzx;
diff --git a/drivers/input/touchscreen/usbtouchscreen.c b/drivers/input/touchscreen/usbtouchscreen.c
index a0966331a89b..f2c6c352c55a 100644
--- a/drivers/input/touchscreen/usbtouchscreen.c
+++ b/drivers/input/touchscreen/usbtouchscreen.c
@@ -132,6 +132,7 @@ enum {
 	DEVTYPE_GUNZE,
 	DEVTYPE_DMC_TSC10,
 	DEVTYPE_IRTOUCH,
+	DEVTYPE_IRTOUCH_HIRES,
 	DEVTYPE_IDEALTEK,
 	DEVTYPE_GENERAL_TOUCH,
 	DEVTYPE_GOTOP,
@@ -198,6 +199,7 @@ static const struct usb_device_id usbtouch_devices[] = {
 #ifdef CONFIG_TOUCHSCREEN_USB_IRTOUCH
 	{USB_DEVICE(0x595a, 0x0001), .driver_info = DEVTYPE_IRTOUCH},
 	{USB_DEVICE(0x6615, 0x0001), .driver_info = DEVTYPE_IRTOUCH},
+	{USB_DEVICE(0x6615, 0x0012), .driver_info = DEVTYPE_IRTOUCH_HIRES},
 #endif
 
 #ifdef CONFIG_TOUCHSCREEN_USB_IDEALTEK
@@ -1177,6 +1179,15 @@ static struct usbtouch_device_info usbtouch_dev_info[] = {
 		.rept_size	= 8,
 		.read_data	= irtouch_read_data,
 	},
+
+	[DEVTYPE_IRTOUCH_HIRES] = {
+		.min_xc		= 0x0,
+		.max_xc		= 0x7fff,
+		.min_yc		= 0x0,
+		.max_yc		= 0x7fff,
+		.rept_size	= 8,
+		.read_data	= irtouch_read_data,
+	},
 #endif
 
 #ifdef CONFIG_TOUCHSCREEN_USB_IDEALTEK
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/intel-iommu.c b/drivers/iommu/intel-iommu.c
index ae4c1a854e57..4fc1f8a7f98e 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -684,7 +684,7 @@ static struct intel_iommu *device_to_iommu(struct device *dev, u8 *bus, u8 *devf
 	if (dev_is_pci(dev)) {
 		pdev = to_pci_dev(dev);
 		segment = pci_domain_nr(pdev->bus);
-	} else if (ACPI_COMPANION(dev))
+	} else if (has_acpi_companion(dev))
 		dev = &ACPI_COMPANION(dev)->dev;
 
 	rcu_read_lock();
@@ -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/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/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 4387dae14e45..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)
@@ -77,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
@@ -86,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
@@ -98,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
@@ -281,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);
@@ -308,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;
@@ -352,11 +345,44 @@ 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;
 }
@@ -369,8 +395,10 @@ static struct notifier_block armada_370_xp_mpic_cpu_notifier = {
 static int mpic_cascaded_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();
 		enable_percpu_irq(parent_irq, IRQ_TYPE_NONE);
+	}
 
 	return NOTIFY_OK;
 }
@@ -379,7 +407,6 @@ 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 = {
@@ -588,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);
 
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 596b0a9eee99..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;
 }
 
@@ -802,6 +802,7 @@ static int its_alloc_tables(struct its_node *its)
 	int i;
 	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);
@@ -848,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);
 
@@ -874,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;
 		}
 
@@ -980,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);
@@ -1026,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 */
@@ -1422,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(GITS_CTLR_ENABLE, 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) {
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index fd8850def1b8..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)
 {
@@ -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 471e1cdc1933..a6ce3476834e 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -151,13 +151,24 @@ 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);
+}
+
+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);
-	writel_relaxed(mask, gic_dist_base(d) + GIC_DIST_ENABLE_CLEAR + (gic_irq(d) / 32) * 4);
+	gic_poke_irq(d, GIC_DIST_ENABLE_CLEAR);
 	if (gic_arch_extn.irq_mask)
 		gic_arch_extn.irq_mask(d);
 	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
@@ -165,13 +176,12 @@ static void gic_mask_irq(struct irq_data *d)
 
 static void gic_unmask_irq(struct irq_data *d)
 {
-	u32 mask = 1 << (gic_irq(d) % 32);
 	unsigned long flags;
 
 	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);
+	gic_poke_irq(d, GIC_DIST_ENABLE_SET);
 	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
 }
 
@@ -186,6 +196,55 @@ 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);
@@ -329,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)
@@ -353,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;
@@ -802,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,
@@ -829,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;
 
@@ -895,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)
+void gic_set_irqchip_flags(unsigned long flags)
 {
-	return 0;
+	gic_chip.flags |= flags;
 }
 
-static void gic_routable_irq_domain_unmap(struct irq_domain *d,
-					  unsigned int irq)
-{
-}
-
-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)
@@ -933,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);
 
@@ -989,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/gigaset/ev-layer.c b/drivers/isdn/gigaset/ev-layer.c
index c8ced12fa452..1cfcea62aed9 100644
--- a/drivers/isdn/gigaset/ev-layer.c
+++ b/drivers/isdn/gigaset/ev-layer.c
@@ -389,22 +389,49 @@ zsau_resp[] =
 	{NULL,				ZSAU_UNKNOWN}
 };
 
-/* retrieve CID from parsed response
- * returns 0 if no CID, -1 if invalid CID, or CID value 1..65535
+/* check for and remove fixed string prefix
+ * If s starts with prefix terminated by a non-alphanumeric character,
+ * return pointer to the first character after that, otherwise return NULL.
  */
-static int cid_of_response(char *s)
+static char *skip_prefix(char *s, const char *prefix)
 {
-	int cid;
-	int rc;
-
-	if (s[-1] != ';')
-		return 0;	/* no CID separator */
-	rc = kstrtoint(s, 10, &cid);
-	if (rc)
-		return 0;	/* CID not numeric */
-	if (cid < 1 || cid > 65535)
-		return -1;	/* CID out of range */
-	return cid;
+	while (*prefix)
+		if (*s++ != *prefix++)
+			return NULL;
+	if (isalnum(*s))
+		return NULL;
+	return s;
+}
+
+/* queue event with CID */
+static void add_cid_event(struct cardstate *cs, int cid, int type,
+			  void *ptr, int parameter)
+{
+	unsigned long flags;
+	unsigned next, tail;
+	struct event_t *event;
+
+	gig_dbg(DEBUG_EVENT, "queueing event %d for cid %d", type, cid);
+
+	spin_lock_irqsave(&cs->ev_lock, flags);
+
+	tail = cs->ev_tail;
+	next = (tail + 1) % MAX_EVENTS;
+	if (unlikely(next == cs->ev_head)) {
+		dev_err(cs->dev, "event queue full\n");
+		kfree(ptr);
+	} else {
+		event = cs->events + tail;
+		event->type = type;
+		event->cid = cid;
+		event->ptr = ptr;
+		event->arg = NULL;
+		event->parameter = parameter;
+		event->at_state = NULL;
+		cs->ev_tail = next;
+	}
+
+	spin_unlock_irqrestore(&cs->ev_lock, flags);
 }
 
 /**
@@ -417,190 +444,188 @@ static int cid_of_response(char *s)
  */
 void gigaset_handle_modem_response(struct cardstate *cs)
 {
-	unsigned char *argv[MAX_REC_PARAMS + 1];
-	int params;
-	int i, j;
+	char *eoc, *psep, *ptr;
 	const struct resp_type_t *rt;
 	const struct zsau_resp_t *zr;
-	int curarg;
-	unsigned long flags;
-	unsigned next, tail, head;
-	struct event_t *event;
-	int resp_code;
-	int param_type;
-	int abort;
-	size_t len;
-	int cid;
-	int rawstring;
-
-	len = cs->cbytes;
-	if (!len) {
+	int cid, parameter;
+	u8 type, value;
+
+	if (!cs->cbytes) {
 		/* ignore additional LFs/CRs (M10x config mode or cx100) */
 		gig_dbg(DEBUG_MCMD, "skipped EOL [%02X]", cs->respdata[0]);
 		return;
 	}
-	cs->respdata[len] = 0;
-	argv[0] = cs->respdata;
-	params = 1;
+	cs->respdata[cs->cbytes] = 0;
+
 	if (cs->at_state.getstring) {
-		/* getstring only allowed without cid at the moment */
+		/* state machine wants next line verbatim */
 		cs->at_state.getstring = 0;
-		rawstring = 1;
-		cid = 0;
-	} else {
-		/* parse line */
-		for (i = 0; i < len; i++)
-			switch (cs->respdata[i]) {
-			case ';':
-			case ',':
-			case '=':
-				if (params > MAX_REC_PARAMS) {
-					dev_warn(cs->dev,
-						 "too many parameters in response\n");
-					/* need last parameter (might be CID) */
-					params--;
-				}
-				argv[params++] = cs->respdata + i + 1;
-			}
-
-		rawstring = 0;
-		cid = params > 1 ? cid_of_response(argv[params - 1]) : 0;
-		if (cid < 0) {
-			gigaset_add_event(cs, &cs->at_state, RSP_INVAL,
-					  NULL, 0, NULL);
-			return;
-		}
+		ptr = kstrdup(cs->respdata, GFP_ATOMIC);
+		gig_dbg(DEBUG_EVENT, "string==%s", ptr ? ptr : "NULL");
+		add_cid_event(cs, 0, RSP_STRING, ptr, 0);
+		return;
+	}
 
-		for (j = 1; j < params; ++j)
-			argv[j][-1] = 0;
+	/* look up response type */
+	for (rt = resp_type; rt->response; ++rt) {
+		eoc = skip_prefix(cs->respdata, rt->response);
+		if (eoc)
+			break;
+	}
+	if (!rt->response) {
+		add_cid_event(cs, 0, RSP_NONE, NULL, 0);
+		gig_dbg(DEBUG_EVENT, "unknown modem response: '%s'\n",
+			cs->respdata);
+		return;
+	}
 
-		gig_dbg(DEBUG_EVENT, "CMD received: %s", argv[0]);
-		if (cid) {
-			--params;
-			gig_dbg(DEBUG_EVENT, "CID: %s", argv[params]);
-		}
-		gig_dbg(DEBUG_EVENT, "available params: %d", params - 1);
-		for (j = 1; j < params; j++)
-			gig_dbg(DEBUG_EVENT, "param %d: %s", j, argv[j]);
+	/* check for CID */
+	psep = strrchr(cs->respdata, ';');
+	if (psep &&
+	    !kstrtoint(psep + 1, 10, &cid) &&
+	    cid >= 1 && cid <= 65535) {
+		/* valid CID: chop it off */
+		*psep = 0;
+	} else {
+		/* no valid CID: leave unchanged */
+		cid = 0;
 	}
 
-	spin_lock_irqsave(&cs->ev_lock, flags);
-	head = cs->ev_head;
-	tail = cs->ev_tail;
+	gig_dbg(DEBUG_EVENT, "CMD received: %s", cs->respdata);
+	if (cid)
+		gig_dbg(DEBUG_EVENT, "CID: %d", cid);
 
-	abort = 1;
-	curarg = 0;
-	while (curarg < params) {
-		next = (tail + 1) % MAX_EVENTS;
-		if (unlikely(next == head)) {
-			dev_err(cs->dev, "event queue full\n");
-			break;
-		}
+	switch (rt->type) {
+	case RT_NOTHING:
+		/* check parameter separator */
+		if (*eoc)
+			goto bad_param;	/* extra parameter */
 
-		event = cs->events + tail;
-		event->at_state = NULL;
-		event->cid = cid;
-		event->ptr = NULL;
-		event->arg = NULL;
-		tail = next;
+		add_cid_event(cs, cid, rt->resp_code, NULL, 0);
+		break;
 
-		if (rawstring) {
-			resp_code = RSP_STRING;
-			param_type = RT_STRING;
-		} else {
-			for (rt = resp_type; rt->response; ++rt)
-				if (!strcmp(argv[curarg], rt->response))
+	case RT_RING:
+		/* check parameter separator */
+		if (!*eoc)
+			eoc = NULL;	/* no parameter */
+		else if (*eoc++ != ',')
+			goto bad_param;
+
+		add_cid_event(cs, 0, rt->resp_code, NULL, cid);
+
+		/* process parameters as individual responses */
+		while (eoc) {
+			/* look up parameter type */
+			psep = NULL;
+			for (rt = resp_type; rt->response; ++rt) {
+				psep = skip_prefix(eoc, rt->response);
+				if (psep)
 					break;
+			}
 
-			if (!rt->response) {
-				event->type = RSP_NONE;
-				gig_dbg(DEBUG_EVENT,
-					"unknown modem response: '%s'\n",
-					argv[curarg]);
-				break;
+			/* all legal parameters are of type RT_STRING */
+			if (!psep || rt->type != RT_STRING) {
+				dev_warn(cs->dev,
+					 "illegal RING parameter: '%s'\n",
+					 eoc);
+				return;
 			}
 
-			resp_code = rt->resp_code;
-			param_type = rt->type;
-			++curarg;
-		}
+			/* skip parameter value separator */
+			if (*psep++ != '=')
+				goto bad_param;
 
-		event->type = resp_code;
+			/* look up end of parameter */
+			eoc = strchr(psep, ',');
+			if (eoc)
+				*eoc++ = 0;
 
-		switch (param_type) {
-		case RT_NOTHING:
-			break;
-		case RT_RING:
-			if (!cid) {
-				dev_err(cs->dev,
-					"received RING without CID!\n");
-				event->type = RSP_INVAL;
-				abort = 1;
-			} else {
-				event->cid = 0;
-				event->parameter = cid;
-				abort = 0;
-			}
+			/* retrieve parameter value */
+			ptr = kstrdup(psep, GFP_ATOMIC);
+
+			/* queue event */
+			add_cid_event(cs, cid, rt->resp_code, ptr, 0);
+		}
+		break;
+
+	case RT_ZSAU:
+		/* check parameter separator */
+		if (!*eoc) {
+			/* no parameter */
+			add_cid_event(cs, cid, rt->resp_code, NULL, ZSAU_NONE);
 			break;
-		case RT_ZSAU:
-			if (curarg >= params) {
-				event->parameter = ZSAU_NONE;
+		}
+		if (*eoc++ != '=')
+			goto bad_param;
+
+		/* look up parameter value */
+		for (zr = zsau_resp; zr->str; ++zr)
+			if (!strcmp(eoc, zr->str))
 				break;
-			}
-			for (zr = zsau_resp; zr->str; ++zr)
-				if (!strcmp(argv[curarg], zr->str))
-					break;
-			event->parameter = zr->code;
-			if (!zr->str)
-				dev_warn(cs->dev,
-					 "%s: unknown parameter %s after ZSAU\n",
-					 __func__, argv[curarg]);
-			++curarg;
-			break;
-		case RT_STRING:
-			if (curarg < params) {
-				event->ptr = kstrdup(argv[curarg], GFP_ATOMIC);
-				if (!event->ptr)
-					dev_err(cs->dev, "out of memory\n");
-				++curarg;
-			}
-			gig_dbg(DEBUG_EVENT, "string==%s",
-				event->ptr ? (char *) event->ptr : "NULL");
-			break;
-		case RT_ZCAU:
-			event->parameter = -1;
-			if (curarg + 1 < params) {
-				u8 type, value;
-
-				i = kstrtou8(argv[curarg++], 16, &type);
-				j = kstrtou8(argv[curarg++], 16, &value);
-				if (i == 0 && j == 0)
-					event->parameter = (type << 8) | value;
-			} else
-				curarg = params - 1;
-			break;
-		case RT_NUMBER:
-			if (curarg >= params ||
-			    kstrtoint(argv[curarg++], 10, &event->parameter))
-				event->parameter = -1;
-			gig_dbg(DEBUG_EVENT, "parameter==%d", event->parameter);
-			break;
+		if (!zr->str)
+			goto bad_param;
+
+		add_cid_event(cs, cid, rt->resp_code, NULL, zr->code);
+		break;
+
+	case RT_STRING:
+		/* check parameter separator */
+		if (*eoc++ != '=')
+			goto bad_param;
+
+		/* retrieve parameter value */
+		ptr = kstrdup(eoc, GFP_ATOMIC);
+
+		/* queue event */
+		add_cid_event(cs, cid, rt->resp_code, ptr, 0);
+		break;
+
+	case RT_ZCAU:
+		/* check parameter separators */
+		if (*eoc++ != '=')
+			goto bad_param;
+		psep = strchr(eoc, ',');
+		if (!psep)
+			goto bad_param;
+		*psep++ = 0;
+
+		/* decode parameter values */
+		if (kstrtou8(eoc, 16, &type) || kstrtou8(psep, 16, &value)) {
+			*--psep = ',';
+			goto bad_param;
 		}
+		parameter = (type << 8) | value;
 
-		if (resp_code == RSP_ZDLE)
-			cs->dle = event->parameter;
+		add_cid_event(cs, cid, rt->resp_code, NULL, parameter);
+		break;
 
-		if (abort)
-			break;
-	}
+	case RT_NUMBER:
+		/* check parameter separator */
+		if (*eoc++ != '=')
+			goto bad_param;
 
-	cs->ev_tail = tail;
-	spin_unlock_irqrestore(&cs->ev_lock, flags);
+		/* decode parameter value */
+		if (kstrtoint(eoc, 10, &parameter))
+			goto bad_param;
+
+		/* special case ZDLE: set flag before queueing event */
+		if (rt->resp_code == RSP_ZDLE)
+			cs->dle = parameter;
 
-	if (curarg != params)
-		gig_dbg(DEBUG_EVENT,
-			"invalid number of processed parameters: %d/%d",
-			curarg, params);
+		add_cid_event(cs, cid, rt->resp_code, NULL, parameter);
+		break;
+
+bad_param:
+		/* parameter unexpected, incomplete or malformed */
+		dev_warn(cs->dev, "bad parameter in response '%s'\n",
+			 cs->respdata);
+		add_cid_event(cs, cid, rt->resp_code, NULL, -1);
+		break;
+
+	default:
+		dev_err(cs->dev, "%s: internal error on '%s'\n",
+			__func__, cs->respdata);
+	}
 }
 EXPORT_SYMBOL_GPL(gigaset_handle_modem_response);
 
diff --git a/drivers/isdn/hardware/mISDN/mISDNinfineon.c b/drivers/isdn/hardware/mISDN/mISDNinfineon.c
index c1493f4162fb..d5bdbaf93a1a 100644
--- a/drivers/isdn/hardware/mISDN/mISDNinfineon.c
+++ b/drivers/isdn/hardware/mISDN/mISDNinfineon.c
@@ -1092,7 +1092,7 @@ inf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 	card->ci = get_card_info(ent->driver_data);
 	if (!card->ci) {
-		pr_info("mISDN: do not have informations about adapter at %s\n",
+		pr_info("mISDN: do not have information about adapter at %s\n",
 			pci_name(pdev));
 		kfree(card);
 		pci_disable_device(pdev);
diff --git a/drivers/isdn/i4l/isdn_net.c b/drivers/isdn/i4l/isdn_net.c
index 94affa5e6f28..546b7e81161d 100644
--- a/drivers/isdn/i4l/isdn_net.c
+++ b/drivers/isdn/i4l/isdn_net.c
@@ -1951,38 +1951,6 @@ static int isdn_net_header(struct sk_buff *skb, struct net_device *dev,
 	return len;
 }
 
-/* We don't need to send arp, because we have point-to-point connections. */
-static int
-isdn_net_rebuild_header(struct sk_buff *skb)
-{
-	struct net_device *dev = skb->dev;
-	isdn_net_local *lp = netdev_priv(dev);
-	int ret = 0;
-
-	if (lp->p_encap == ISDN_NET_ENCAP_ETHER) {
-		struct ethhdr *eth = (struct ethhdr *) skb->data;
-
-		/*
-		 *      Only ARP/IP is currently supported
-		 */
-
-		if (eth->h_proto != htons(ETH_P_IP)) {
-			printk(KERN_WARNING
-			       "isdn_net: %s don't know how to resolve type %d addresses?\n",
-			       dev->name, (int) eth->h_proto);
-			memcpy(eth->h_source, dev->dev_addr, dev->addr_len);
-			return 0;
-		}
-		/*
-		 *      Try to get ARP to resolve the header.
-		 */
-#ifdef CONFIG_INET
-		ret = arp_find(eth->h_dest, skb);
-#endif
-	}
-	return ret;
-}
-
 static int isdn_header_cache(const struct neighbour *neigh, struct hh_cache *hh,
 			     __be16 type)
 {
@@ -2005,7 +1973,6 @@ static void isdn_header_cache_update(struct hh_cache *hh,
 
 static const struct header_ops isdn_header_ops = {
 	.create = isdn_net_header,
-	.rebuild = isdn_net_rebuild_header,
 	.cache = isdn_header_cache,
 	.cache_update = isdn_header_cache_update,
 };
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/isdn/mISDN/dsp_cmx.c b/drivers/isdn/mISDN/dsp_cmx.c
index 87f7dff20ff6..52c43821f746 100644
--- a/drivers/isdn/mISDN/dsp_cmx.c
+++ b/drivers/isdn/mISDN/dsp_cmx.c
@@ -295,7 +295,7 @@ dsp_cmx_del_conf_member(struct dsp *dsp)
 		}
 	}
 	printk(KERN_WARNING
-	       "%s: dsp is not present in its own conf_meber list.\n",
+	       "%s: dsp is not present in its own conf_member list.\n",
 	       __func__);
 
 	return -EINVAL;
diff --git a/drivers/isdn/mISDN/dsp_core.c b/drivers/isdn/mISDN/dsp_core.c
index 77025f5cb57d..0222b1a35a2d 100644
--- a/drivers/isdn/mISDN/dsp_core.c
+++ b/drivers/isdn/mISDN/dsp_core.c
@@ -460,7 +460,7 @@ dsp_control_req(struct dsp *dsp, struct mISDNhead *hh, struct sk_buff *skb)
 		}
 		if (dsp_debug & DEBUG_DSP_CORE)
 			printk(KERN_DEBUG "%s: enable mixing of "
-			       "tx-data with conf mebers\n", __func__);
+			       "tx-data with conf members\n", __func__);
 		dsp->tx_mix = 1;
 		dsp_cmx_hardware(dsp->conf, dsp);
 		dsp_rx_off(dsp);
@@ -474,7 +474,7 @@ dsp_control_req(struct dsp *dsp, struct mISDNhead *hh, struct sk_buff *skb)
 		}
 		if (dsp_debug & DEBUG_DSP_CORE)
 			printk(KERN_DEBUG "%s: disable mixing of "
-			       "tx-data with conf mebers\n", __func__);
+			       "tx-data with conf members\n", __func__);
 		dsp->tx_mix = 0;
 		dsp_cmx_hardware(dsp->conf, dsp);
 		dsp_rx_off(dsp);
diff --git a/drivers/isdn/mISDN/socket.c b/drivers/isdn/mISDN/socket.c
index 84b35925ee4d..8dc7290089bb 100644
--- a/drivers/isdn/mISDN/socket.c
+++ b/drivers/isdn/mISDN/socket.c
@@ -112,8 +112,8 @@ mISDN_sock_cmsg(struct sock *sk, struct msghdr *msg, struct sk_buff *skb)
 }
 
 static int
-mISDN_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
-		   struct msghdr *msg, size_t len, int flags)
+mISDN_sock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
+		   int flags)
 {
 	struct sk_buff		*skb;
 	struct sock		*sk = sock->sk;
@@ -173,8 +173,7 @@ mISDN_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 }
 
 static int
-mISDN_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
-		   struct msghdr *msg, size_t len)
+mISDN_sock_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
 {
 	struct sock		*sk = sock->sk;
 	struct sk_buff		*skb;
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 25b320d64e26..966b9605f5f0 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -526,6 +526,14 @@ config LEDS_VERSATILE
 	  This option enabled support for the LEDs on the ARM Versatile
 	  and RealView boards. Say Y to enabled these.
 
+config LEDS_PM8941_WLED
+	tristate "LED support for the Qualcomm PM8941 WLED block"
+	depends on LEDS_CLASS
+	select REGMAP
+	help
+	  This option enables support for the 'White' LED block
+	  on Qualcomm PM8941 PMICs.
+
 comment "LED Triggers"
 source "drivers/leds/trigger/Kconfig"
 
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index cbba921b6f1c..bf4609338e10 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -58,6 +58,7 @@ obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
 obj-$(CONFIG_LEDS_SYSCON)		+= leds-syscon.o
 obj-$(CONFIG_LEDS_VERSATILE)		+= leds-versatile.o
 obj-$(CONFIG_LEDS_MENF21BMC)		+= leds-menf21bmc.o
+obj-$(CONFIG_LEDS_PM8941_WLED)		+= leds-pm8941-wled.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
diff --git a/drivers/leds/led-class-flash.c b/drivers/leds/led-class-flash.c
index 4a19fd44f93f..3b2573411a37 100644
--- a/drivers/leds/led-class-flash.c
+++ b/drivers/leds/led-class-flash.c
@@ -216,75 +216,6 @@ static ssize_t flash_fault_show(struct device *dev,
 }
 static DEVICE_ATTR_RO(flash_fault);
 
-static ssize_t available_sync_leds_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct led_classdev *led_cdev = dev_get_drvdata(dev);
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	char *pbuf = buf;
-	int i, buf_len;
-
-	buf_len = sprintf(pbuf, "[0: none] ");
-	pbuf += buf_len;
-
-	for (i = 0; i < fled_cdev->num_sync_leds; ++i) {
-		buf_len = sprintf(pbuf, "[%d: %s] ", i + 1,
-				  fled_cdev->sync_leds[i]->led_cdev.name);
-		pbuf += buf_len;
-	}
-
-	return sprintf(buf, "%s\n", buf);
-}
-static DEVICE_ATTR_RO(available_sync_leds);
-
-static ssize_t flash_sync_strobe_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct led_classdev *led_cdev = dev_get_drvdata(dev);
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	unsigned long led_id;
-	ssize_t ret;
-
-	mutex_lock(&led_cdev->led_access);
-
-	if (led_sysfs_is_disabled(led_cdev)) {
-		ret = -EBUSY;
-		goto unlock;
-	}
-
-	ret = kstrtoul(buf, 10, &led_id);
-	if (ret)
-		goto unlock;
-
-	if (led_id > fled_cdev->num_sync_leds) {
-		ret = -ERANGE;
-		goto unlock;
-	}
-
-	fled_cdev->sync_led_id = led_id;
-
-	ret = size;
-unlock:
-	mutex_unlock(&led_cdev->led_access);
-	return ret;
-}
-
-static ssize_t flash_sync_strobe_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct led_classdev *led_cdev = dev_get_drvdata(dev);
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	int sled_id = fled_cdev->sync_led_id;
-	char *sync_led_name = "none";
-
-	if (fled_cdev->sync_led_id > 0)
-		sync_led_name = (char *)
-			fled_cdev->sync_leds[sled_id - 1]->led_cdev.name;
-
-	return sprintf(buf, "[%d: %s]\n", sled_id, sync_led_name);
-}
-static DEVICE_ATTR_RW(flash_sync_strobe);
-
 static struct attribute *led_flash_strobe_attrs[] = {
 	&dev_attr_flash_strobe.attr,
 	NULL,
@@ -307,12 +238,6 @@ static struct attribute *led_flash_fault_attrs[] = {
 	NULL,
 };
 
-static struct attribute *led_flash_sync_strobe_attrs[] = {
-	&dev_attr_available_sync_leds.attr,
-	&dev_attr_flash_sync_strobe.attr,
-	NULL,
-};
-
 static const struct attribute_group led_flash_strobe_group = {
 	.attrs = led_flash_strobe_attrs,
 };
@@ -329,10 +254,6 @@ static const struct attribute_group led_flash_fault_group = {
 	.attrs = led_flash_fault_attrs,
 };
 
-static const struct attribute_group led_flash_sync_strobe_group = {
-	.attrs = led_flash_sync_strobe_attrs,
-};
-
 static void led_flash_resume(struct led_classdev *led_cdev)
 {
 	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
@@ -361,9 +282,6 @@ static void led_flash_init_sysfs_groups(struct led_classdev_flash *fled_cdev)
 	if (ops->fault_get)
 		flash_groups[num_sysfs_groups++] = &led_flash_fault_group;
 
-	if (led_cdev->flags & LED_DEV_CAP_SYNC_STROBE)
-		flash_groups[num_sysfs_groups++] = &led_flash_sync_strobe_group;
-
 	led_cdev->groups = flash_groups;
 }
 
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index 795ec994c663..728681debdbe 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -212,6 +212,31 @@ static const struct dev_pm_ops leds_class_dev_pm_ops = {
 	.resume         = led_resume,
 };
 
+static int match_name(struct device *dev, const void *data)
+{
+	if (!dev_name(dev))
+		return 0;
+	return !strcmp(dev_name(dev), (char *)data);
+}
+
+static int led_classdev_next_name(const char *init_name, char *name,
+				  size_t len)
+{
+	unsigned int i = 0;
+	int ret = 0;
+
+	strlcpy(name, init_name, len);
+
+	while (class_find_device(leds_class, NULL, name, match_name) &&
+	       (ret < len))
+		ret = snprintf(name, len, "%s_%u", init_name, ++i);
+
+	if (ret >= len)
+		return -ENOMEM;
+
+	return i;
+}
+
 /**
  * led_classdev_register - register a new object of led_classdev class.
  * @parent: The device to register.
@@ -219,12 +244,22 @@ static const struct dev_pm_ops leds_class_dev_pm_ops = {
  */
 int led_classdev_register(struct device *parent, struct led_classdev *led_cdev)
 {
+	char name[64];
+	int ret;
+
+	ret = led_classdev_next_name(led_cdev->name, name, sizeof(name));
+	if (ret < 0)
+		return ret;
+
 	led_cdev->dev = device_create_with_groups(leds_class, parent, 0,
-					led_cdev, led_cdev->groups,
-					"%s", led_cdev->name);
+				led_cdev, led_cdev->groups, "%s", name);
 	if (IS_ERR(led_cdev->dev))
 		return PTR_ERR(led_cdev->dev);
 
+	if (ret)
+		dev_warn(parent, "Led %s renamed to %s due to name collision",
+				led_cdev->name, dev_name(led_cdev->dev));
+
 #ifdef CONFIG_LEDS_TRIGGERS
 	init_rwsem(&led_cdev->trigger_lock);
 #endif
@@ -288,6 +323,63 @@ void led_classdev_unregister(struct led_classdev *led_cdev)
 }
 EXPORT_SYMBOL_GPL(led_classdev_unregister);
 
+static void devm_led_classdev_release(struct device *dev, void *res)
+{
+	led_classdev_unregister(*(struct led_classdev **)res);
+}
+
+/**
+ * devm_led_classdev_register - resource managed led_classdev_register()
+ * @parent: The device to register.
+ * @led_cdev: the led_classdev structure for this device.
+ */
+int devm_led_classdev_register(struct device *parent,
+			       struct led_classdev *led_cdev)
+{
+	struct led_classdev **dr;
+	int rc;
+
+	dr = devres_alloc(devm_led_classdev_release, sizeof(*dr), GFP_KERNEL);
+	if (!dr)
+		return -ENOMEM;
+
+	rc = led_classdev_register(parent, led_cdev);
+	if (rc) {
+		devres_free(dr);
+		return rc;
+	}
+
+	*dr = led_cdev;
+	devres_add(parent, dr);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devm_led_classdev_register);
+
+static int devm_led_classdev_match(struct device *dev, void *res, void *data)
+{
+	struct led_cdev **p = res;
+
+	if (WARN_ON(!p || !*p))
+		return 0;
+
+	return *p == data;
+}
+
+/**
+ * devm_led_classdev_unregister() - resource managed led_classdev_unregister()
+ * @parent: The device to unregister.
+ * @led_cdev: the led_classdev structure for this device.
+ */
+void devm_led_classdev_unregister(struct device *dev,
+				  struct led_classdev *led_cdev)
+{
+	WARN_ON(devres_release(dev,
+			       devm_led_classdev_release,
+			       devm_led_classdev_match, led_cdev));
+}
+EXPORT_SYMBOL_GPL(devm_led_classdev_unregister);
+
 static int __init leds_init(void)
 {
 	leds_class = class_create(THIS_MODULE, "leds");
diff --git a/drivers/leds/leds-lp8501.c b/drivers/leds/leds-lp8501.c
index 00f068b0fa6f..d3098e395fff 100644
--- a/drivers/leds/leds-lp8501.c
+++ b/drivers/leds/leds-lp8501.c
@@ -406,6 +406,6 @@ static struct i2c_driver lp8501_driver = {
 
 module_i2c_driver(lp8501_driver);
 
-MODULE_DESCRIPTION("Texas Instruments LP8501 LED drvier");
+MODULE_DESCRIPTION("Texas Instruments LP8501 LED driver");
 MODULE_AUTHOR("Milo Kim");
 MODULE_LICENSE("GPL");
diff --git a/drivers/leds/leds-lp8860.c b/drivers/leds/leds-lp8860.c
index 840e93f3ab3e..8c2b7fbe2392 100644
--- a/drivers/leds/leds-lp8860.c
+++ b/drivers/leds/leds-lp8860.c
@@ -391,11 +391,13 @@ static int lp8860_probe(struct i2c_client *client,
 		}
 	}
 
-	led->enable_gpio = devm_gpiod_get(&client->dev, "enable");
-	if (IS_ERR(led->enable_gpio))
-		led->enable_gpio = NULL;
-	else
-		gpiod_direction_output(led->enable_gpio, 0);
+	led->enable_gpio = devm_gpiod_get_optional(&client->dev,
+						   "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(led->enable_gpio)) {
+		ret = PTR_ERR(led->enable_gpio);
+		dev_err(&client->dev, "Failed to get enable gpio: %d\n", ret);
+		return ret;
+	}
 
 	led->regulator = devm_regulator_get(&client->dev, "vled");
 	if (IS_ERR(led->regulator))
@@ -486,6 +488,6 @@ static struct i2c_driver lp8860_driver = {
 };
 module_i2c_driver(lp8860_driver);
 
-MODULE_DESCRIPTION("Texas Instruments LP8860 LED drvier");
+MODULE_DESCRIPTION("Texas Instruments LP8860 LED driver");
 MODULE_AUTHOR("Dan Murphy <dmurphy@ti.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/leds/leds-pca963x.c b/drivers/leds/leds-pca963x.c
index f110b4c456ba..bee3e1ab27fd 100644
--- a/drivers/leds/leds-pca963x.c
+++ b/drivers/leds/leds-pca963x.c
@@ -289,7 +289,7 @@ pca963x_dt_init(struct i2c_client *client, struct pca963x_chipdef *chip)
 		return ERR_PTR(-ENOMEM);
 
 	for_each_child_of_node(np, child) {
-		struct led_info led;
+		struct led_info led = {};
 		u32 reg;
 		int res;
 
diff --git a/drivers/leds/leds-pm8941-wled.c b/drivers/leds/leds-pm8941-wled.c
new file mode 100644
index 000000000000..bf64a593fbf1
--- /dev/null
+++ b/drivers/leds/leds-pm8941-wled.c
@@ -0,0 +1,435 @@
+/* Copyright (c) 2015, Sony Mobile Communications, AB.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+
+#define PM8941_WLED_REG_VAL_BASE		0x40
+#define  PM8941_WLED_REG_VAL_MAX		0xFFF
+
+#define PM8941_WLED_REG_MOD_EN			0x46
+#define  PM8941_WLED_REG_MOD_EN_BIT		BIT(7)
+#define  PM8941_WLED_REG_MOD_EN_MASK		BIT(7)
+
+#define PM8941_WLED_REG_SYNC			0x47
+#define  PM8941_WLED_REG_SYNC_MASK		0x07
+#define  PM8941_WLED_REG_SYNC_LED1		BIT(0)
+#define  PM8941_WLED_REG_SYNC_LED2		BIT(1)
+#define  PM8941_WLED_REG_SYNC_LED3		BIT(2)
+#define  PM8941_WLED_REG_SYNC_ALL		0x07
+#define  PM8941_WLED_REG_SYNC_CLEAR		0x00
+
+#define PM8941_WLED_REG_FREQ			0x4c
+#define  PM8941_WLED_REG_FREQ_MASK		0x0f
+
+#define PM8941_WLED_REG_OVP			0x4d
+#define  PM8941_WLED_REG_OVP_MASK		0x03
+
+#define PM8941_WLED_REG_BOOST			0x4e
+#define  PM8941_WLED_REG_BOOST_MASK		0x07
+
+#define PM8941_WLED_REG_SINK			0x4f
+#define  PM8941_WLED_REG_SINK_MASK		0xe0
+#define  PM8941_WLED_REG_SINK_SHFT		0x05
+
+/* Per-'string' registers below */
+#define PM8941_WLED_REG_STR_OFFSET		0x10
+
+#define PM8941_WLED_REG_STR_MOD_EN_BASE		0x60
+#define  PM8941_WLED_REG_STR_MOD_MASK		BIT(7)
+#define  PM8941_WLED_REG_STR_MOD_EN		BIT(7)
+
+#define PM8941_WLED_REG_STR_SCALE_BASE		0x62
+#define  PM8941_WLED_REG_STR_SCALE_MASK		0x1f
+
+#define PM8941_WLED_REG_STR_MOD_SRC_BASE	0x63
+#define  PM8941_WLED_REG_STR_MOD_SRC_MASK	0x01
+#define  PM8941_WLED_REG_STR_MOD_SRC_INT	0x00
+#define  PM8941_WLED_REG_STR_MOD_SRC_EXT	0x01
+
+#define PM8941_WLED_REG_STR_CABC_BASE		0x66
+#define  PM8941_WLED_REG_STR_CABC_MASK		BIT(7)
+#define  PM8941_WLED_REG_STR_CABC_EN		BIT(7)
+
+struct pm8941_wled_config {
+	u32 i_boost_limit;
+	u32 ovp;
+	u32 switch_freq;
+	u32 num_strings;
+	u32 i_limit;
+	bool cs_out_en;
+	bool ext_gen;
+	bool cabc_en;
+};
+
+struct pm8941_wled {
+	struct regmap *regmap;
+	u16 addr;
+
+	struct led_classdev cdev;
+
+	struct pm8941_wled_config cfg;
+};
+
+static int pm8941_wled_set(struct led_classdev *cdev,
+			   enum led_brightness value)
+{
+	struct pm8941_wled *wled;
+	u8 ctrl = 0;
+	u16 val;
+	int rc;
+	int i;
+
+	wled = container_of(cdev, struct pm8941_wled, cdev);
+
+	if (value != 0)
+		ctrl = PM8941_WLED_REG_MOD_EN_BIT;
+
+	val = value * PM8941_WLED_REG_VAL_MAX / LED_FULL;
+
+	rc = regmap_update_bits(wled->regmap,
+			wled->addr + PM8941_WLED_REG_MOD_EN,
+			PM8941_WLED_REG_MOD_EN_MASK, ctrl);
+	if (rc)
+		return rc;
+
+	for (i = 0; i < wled->cfg.num_strings; ++i) {
+		u8 v[2] = { val & 0xff, (val >> 8) & 0xf };
+
+		rc = regmap_bulk_write(wled->regmap,
+				wled->addr + PM8941_WLED_REG_VAL_BASE + 2 * i,
+				v, 2);
+		if (rc)
+			return rc;
+	}
+
+	rc = regmap_update_bits(wled->regmap,
+			wled->addr + PM8941_WLED_REG_SYNC,
+			PM8941_WLED_REG_SYNC_MASK, PM8941_WLED_REG_SYNC_ALL);
+	if (rc)
+		return rc;
+
+	rc = regmap_update_bits(wled->regmap,
+			wled->addr + PM8941_WLED_REG_SYNC,
+			PM8941_WLED_REG_SYNC_MASK, PM8941_WLED_REG_SYNC_CLEAR);
+	return rc;
+}
+
+static void pm8941_wled_set_brightness(struct led_classdev *cdev,
+				       enum led_brightness value)
+{
+	if (pm8941_wled_set(cdev, value)) {
+		dev_err(cdev->dev, "Unable to set brightness\n");
+		return;
+	}
+	cdev->brightness = value;
+}
+
+static int pm8941_wled_setup(struct pm8941_wled *wled)
+{
+	int rc;
+	int i;
+
+	rc = regmap_update_bits(wled->regmap,
+			wled->addr + PM8941_WLED_REG_OVP,
+			PM8941_WLED_REG_OVP_MASK, wled->cfg.ovp);
+	if (rc)
+		return rc;
+
+	rc = regmap_update_bits(wled->regmap,
+			wled->addr + PM8941_WLED_REG_BOOST,
+			PM8941_WLED_REG_BOOST_MASK, wled->cfg.i_boost_limit);
+	if (rc)
+		return rc;
+
+	rc = regmap_update_bits(wled->regmap,
+			wled->addr + PM8941_WLED_REG_FREQ,
+			PM8941_WLED_REG_FREQ_MASK, wled->cfg.switch_freq);
+	if (rc)
+		return rc;
+
+	if (wled->cfg.cs_out_en) {
+		u8 all = (BIT(wled->cfg.num_strings) - 1)
+				<< PM8941_WLED_REG_SINK_SHFT;
+
+		rc = regmap_update_bits(wled->regmap,
+				wled->addr + PM8941_WLED_REG_SINK,
+				PM8941_WLED_REG_SINK_MASK, all);
+		if (rc)
+			return rc;
+	}
+
+	for (i = 0; i < wled->cfg.num_strings; ++i) {
+		u16 addr = wled->addr + PM8941_WLED_REG_STR_OFFSET * i;
+
+		rc = regmap_update_bits(wled->regmap,
+				addr + PM8941_WLED_REG_STR_MOD_EN_BASE,
+				PM8941_WLED_REG_STR_MOD_MASK,
+				PM8941_WLED_REG_STR_MOD_EN);
+		if (rc)
+			return rc;
+
+		if (wled->cfg.ext_gen) {
+			rc = regmap_update_bits(wled->regmap,
+					addr + PM8941_WLED_REG_STR_MOD_SRC_BASE,
+					PM8941_WLED_REG_STR_MOD_SRC_MASK,
+					PM8941_WLED_REG_STR_MOD_SRC_EXT);
+			if (rc)
+				return rc;
+		}
+
+		rc = regmap_update_bits(wled->regmap,
+				addr + PM8941_WLED_REG_STR_SCALE_BASE,
+				PM8941_WLED_REG_STR_SCALE_MASK,
+				wled->cfg.i_limit);
+		if (rc)
+			return rc;
+
+		rc = regmap_update_bits(wled->regmap,
+				addr + PM8941_WLED_REG_STR_CABC_BASE,
+				PM8941_WLED_REG_STR_CABC_MASK,
+				wled->cfg.cabc_en ?
+					PM8941_WLED_REG_STR_CABC_EN : 0);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static const struct pm8941_wled_config pm8941_wled_config_defaults = {
+	.i_boost_limit = 3,
+	.i_limit = 20,
+	.ovp = 2,
+	.switch_freq = 5,
+	.num_strings = 0,
+	.cs_out_en = false,
+	.ext_gen = false,
+	.cabc_en = false,
+};
+
+struct pm8941_wled_var_cfg {
+	const u32 *values;
+	u32 (*fn)(u32);
+	int size;
+};
+
+static const u32 pm8941_wled_i_boost_limit_values[] = {
+	105, 385, 525, 805, 980, 1260, 1400, 1680,
+};
+
+static const struct pm8941_wled_var_cfg pm8941_wled_i_boost_limit_cfg = {
+	.values = pm8941_wled_i_boost_limit_values,
+	.size = ARRAY_SIZE(pm8941_wled_i_boost_limit_values),
+};
+
+static const u32 pm8941_wled_ovp_values[] = {
+	35, 32, 29, 27,
+};
+
+static const struct pm8941_wled_var_cfg pm8941_wled_ovp_cfg = {
+	.values = pm8941_wled_ovp_values,
+	.size = ARRAY_SIZE(pm8941_wled_ovp_values),
+};
+
+static u32 pm8941_wled_num_strings_values_fn(u32 idx)
+{
+	return idx + 1;
+}
+
+static const struct pm8941_wled_var_cfg pm8941_wled_num_strings_cfg = {
+	.fn = pm8941_wled_num_strings_values_fn,
+	.size = 3,
+};
+
+static u32 pm8941_wled_switch_freq_values_fn(u32 idx)
+{
+	return 19200 / (2 * (1 + idx));
+}
+
+static const struct pm8941_wled_var_cfg pm8941_wled_switch_freq_cfg = {
+	.fn = pm8941_wled_switch_freq_values_fn,
+	.size = 16,
+};
+
+static const struct pm8941_wled_var_cfg pm8941_wled_i_limit_cfg = {
+	.size = 26,
+};
+
+static u32 pm8941_wled_values(const struct pm8941_wled_var_cfg *cfg, u32 idx)
+{
+	if (idx >= cfg->size)
+		return UINT_MAX;
+	if (cfg->fn)
+		return cfg->fn(idx);
+	if (cfg->values)
+		return cfg->values[idx];
+	return idx;
+}
+
+static int pm8941_wled_configure(struct pm8941_wled *wled, struct device *dev)
+{
+	struct pm8941_wled_config *cfg = &wled->cfg;
+	u32 val;
+	int rc;
+	u32 c;
+	int i;
+	int j;
+
+	const struct {
+		const char *name;
+		u32 *val_ptr;
+		const struct pm8941_wled_var_cfg *cfg;
+	} u32_opts[] = {
+		{
+			"qcom,current-boost-limit",
+			&cfg->i_boost_limit,
+			.cfg = &pm8941_wled_i_boost_limit_cfg,
+		},
+		{
+			"qcom,current-limit",
+			&cfg->i_limit,
+			.cfg = &pm8941_wled_i_limit_cfg,
+		},
+		{
+			"qcom,ovp",
+			&cfg->ovp,
+			.cfg = &pm8941_wled_ovp_cfg,
+		},
+		{
+			"qcom,switching-freq",
+			&cfg->switch_freq,
+			.cfg = &pm8941_wled_switch_freq_cfg,
+		},
+		{
+			"qcom,num-strings",
+			&cfg->num_strings,
+			.cfg = &pm8941_wled_num_strings_cfg,
+		},
+	};
+	const struct {
+		const char *name;
+		bool *val_ptr;
+	} bool_opts[] = {
+		{ "qcom,cs-out", &cfg->cs_out_en, },
+		{ "qcom,ext-gen", &cfg->ext_gen, },
+		{ "qcom,cabc", &cfg->cabc_en, },
+	};
+
+	rc = of_property_read_u32(dev->of_node, "reg", &val);
+	if (rc || val > 0xffff) {
+		dev_err(dev, "invalid IO resources\n");
+		return rc ? rc : -EINVAL;
+	}
+	wled->addr = val;
+
+	rc = of_property_read_string(dev->of_node, "label", &wled->cdev.name);
+	if (rc)
+		wled->cdev.name = dev->of_node->name;
+
+	wled->cdev.default_trigger = of_get_property(dev->of_node,
+			"linux,default-trigger", NULL);
+
+	*cfg = pm8941_wled_config_defaults;
+	for (i = 0; i < ARRAY_SIZE(u32_opts); ++i) {
+		rc = of_property_read_u32(dev->of_node, u32_opts[i].name, &val);
+		if (rc == -EINVAL) {
+			continue;
+		} else if (rc) {
+			dev_err(dev, "error reading '%s'\n", u32_opts[i].name);
+			return rc;
+		}
+
+		c = UINT_MAX;
+		for (j = 0; c != val; j++) {
+			c = pm8941_wled_values(u32_opts[i].cfg, j);
+			if (c == UINT_MAX) {
+				dev_err(dev, "invalid value for '%s'\n",
+					u32_opts[i].name);
+				return -EINVAL;
+			}
+		}
+
+		dev_dbg(dev, "'%s' = %u\n", u32_opts[i].name, c);
+		*u32_opts[i].val_ptr = j;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bool_opts); ++i) {
+		if (of_property_read_bool(dev->of_node, bool_opts[i].name))
+			*bool_opts[i].val_ptr = true;
+	}
+
+	cfg->num_strings = cfg->num_strings + 1;
+
+	return 0;
+}
+
+static int pm8941_wled_probe(struct platform_device *pdev)
+{
+	struct pm8941_wled *wled;
+	struct regmap *regmap;
+	int rc;
+
+	regmap = dev_get_regmap(pdev->dev.parent, NULL);
+	if (!regmap) {
+		dev_err(&pdev->dev, "Unable to get regmap\n");
+		return -EINVAL;
+	}
+
+	wled = devm_kzalloc(&pdev->dev, sizeof(*wled), GFP_KERNEL);
+	if (!wled)
+		return -ENOMEM;
+
+	wled->regmap = regmap;
+
+	rc = pm8941_wled_configure(wled, &pdev->dev);
+	if (rc)
+		return rc;
+
+	rc = pm8941_wled_setup(wled);
+	if (rc)
+		return rc;
+
+	wled->cdev.brightness_set = pm8941_wled_set_brightness;
+
+	rc = devm_led_classdev_register(&pdev->dev, &wled->cdev);
+	if (rc)
+		return rc;
+
+	platform_set_drvdata(pdev, wled);
+
+	return 0;
+};
+
+static const struct of_device_id pm8941_wled_match_table[] = {
+	{ .compatible = "qcom,pm8941-wled" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, pm8941_wled_match_table);
+
+static struct platform_driver pm8941_wled_driver = {
+	.probe = pm8941_wled_probe,
+	.driver	= {
+		.name = "pm8941-wled",
+		.of_match_table	= pm8941_wled_match_table,
+	},
+};
+
+module_platform_driver(pm8941_wled_driver);
+
+MODULE_DESCRIPTION("pm8941 wled driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:pm8941-wled");
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index f668500a2157..1d07e3e83d29 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -121,9 +121,6 @@ static int led_pwm_add(struct device *dev, struct led_pwm_priv *priv,
 		return ret;
 	}
 
-	if (child)
-		led_data->period = pwm_get_period(led_data->pwm);
-
 	led_data->can_sleep = pwm_can_sleep(led_data->pwm);
 	if (led_data->can_sleep)
 		INIT_WORK(&led_data->work, led_pwm_work);
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 cadf9cc02b25..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);
@@ -5080,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/media/dvb-core/dvb_net.c b/drivers/media/dvb-core/dvb_net.c
index 686d3277dad1..4a77cb02dffc 100644
--- a/drivers/media/dvb-core/dvb_net.c
+++ b/drivers/media/dvb-core/dvb_net.c
@@ -1190,7 +1190,6 @@ static int dvb_net_stop(struct net_device *dev)
 static const struct header_ops dvb_header_ops = {
 	.create		= eth_header,
 	.parse		= eth_header_parse,
-	.rebuild	= eth_rebuild_header,
 };
 
 
diff --git a/drivers/media/dvb-frontends/m88ds3103.c b/drivers/media/dvb-frontends/m88ds3103.c
index ba4ee0b48834..d3d928e1c0ce 100644
--- a/drivers/media/dvb-frontends/m88ds3103.c
+++ b/drivers/media/dvb-frontends/m88ds3103.c
@@ -630,7 +630,7 @@ static int m88ds3103_init(struct dvb_frontend *fe)
 	/* request the firmware, this will block and timeout */
 	ret = request_firmware(&fw, fw_file, priv->i2c->dev.parent);
 	if (ret) {
-		dev_err(&priv->i2c->dev, "%s: firmare file '%s' not found\n",
+		dev_err(&priv->i2c->dev, "%s: firmware file '%s' not found\n",
 				KBUILD_MODNAME, fw_file);
 		goto err;
 	}
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/dvb-frontends/si2168_priv.h b/drivers/media/dvb-frontends/si2168_priv.h
index aadd1367673f..d7efce8043ed 100644
--- a/drivers/media/dvb-frontends/si2168_priv.h
+++ b/drivers/media/dvb-frontends/si2168_priv.h
@@ -40,7 +40,7 @@ struct si2168_dev {
 	bool ts_clock_inv;
 };
 
-/* firmare command struct */
+/* firmware command struct */
 #define SI2168_ARGLEN      30
 struct si2168_cmd {
 	u8 args[SI2168_ARGLEN];
diff --git a/drivers/media/dvb-frontends/tda10071_priv.h b/drivers/media/dvb-frontends/tda10071_priv.h
index 420486192736..03f839c431e9 100644
--- a/drivers/media/dvb-frontends/tda10071_priv.h
+++ b/drivers/media/dvb-frontends/tda10071_priv.h
@@ -99,7 +99,7 @@ struct tda10071_reg_val_mask {
 #define CMD_BER_CONTROL         0x3e
 #define CMD_BER_UPDATE_COUNTERS 0x3f
 
-/* firmare command struct */
+/* firmware command struct */
 #define TDA10071_ARGLEN      30
 struct tda10071_cmd {
 	u8 args[TDA10071_ARGLEN];
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/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index e80f2c6c5f1a..8d77e1c4a141 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -1195,16 +1195,6 @@ static struct pnp_driver ene_driver = {
 	.shutdown = ene_shutdown,
 };
 
-static int __init ene_init(void)
-{
-	return pnp_register_driver(&ene_driver);
-}
-
-static void ene_exit(void)
-{
-	pnp_unregister_driver(&ene_driver);
-}
-
 module_param(sample_period, int, S_IRUGO);
 MODULE_PARM_DESC(sample_period, "Hardware sample period (50 us default)");
 
@@ -1226,5 +1216,4 @@ MODULE_DESCRIPTION
 MODULE_AUTHOR("Maxim Levitsky");
 MODULE_LICENSE("GPL");
 
-module_init(ene_init);
-module_exit(ene_exit);
+module_pnp_driver(ene_driver);
diff --git a/drivers/media/rc/fintek-cir.c b/drivers/media/rc/fintek-cir.c
index b5167573240e..5c63c2ec6183 100644
--- a/drivers/media/rc/fintek-cir.c
+++ b/drivers/media/rc/fintek-cir.c
@@ -684,16 +684,6 @@ static struct pnp_driver fintek_driver = {
 	.shutdown	= fintek_shutdown,
 };
 
-static int __init fintek_init(void)
-{
-	return pnp_register_driver(&fintek_driver);
-}
-
-static void __exit fintek_exit(void)
-{
-	pnp_unregister_driver(&fintek_driver);
-}
-
 module_param(debug, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Enable debugging output");
 
@@ -703,5 +693,4 @@ MODULE_DESCRIPTION(FINTEK_DESCRIPTION " driver");
 MODULE_AUTHOR("Jarod Wilson <jarod@redhat.com>");
 MODULE_LICENSE("GPL");
 
-module_init(fintek_init);
-module_exit(fintek_exit);
+module_pnp_driver(fintek_driver);
diff --git a/drivers/media/rc/ite-cir.c b/drivers/media/rc/ite-cir.c
index 56abf9120cc2..0f301903aa6f 100644
--- a/drivers/media/rc/ite-cir.c
+++ b/drivers/media/rc/ite-cir.c
@@ -1708,21 +1708,10 @@ static struct pnp_driver ite_driver = {
 	.shutdown	= ite_shutdown,
 };
 
-static int __init ite_init(void)
-{
-	return pnp_register_driver(&ite_driver);
-}
-
-static void __exit ite_exit(void)
-{
-	pnp_unregister_driver(&ite_driver);
-}
-
 MODULE_DEVICE_TABLE(pnp, ite_ids);
 MODULE_DESCRIPTION("ITE Tech Inc. IT8712F/ITE8512F CIR driver");
 
 MODULE_AUTHOR("Juan J. Garcia de Soria <skandalfo@gmail.com>");
 MODULE_LICENSE("GPL");
 
-module_init(ite_init);
-module_exit(ite_exit);
+module_pnp_driver(ite_driver);
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 9c2c8635ff33..85af7a869167 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -1219,16 +1219,6 @@ static struct pnp_driver nvt_driver = {
 	.shutdown	= nvt_shutdown,
 };
 
-static int __init nvt_init(void)
-{
-	return pnp_register_driver(&nvt_driver);
-}
-
-static void __exit nvt_exit(void)
-{
-	pnp_unregister_driver(&nvt_driver);
-}
-
 module_param(debug, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Enable debugging output");
 
@@ -1238,5 +1228,4 @@ MODULE_DESCRIPTION("Nuvoton W83667HG-A & W83677HG-I CIR driver");
 MODULE_AUTHOR("Jarod Wilson <jarod@redhat.com>");
 MODULE_LICENSE("GPL");
 
-module_init(nvt_init);
-module_exit(nvt_exit);
+module_pnp_driver(nvt_driver);
diff --git a/drivers/media/tuners/msi001.c b/drivers/media/tuners/msi001.c
index 26019e731993..74cfc3c98edb 100644
--- a/drivers/media/tuners/msi001.c
+++ b/drivers/media/tuners/msi001.c
@@ -408,7 +408,7 @@ static int msi001_s_ctrl(struct v4l2_ctrl *ctrl)
 				s->mixer_gain->cur.val, s->if_gain->val);
 		break;
 	default:
-		dev_dbg(&s->spi->dev, "unkown control %d\n", ctrl->id);
+		dev_dbg(&s->spi->dev, "unknown control %d\n", ctrl->id);
 		ret = -EINVAL;
 	}
 
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/Kconfig b/drivers/mfd/Kconfig
index 38356e39adba..d5ad04dad081 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -283,6 +283,18 @@ config HTC_I2CPLD
 	  This device provides input and output GPIOs through an I2C
 	  interface to one or more sub-chips.
 
+config MFD_INTEL_QUARK_I2C_GPIO
+	tristate "Intel Quark MFD I2C GPIO"
+	depends on PCI
+	depends on X86
+	depends on COMMON_CLK
+	select MFD_CORE
+	help
+	  This MFD provides support for I2C and GPIO that exist only
+	  in a single PCI device. It splits the 2 IO devices to
+	  their respective IO driver.
+	  The GPIO exports a total amount of 8 interrupt-capable GPIOs.
+
 config LPC_ICH
 	tristate "Intel ICH LPC"
 	depends on PCI
@@ -364,6 +376,7 @@ config MFD_KEMPLD
 		* COMe-bIP#
 		* COMe-bPC2 (ETXexpress-PC)
 		* COMe-bSC# (ETXexpress-SC T#)
+		* COMe-cBL6
 		* COMe-cBT6
 		* COMe-cCT6
 		* COMe-cDC2 (microETXexpress-DC)
@@ -455,6 +468,20 @@ config MFD_MAX77693
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
 
+config MFD_MAX77843
+	bool "Maxim Semiconductor MAX77843 PMIC Support"
+	depends on I2C=y
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	help
+	  Say yes here to add support for Maxim Semiconductor MAX77843.
+	  This is companion Power Management IC with LEDs, Haptic, Charger,
+	  Fuel Gauge, MUIC(Micro USB Interface Controller) controls on chip.
+	  This driver provides common support for accessing the device;
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.
+
 config MFD_MAX8907
 	tristate "Maxim Semiconductor MAX8907 PMIC Support"
 	select MFD_CORE
@@ -502,6 +529,16 @@ config MFD_MAX8998
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
 
+config MFD_MT6397
+	tristate "MediaTek MT6397 PMIC Support"
+	select MFD_CORE
+	select IRQ_DOMAIN
+	help
+	  Say yes here to add support for MediaTek MT6397 PMIC. This is
+	  a Power Management IC. This driver provides common support for
+	  accessing the device; additional drivers must be enabled in order
+	  to use the functionality of the device.
+
 config MFD_MENF21BMC
 	tristate "MEN 14F021P00 Board Management Controller Support"
 	depends on I2C
@@ -655,6 +692,7 @@ config MFD_RT5033
 	depends on I2C=y
 	select MFD_CORE
 	select REGMAP_I2C
+	select REGMAP_IRQ
 	help
 	  This driver provides for the Richtek RT5033 Power Management IC,
 	  which includes the I2C driver and the Core APIs. This driver provides
@@ -753,6 +791,18 @@ config MFD_SM501_GPIO
 	 lines on the SM501. The platform data is used to supply the
 	 base number for the first GPIO line to register.
 
+config MFD_SKY81452
+	tristate "Skyworks Solutions SKY81452"
+	select MFD_CORE
+	select REGMAP_I2C
+	depends on I2C
+	help
+	  This is the core driver for the Skyworks SKY81452 backlight and
+	  voltage regulator device.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called sky81452.
+
 config MFD_SMSC
        bool "SMSC ECE1099 series chips"
        depends on I2C=y
@@ -1210,6 +1260,7 @@ config MFD_TIMBERDALE
 config MFD_TC3589X
 	bool "Toshiba TC35892 and variants"
 	depends on I2C=y
+	depends on OF
 	select MFD_CORE
 	help
 	  Support for the Toshiba TC35892 and variants I/O Expander.
@@ -1289,10 +1340,11 @@ config MFD_WM5102
 	  Support for Wolfson Microelectronics WM5102 low power audio SoC
 
 config MFD_WM5110
-	bool "Wolfson Microelectronics WM5110"
+	bool "Wolfson Microelectronics WM5110 and WM8280/WM8281"
 	depends on MFD_ARIZONA
 	help
-	  Support for Wolfson Microelectronics WM5110 low power audio SoC
+	  Support for Wolfson Microelectronics WM5110 and WM8280/WM8281
+	  low power audio SoC
 
 config MFD_WM8997
 	bool "Wolfson Microelectronics WM8997"
@@ -1362,7 +1414,7 @@ config MFD_WM8994
 	depends on I2C
 	help
 	  The WM8994 is a highly integrated hi-fi CODEC designed for
-	  smartphone applicatiosn.  As well as audio functionality it
+	  smartphone applications.  As well as audio functionality it
 	  has on board GPIO and regulator functionality which is
 	  supported via the relevant subsystems.  This driver provides
 	  core support for the WM8994, in order to use the actual
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 19f3d744e3bd..0e5cfeba107c 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -13,7 +13,7 @@ obj-$(CONFIG_MFD_CROS_EC)	+= cros_ec.o
 obj-$(CONFIG_MFD_CROS_EC_I2C)	+= cros_ec_i2c.o
 obj-$(CONFIG_MFD_CROS_EC_SPI)	+= cros_ec_spi.o
 
-rtsx_pci-objs			:= rtsx_pcr.o rtsx_gops.o rts5209.o rts5229.o rtl8411.o rts5227.o rts5249.o
+rtsx_pci-objs			:= rtsx_pcr.o rts5209.o rts5229.o rtl8411.o rts5227.o rts5249.o
 obj-$(CONFIG_MFD_RTSX_PCI)	+= rtsx_pci.o
 obj-$(CONFIG_MFD_RTSX_USB)	+= rtsx_usb.o
 
@@ -117,6 +117,7 @@ obj-$(CONFIG_MFD_DA9150)	+= da9150-core.o
 obj-$(CONFIG_MFD_MAX14577)	+= max14577.o
 obj-$(CONFIG_MFD_MAX77686)	+= max77686.o
 obj-$(CONFIG_MFD_MAX77693)	+= max77693.o
+obj-$(CONFIG_MFD_MAX77843)	+= max77843.o
 obj-$(CONFIG_MFD_MAX8907)	+= max8907.o
 max8925-objs			:= max8925-core.o max8925-i2c.o
 obj-$(CONFIG_MFD_MAX8925)	+= max8925.o
@@ -138,6 +139,7 @@ obj-$(CONFIG_AB8500_CORE)	+= ab8500-core.o ab8500-sysctrl.o
 obj-$(CONFIG_MFD_TIMBERDALE)    += timberdale.o
 obj-$(CONFIG_PMIC_ADP5520)	+= adp5520.o
 obj-$(CONFIG_MFD_KEMPLD)	+= kempld-core.o
+obj-$(CONFIG_MFD_INTEL_QUARK_I2C_GPIO)	+= intel_quark_i2c_gpio.o
 obj-$(CONFIG_LPC_SCH)		+= lpc_sch.o
 obj-$(CONFIG_LPC_ICH)		+= lpc_ich.o
 obj-$(CONFIG_MFD_RDC321X)	+= rdc321x-southbridge.o
@@ -178,6 +180,8 @@ obj-$(CONFIG_MFD_MENF21BMC)	+= menf21bmc.o
 obj-$(CONFIG_MFD_HI6421_PMIC)	+= hi6421-pmic-core.o
 obj-$(CONFIG_MFD_DLN2)		+= dln2.o
 obj-$(CONFIG_MFD_RT5033)	+= rt5033.o
+obj-$(CONFIG_MFD_SKY81452)	+= sky81452.o
 
 intel-soc-pmic-objs		:= intel_soc_pmic_core.o intel_soc_pmic_crc.o
 obj-$(CONFIG_INTEL_SOC_PMIC)	+= intel-soc-pmic.o
+obj-$(CONFIG_MFD_MT6397)	+= mt6397-core.o
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 9a8e185f11df..cdd6f3d63314 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -1283,7 +1283,7 @@ static irqreturn_t ab8500_debug_handler(int irq, void *data)
 
 /* Prints to seq_file or log_buf */
 static int ab8500_registers_print(struct device *dev, u32 bank,
-				struct seq_file *s)
+				  struct seq_file *s)
 {
 	unsigned int i;
 
@@ -1304,20 +1304,19 @@ static int ab8500_registers_print(struct device *dev, u32 bank,
 			}
 
 			if (s) {
-				err = seq_printf(s,
-						 "  [0x%02X/0x%02X]: 0x%02X\n",
-						 bank, reg, value);
-				if (err < 0) {
-					/* Error is not returned here since
-					 * the output is wanted in any case */
+				seq_printf(s, "  [0x%02X/0x%02X]: 0x%02X\n",
+					   bank, reg, value);
+				/* Error is not returned here since
+				 * the output is wanted in any case */
+				if (seq_has_overflowed(s))
 					return 0;
-				}
 			} else {
 				dev_info(dev, " [0x%02X/0x%02X]: 0x%02X\n",
 					 bank, reg, value);
 			}
 		}
 	}
+
 	return 0;
 }
 
@@ -1330,8 +1329,7 @@ static int ab8500_print_bank_registers(struct seq_file *s, void *p)
 
 	seq_printf(s, " bank 0x%02X:\n", bank);
 
-	ab8500_registers_print(dev, bank, s);
-	return 0;
+	return ab8500_registers_print(dev, bank, s);
 }
 
 static int ab8500_registers_open(struct inode *inode, struct file *file)
@@ -1355,9 +1353,12 @@ static int ab8500_print_all_banks(struct seq_file *s, void *p)
 	seq_puts(s, AB8500_NAME_STRING " register values:\n");
 
 	for (i = 0; i < AB8500_NUM_BANKS; i++) {
-		seq_printf(s, " bank 0x%02X:\n", i);
+		int err;
 
-		ab8500_registers_print(dev, i, s);
+		seq_printf(s, " bank 0x%02X:\n", i);
+		err = ab8500_registers_print(dev, i, s);
+		if (err)
+			return err;
 	}
 	return 0;
 }
@@ -1458,7 +1459,8 @@ static const struct file_operations ab8500_all_banks_fops = {
 
 static int ab8500_bank_print(struct seq_file *s, void *p)
 {
-	return seq_printf(s, "0x%02X\n", debug_bank);
+	seq_printf(s, "0x%02X\n", debug_bank);
+	return 0;
 }
 
 static int ab8500_bank_open(struct inode *inode, struct file *file)
@@ -1490,7 +1492,8 @@ static ssize_t ab8500_bank_write(struct file *file,
 
 static int ab8500_address_print(struct seq_file *s, void *p)
 {
-	return seq_printf(s, "0x%02X\n", debug_address);
+	seq_printf(s, "0x%02X\n", debug_address);
+	return 0;
 }
 
 static int ab8500_address_open(struct inode *inode, struct file *file)
@@ -1598,7 +1601,8 @@ static int ab8500_interrupts_print(struct seq_file *s, void *p)
 	for (line = 0; line < num_interrupt_lines; line++) {
 		struct irq_desc *desc = irq_to_desc(line + irq_first);
 
-		seq_printf(s, "%3i:  %6i %4i", line,
+		seq_printf(s, "%3i:  %6i %4i",
+			   line,
 			   num_interrupts[line],
 			   num_wake_interrupts[line]);
 
@@ -1705,8 +1709,7 @@ static int ab8500_print_modem_registers(struct seq_file *s, void *p)
 			dev_err(dev, "ab->read fail %d\n", err);
 			return err;
 		}
-		err = seq_printf(s, "  [0x%02X/0x%02X]: 0x%02X\n",
-			bank, reg, value);
+		seq_printf(s, "  [0x%02X/0x%02X]: 0x%02X\n", bank, reg, value);
 	}
 	err = abx500_set_register_interruptible(dev,
 		AB8500_REGU_CTRL1, AB8500_SUPPLY_CONTROL_REG, orig_value);
@@ -1743,8 +1746,9 @@ static int ab8500_gpadc_bat_ctrl_print(struct seq_file *s, void *p)
 	bat_ctrl_convert = ab8500_gpadc_ad_to_voltage(gpadc,
 		BAT_CTRL, bat_ctrl_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		bat_ctrl_convert, bat_ctrl_raw);
+	seq_printf(s, "%d,0x%X\n", bat_ctrl_convert, bat_ctrl_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_bat_ctrl_open(struct inode *inode, struct file *file)
@@ -1773,8 +1777,9 @@ static int ab8500_gpadc_btemp_ball_print(struct seq_file *s, void *p)
 	btemp_ball_convert = ab8500_gpadc_ad_to_voltage(gpadc, BTEMP_BALL,
 		btemp_ball_raw);
 
-	return seq_printf(s,
-		"%d,0x%X\n", btemp_ball_convert, btemp_ball_raw);
+	seq_printf(s, "%d,0x%X\n", btemp_ball_convert, btemp_ball_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_btemp_ball_open(struct inode *inode,
@@ -1804,8 +1809,9 @@ static int ab8500_gpadc_main_charger_v_print(struct seq_file *s, void *p)
 	main_charger_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
 		MAIN_CHARGER_V, main_charger_v_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-			main_charger_v_convert, main_charger_v_raw);
+	seq_printf(s, "%d,0x%X\n", main_charger_v_convert, main_charger_v_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_main_charger_v_open(struct inode *inode,
@@ -1835,8 +1841,9 @@ static int ab8500_gpadc_acc_detect1_print(struct seq_file *s, void *p)
 	acc_detect1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ACC_DETECT1,
 		acc_detect1_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		acc_detect1_convert, acc_detect1_raw);
+	seq_printf(s, "%d,0x%X\n", acc_detect1_convert, acc_detect1_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_acc_detect1_open(struct inode *inode,
@@ -1866,8 +1873,9 @@ static int ab8500_gpadc_acc_detect2_print(struct seq_file *s, void *p)
 	acc_detect2_convert = ab8500_gpadc_ad_to_voltage(gpadc,
 		ACC_DETECT2, acc_detect2_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		acc_detect2_convert, acc_detect2_raw);
+	seq_printf(s, "%d,0x%X\n", acc_detect2_convert, acc_detect2_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_acc_detect2_open(struct inode *inode,
@@ -1897,8 +1905,9 @@ static int ab8500_gpadc_aux1_print(struct seq_file *s, void *p)
 	aux1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX1,
 		aux1_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		aux1_convert, aux1_raw);
+	seq_printf(s, "%d,0x%X\n", aux1_convert, aux1_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_aux1_open(struct inode *inode, struct file *file)
@@ -1926,8 +1935,9 @@ static int ab8500_gpadc_aux2_print(struct seq_file *s, void *p)
 	aux2_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX2,
 		aux2_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-			aux2_convert, aux2_raw);
+	seq_printf(s, "%d,0x%X\n", aux2_convert, aux2_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_aux2_open(struct inode *inode, struct file *file)
@@ -1955,8 +1965,9 @@ static int ab8500_gpadc_main_bat_v_print(struct seq_file *s, void *p)
 	main_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, MAIN_BAT_V,
 		main_bat_v_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		main_bat_v_convert, main_bat_v_raw);
+	seq_printf(s, "%d,0x%X\n", main_bat_v_convert, main_bat_v_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_main_bat_v_open(struct inode *inode,
@@ -1986,8 +1997,9 @@ static int ab8500_gpadc_vbus_v_print(struct seq_file *s, void *p)
 	vbus_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, VBUS_V,
 		vbus_v_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		vbus_v_convert, vbus_v_raw);
+	seq_printf(s, "%d,0x%X\n", vbus_v_convert, vbus_v_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_vbus_v_open(struct inode *inode, struct file *file)
@@ -2015,8 +2027,9 @@ static int ab8500_gpadc_main_charger_c_print(struct seq_file *s, void *p)
 	main_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
 		MAIN_CHARGER_C, main_charger_c_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		main_charger_c_convert, main_charger_c_raw);
+	seq_printf(s, "%d,0x%X\n", main_charger_c_convert, main_charger_c_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_main_charger_c_open(struct inode *inode,
@@ -2046,8 +2059,9 @@ static int ab8500_gpadc_usb_charger_c_print(struct seq_file *s, void *p)
 	usb_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
 		USB_CHARGER_C, usb_charger_c_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		usb_charger_c_convert, usb_charger_c_raw);
+	seq_printf(s, "%d,0x%X\n", usb_charger_c_convert, usb_charger_c_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_usb_charger_c_open(struct inode *inode,
@@ -2077,8 +2091,9 @@ static int ab8500_gpadc_bk_bat_v_print(struct seq_file *s, void *p)
 	bk_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
 		BK_BAT_V, bk_bat_v_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		bk_bat_v_convert, bk_bat_v_raw);
+	seq_printf(s, "%d,0x%X\n", bk_bat_v_convert, bk_bat_v_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_bk_bat_v_open(struct inode *inode, struct file *file)
@@ -2107,8 +2122,9 @@ static int ab8500_gpadc_die_temp_print(struct seq_file *s, void *p)
 	die_temp_convert = ab8500_gpadc_ad_to_voltage(gpadc, DIE_TEMP,
 		die_temp_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		die_temp_convert, die_temp_raw);
+	seq_printf(s, "%d,0x%X\n", die_temp_convert, die_temp_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_die_temp_open(struct inode *inode, struct file *file)
@@ -2137,8 +2153,9 @@ static int ab8500_gpadc_usb_id_print(struct seq_file *s, void *p)
 	usb_id_convert = ab8500_gpadc_ad_to_voltage(gpadc, USB_ID,
 		usb_id_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		usb_id_convert, usb_id_raw);
+	seq_printf(s, "%d,0x%X\n", usb_id_convert, usb_id_raw);
+
+	return 0;
 }
 
 static int ab8500_gpadc_usb_id_open(struct inode *inode, struct file *file)
@@ -2166,8 +2183,9 @@ static int ab8540_gpadc_xtal_temp_print(struct seq_file *s, void *p)
 	xtal_temp_convert = ab8500_gpadc_ad_to_voltage(gpadc, XTAL_TEMP,
 		xtal_temp_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		xtal_temp_convert, xtal_temp_raw);
+	seq_printf(s, "%d,0x%X\n", xtal_temp_convert, xtal_temp_raw);
+
+	return 0;
 }
 
 static int ab8540_gpadc_xtal_temp_open(struct inode *inode, struct file *file)
@@ -2197,8 +2215,9 @@ static int ab8540_gpadc_vbat_true_meas_print(struct seq_file *s, void *p)
 		ab8500_gpadc_ad_to_voltage(gpadc, VBAT_TRUE_MEAS,
 					   vbat_true_meas_raw);
 
-	return seq_printf(s, "%d,0x%X\n",
-		vbat_true_meas_convert, vbat_true_meas_raw);
+	seq_printf(s, "%d,0x%X\n", vbat_true_meas_convert, vbat_true_meas_raw);
+
+	return 0;
 }
 
 static int ab8540_gpadc_vbat_true_meas_open(struct inode *inode,
@@ -2233,9 +2252,13 @@ static int ab8540_gpadc_bat_ctrl_and_ibat_print(struct seq_file *s, void *p)
 	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
 		ibat_raw);
 
-	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
-		bat_ctrl_convert, bat_ctrl_raw,
-		ibat_convert, ibat_raw);
+	seq_printf(s,
+		   "%d,0x%X\n"
+		   "%d,0x%X\n",
+		   bat_ctrl_convert, bat_ctrl_raw,
+		   ibat_convert, ibat_raw);
+
+	return 0;
 }
 
 static int ab8540_gpadc_bat_ctrl_and_ibat_open(struct inode *inode,
@@ -2269,9 +2292,13 @@ static int ab8540_gpadc_vbat_meas_and_ibat_print(struct seq_file *s, void *p)
 	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
 		ibat_raw);
 
-	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
-		vbat_meas_convert, vbat_meas_raw,
-		ibat_convert, ibat_raw);
+	seq_printf(s,
+		   "%d,0x%X\n"
+		   "%d,0x%X\n",
+		   vbat_meas_convert, vbat_meas_raw,
+		   ibat_convert, ibat_raw);
+
+	return 0;
 }
 
 static int ab8540_gpadc_vbat_meas_and_ibat_open(struct inode *inode,
@@ -2307,9 +2334,13 @@ static int ab8540_gpadc_vbat_true_meas_and_ibat_print(struct seq_file *s,
 	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
 		ibat_raw);
 
-	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
-		vbat_true_meas_convert, vbat_true_meas_raw,
-		ibat_convert, ibat_raw);
+	seq_printf(s,
+		   "%d,0x%X\n"
+		   "%d,0x%X\n",
+		   vbat_true_meas_convert, vbat_true_meas_raw,
+		   ibat_convert, ibat_raw);
+
+	return 0;
 }
 
 static int ab8540_gpadc_vbat_true_meas_and_ibat_open(struct inode *inode,
@@ -2344,9 +2375,13 @@ static int ab8540_gpadc_bat_temp_and_ibat_print(struct seq_file *s, void *p)
 	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
 		ibat_raw);
 
-	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
-		bat_temp_convert, bat_temp_raw,
-		ibat_convert, ibat_raw);
+	seq_printf(s,
+		   "%d,0x%X\n"
+		   "%d,0x%X\n",
+		   bat_temp_convert, bat_temp_raw,
+		   ibat_convert, ibat_raw);
+
+	return 0;
 }
 
 static int ab8540_gpadc_bat_temp_and_ibat_open(struct inode *inode,
@@ -2373,16 +2408,19 @@ static int ab8540_gpadc_otp_cal_print(struct seq_file *s, void *p)
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
 	ab8540_gpadc_get_otp(gpadc, &vmain_l, &vmain_h, &btemp_l, &btemp_h,
 			&vbat_l, &vbat_h, &ibat_l, &ibat_h);
-	return seq_printf(s, "VMAIN_L:0x%X\n"
-			  "VMAIN_H:0x%X\n"
-			  "BTEMP_L:0x%X\n"
-			  "BTEMP_H:0x%X\n"
-			  "VBAT_L:0x%X\n"
-			  "VBAT_H:0x%X\n"
-			  "IBAT_L:0x%X\n"
-			  "IBAT_H:0x%X\n",
-			  vmain_l, vmain_h, btemp_l, btemp_h,
-			  vbat_l, vbat_h, ibat_l, ibat_h);
+	seq_printf(s,
+		   "VMAIN_L:0x%X\n"
+		   "VMAIN_H:0x%X\n"
+		   "BTEMP_L:0x%X\n"
+		   "BTEMP_H:0x%X\n"
+		   "VBAT_L:0x%X\n"
+		   "VBAT_H:0x%X\n"
+		   "IBAT_L:0x%X\n"
+		   "IBAT_H:0x%X\n",
+		   vmain_l, vmain_h, btemp_l, btemp_h,
+		   vbat_l, vbat_h, ibat_l, ibat_h);
+
+	return 0;
 }
 
 static int ab8540_gpadc_otp_cal_open(struct inode *inode, struct file *file)
@@ -2400,7 +2438,9 @@ static const struct file_operations ab8540_gpadc_otp_calib_fops = {
 
 static int ab8500_gpadc_avg_sample_print(struct seq_file *s, void *p)
 {
-	return seq_printf(s, "%d\n", avg_sample);
+	seq_printf(s, "%d\n", avg_sample);
+
+	return 0;
 }
 
 static int ab8500_gpadc_avg_sample_open(struct inode *inode, struct file *file)
@@ -2445,7 +2485,9 @@ static const struct file_operations ab8500_gpadc_avg_sample_fops = {
 
 static int ab8500_gpadc_trig_edge_print(struct seq_file *s, void *p)
 {
-	return seq_printf(s, "%d\n", trig_edge);
+	seq_printf(s, "%d\n", trig_edge);
+
+	return 0;
 }
 
 static int ab8500_gpadc_trig_edge_open(struct inode *inode, struct file *file)
@@ -2490,7 +2532,9 @@ static const struct file_operations ab8500_gpadc_trig_edge_fops = {
 
 static int ab8500_gpadc_trig_timer_print(struct seq_file *s, void *p)
 {
-	return seq_printf(s, "%d\n", trig_timer);
+	seq_printf(s, "%d\n", trig_timer);
+
+	return 0;
 }
 
 static int ab8500_gpadc_trig_timer_open(struct inode *inode, struct file *file)
@@ -2533,7 +2577,9 @@ static const struct file_operations ab8500_gpadc_trig_timer_fops = {
 
 static int ab8500_gpadc_conv_type_print(struct seq_file *s, void *p)
 {
-	return seq_printf(s, "%d\n", conv_type);
+	seq_printf(s, "%d\n", conv_type);
+
+	return 0;
 }
 
 static int ab8500_gpadc_conv_type_open(struct inode *inode, struct file *file)
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/arizona-core.c b/drivers/mfd/arizona-core.c
index 09ba8f186e6a..6ca6dfab50eb 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -561,12 +561,23 @@ static int arizona_of_get_core_pdata(struct arizona *arizona)
 		count++;
 	}
 
+	count = 0;
+	of_property_for_each_u32(arizona->dev->of_node, "wlf,dmic-ref", prop,
+				 cur, val) {
+		if (count == ARRAY_SIZE(arizona->pdata.dmic_ref))
+			break;
+
+		arizona->pdata.dmic_ref[count] = val;
+		count++;
+	}
+
 	return 0;
 }
 
 const struct of_device_id arizona_of_match[] = {
 	{ .compatible = "wlf,wm5102", .data = (void *)WM5102 },
 	{ .compatible = "wlf,wm5110", .data = (void *)WM5110 },
+	{ .compatible = "wlf,wm8280", .data = (void *)WM8280 },
 	{ .compatible = "wlf,wm8997", .data = (void *)WM8997 },
 	{},
 };
@@ -671,6 +682,7 @@ int arizona_dev_init(struct arizona *arizona)
 	switch (arizona->type) {
 	case WM5102:
 	case WM5110:
+	case WM8280:
 	case WM8997:
 		for (i = 0; i < ARRAY_SIZE(wm5102_core_supplies); i++)
 			arizona->core_supplies[i].supply
@@ -834,11 +846,19 @@ int arizona_dev_init(struct arizona *arizona)
 #endif
 #ifdef CONFIG_MFD_WM5110
 	case 0x5110:
-		type_name = "WM5110";
-		if (arizona->type != WM5110) {
+		switch (arizona->type) {
+		case WM5110:
+			type_name = "WM5110";
+			break;
+		case WM8280:
+			type_name = "WM8280";
+			break;
+		default:
+			type_name = "WM5110";
 			dev_err(arizona->dev, "WM5110 registered as %d\n",
 				arizona->type);
 			arizona->type = WM5110;
+			break;
 		}
 		apply_patch = wm5110_patch;
 		break;
@@ -1010,6 +1030,7 @@ int arizona_dev_init(struct arizona *arizona)
 				      ARRAY_SIZE(wm5102_devs), NULL, 0, NULL);
 		break;
 	case WM5110:
+	case WM8280:
 		ret = mfd_add_devices(arizona->dev, -1, wm5110_devs,
 				      ARRAY_SIZE(wm5110_devs), NULL, 0, NULL);
 		break;
diff --git a/drivers/mfd/arizona-i2c.c b/drivers/mfd/arizona-i2c.c
index 9d4156fb082a..ff782a5de235 100644
--- a/drivers/mfd/arizona-i2c.c
+++ b/drivers/mfd/arizona-i2c.c
@@ -44,6 +44,7 @@ static int arizona_i2c_probe(struct i2c_client *i2c,
 #endif
 #ifdef CONFIG_MFD_WM5110
 	case WM5110:
+	case WM8280:
 		regmap_config = &wm5110_i2c_regmap;
 		break;
 #endif
@@ -87,6 +88,7 @@ static int arizona_i2c_remove(struct i2c_client *i2c)
 static const struct i2c_device_id arizona_i2c_id[] = {
 	{ "wm5102", WM5102 },
 	{ "wm5110", WM5110 },
+	{ "wm8280", WM8280 },
 	{ "wm8997", WM8997 },
 	{ }
 };
diff --git a/drivers/mfd/arizona-irq.c b/drivers/mfd/arizona-irq.c
index 3a3fe7cc6d61..d063b94b94b5 100644
--- a/drivers/mfd/arizona-irq.c
+++ b/drivers/mfd/arizona-irq.c
@@ -211,6 +211,7 @@ int arizona_irq_init(struct arizona *arizona)
 #endif
 #ifdef CONFIG_MFD_WM5110
 	case WM5110:
+	case WM8280:
 		aod = &wm5110_aod;
 
 		switch (arizona->rev) {
diff --git a/drivers/mfd/arizona-spi.c b/drivers/mfd/arizona-spi.c
index 8ef58bcff193..1e845f6d407b 100644
--- a/drivers/mfd/arizona-spi.c
+++ b/drivers/mfd/arizona-spi.c
@@ -44,6 +44,7 @@ static int arizona_spi_probe(struct spi_device *spi)
 #endif
 #ifdef CONFIG_MFD_WM5110
 	case WM5110:
+	case WM8280:
 		regmap_config = &wm5110_spi_regmap;
 		break;
 #endif
@@ -84,6 +85,7 @@ static int arizona_spi_remove(struct spi_device *spi)
 static const struct spi_device_id arizona_spi_ids[] = {
 	{ "wm5102", WM5102 },
 	{ "wm5110", WM5110 },
+	{ "wm8280", WM8280 },
 	{ },
 };
 MODULE_DEVICE_TABLE(spi, arizona_spi_ids);
diff --git a/drivers/mfd/axp20x.c b/drivers/mfd/axp20x.c
index b1b580a88654..d18029be6a78 100644
--- a/drivers/mfd/axp20x.c
+++ b/drivers/mfd/axp20x.c
@@ -29,7 +29,7 @@
 
 #define AXP20X_OFF	0x80
 
-static const char const *axp20x_model_names[] = {
+static const char * const axp20x_model_names[] = {
 	"AXP202",
 	"AXP209",
 	"AXP288",
@@ -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,
@@ -290,6 +290,29 @@ static struct resource axp288_adc_resources[] = {
 	},
 };
 
+static struct resource axp288_extcon_resources[] = {
+	{
+		.start = AXP288_IRQ_VBUS_FALL,
+		.end   = AXP288_IRQ_VBUS_FALL,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = AXP288_IRQ_VBUS_RISE,
+		.end   = AXP288_IRQ_VBUS_RISE,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = AXP288_IRQ_MV_CHNG,
+		.end   = AXP288_IRQ_MV_CHNG,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = AXP288_IRQ_BC_USB_CHNG,
+		.end   = AXP288_IRQ_BC_USB_CHNG,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
 static struct resource axp288_charger_resources[] = {
 	{
 		.start = AXP288_IRQ_OV,
@@ -345,14 +368,19 @@ static struct mfd_cell axp288_cells[] = {
 		.resources = axp288_adc_resources,
 	},
 	{
+		.name = "axp288_extcon",
+		.num_resources = ARRAY_SIZE(axp288_extcon_resources),
+		.resources = axp288_extcon_resources,
+	},
+	{
 		.name = "axp288_charger",
 		.num_resources = ARRAY_SIZE(axp288_charger_resources),
 		.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/da9052-irq.c b/drivers/mfd/da9052-irq.c
index 57ae7841f536..e65ca194fa98 100644
--- a/drivers/mfd/da9052-irq.c
+++ b/drivers/mfd/da9052-irq.c
@@ -262,6 +262,8 @@ int da9052_irq_init(struct da9052 *da9052)
 		goto regmap_err;
 	}
 
+	enable_irq_wake(da9052->chip_irq);
+
 	ret = da9052_request_irq(da9052, DA9052_IRQ_ADC_EOM, "adc-irq",
 			    da9052_auxadc_irq, da9052);
 
diff --git a/drivers/mfd/da9052-spi.c b/drivers/mfd/da9052-spi.c
index 45ae0b7d13ef..b5de8a6856c0 100644
--- a/drivers/mfd/da9052-spi.c
+++ b/drivers/mfd/da9052-spi.c
@@ -32,7 +32,7 @@ static int da9052_spi_probe(struct spi_device *spi)
 	if (!da9052)
 		return -ENOMEM;
 
-	spi->mode = SPI_MODE_0 | SPI_CPOL;
+	spi->mode = SPI_MODE_0;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
@@ -43,6 +43,10 @@ static int da9052_spi_probe(struct spi_device *spi)
 
 	config = da9052_regmap_config;
 	config.read_flag_mask = 1;
+	config.reg_bits = 7;
+	config.pad_bits = 1;
+	config.val_bits = 8;
+	config.use_single_rw = 1;
 
 	da9052->regmap = devm_regmap_init_spi(spi, &config);
 	if (IS_ERR(da9052->regmap)) {
diff --git a/drivers/mfd/da9150-core.c b/drivers/mfd/da9150-core.c
index 4d757b97ef9a..5549817df32e 100644
--- a/drivers/mfd/da9150-core.c
+++ b/drivers/mfd/da9150-core.c
@@ -95,7 +95,7 @@ static const struct regmap_range_cfg da9150_range_cfg[] = {
 	},
 };
 
-static struct regmap_config da9150_regmap_config = {
+static const struct regmap_config da9150_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 	.ranges = da9150_range_cfg,
diff --git a/drivers/mfd/dln2.c b/drivers/mfd/dln2.c
index 1be9bd1c046d..704e189ca162 100644
--- a/drivers/mfd/dln2.c
+++ b/drivers/mfd/dln2.c
@@ -435,7 +435,7 @@ static int _dln2_transfer(struct dln2_dev *dln2, u16 handle, u16 cmd,
 	struct dln2_response *rsp;
 	struct dln2_rx_context *rxc;
 	struct device *dev = &dln2->interface->dev;
-	const unsigned long timeout = DLN2_USB_TIMEOUT * HZ / 1000;
+	const unsigned long timeout = msecs_to_jiffies(DLN2_USB_TIMEOUT);
 	struct dln2_mod_rx_slots *rxs = &dln2->mod_rx_slots[handle];
 	int size;
 
diff --git a/drivers/mfd/hi6421-pmic-core.c b/drivers/mfd/hi6421-pmic-core.c
index 7210ae28bf81..95b2ff8f223a 100644
--- a/drivers/mfd/hi6421-pmic-core.c
+++ b/drivers/mfd/hi6421-pmic-core.c
@@ -93,7 +93,7 @@ static int hi6421_pmic_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id of_hi6421_pmic_match_tbl[] = {
+static const struct of_device_id of_hi6421_pmic_match_tbl[] = {
 	{ .compatible = "hisilicon,hi6421-pmic", },
 	{ },
 };
diff --git a/drivers/mfd/intel_quark_i2c_gpio.c b/drivers/mfd/intel_quark_i2c_gpio.c
new file mode 100644
index 000000000000..1ce16037d043
--- /dev/null
+++ b/drivers/mfd/intel_quark_i2c_gpio.c
@@ -0,0 +1,282 @@
+/*
+ * Intel Quark MFD PCI driver for I2C & GPIO
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * Intel Quark PCI device for I2C and GPIO controller sharing the same
+ * PCI function. This PCI driver will split the 2 devices into their
+ * respective drivers.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/mfd/core.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/dmi.h>
+#include <linux/platform_data/gpio-dwapb.h>
+#include <linux/platform_data/i2c-designware.h>
+
+/* PCI BAR for register base address */
+#define MFD_I2C_BAR		0
+#define MFD_GPIO_BAR		1
+
+/* The base GPIO number under GPIOLIB framework */
+#define INTEL_QUARK_MFD_GPIO_BASE	8
+
+/* The default number of South-Cluster GPIO on Quark. */
+#define INTEL_QUARK_MFD_NGPIO		8
+
+/* The DesignWare GPIO ports on Quark. */
+#define INTEL_QUARK_GPIO_NPORTS	1
+
+#define INTEL_QUARK_IORES_MEM	0
+#define INTEL_QUARK_IORES_IRQ	1
+
+#define INTEL_QUARK_I2C_CONTROLLER_CLK "i2c_designware.0"
+
+/* The Quark I2C controller source clock */
+#define INTEL_QUARK_I2C_CLK_HZ	33000000
+
+#define INTEL_QUARK_I2C_NCLK	1
+
+struct intel_quark_mfd {
+	struct pci_dev		*pdev;
+	struct clk		*i2c_clk;
+	struct clk_lookup	*i2c_clk_lookup;
+};
+
+struct i2c_mode_info {
+	const char *name;
+	unsigned int i2c_scl_freq;
+};
+
+static const struct i2c_mode_info platform_i2c_mode_info[] = {
+	{
+		.name = "Galileo",
+		.i2c_scl_freq = 100000,
+	},
+	{
+		.name = "GalileoGen2",
+		.i2c_scl_freq = 400000,
+	},
+	{}
+};
+
+static struct resource intel_quark_i2c_res[] = {
+	[INTEL_QUARK_IORES_MEM] = {
+		.flags = IORESOURCE_MEM,
+	},
+	[INTEL_QUARK_IORES_IRQ] = {
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct resource intel_quark_gpio_res[] = {
+	[INTEL_QUARK_IORES_MEM] = {
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct mfd_cell intel_quark_mfd_cells[] = {
+	{
+		.id = MFD_I2C_BAR,
+		.name = "i2c_designware",
+		.num_resources = ARRAY_SIZE(intel_quark_i2c_res),
+		.resources = intel_quark_i2c_res,
+		.ignore_resource_conflicts = true,
+	},
+	{
+		.id = MFD_GPIO_BAR,
+		.name = "gpio-dwapb",
+		.num_resources = ARRAY_SIZE(intel_quark_gpio_res),
+		.resources = intel_quark_gpio_res,
+		.ignore_resource_conflicts = true,
+	},
+};
+
+static const struct pci_device_id intel_quark_mfd_ids[] = {
+	{ PCI_VDEVICE(INTEL, 0x0934), },
+	{},
+};
+MODULE_DEVICE_TABLE(pci, intel_quark_mfd_ids);
+
+static int intel_quark_register_i2c_clk(struct intel_quark_mfd *quark_mfd)
+{
+	struct pci_dev *pdev = quark_mfd->pdev;
+	struct clk_lookup *i2c_clk_lookup;
+	struct clk *i2c_clk;
+	int ret;
+
+	i2c_clk_lookup = devm_kcalloc(&pdev->dev, INTEL_QUARK_I2C_NCLK,
+				      sizeof(*i2c_clk_lookup), GFP_KERNEL);
+	if (!i2c_clk_lookup)
+		return -ENOMEM;
+
+	i2c_clk_lookup[0].dev_id = INTEL_QUARK_I2C_CONTROLLER_CLK;
+
+	i2c_clk = clk_register_fixed_rate(&pdev->dev,
+					  INTEL_QUARK_I2C_CONTROLLER_CLK, NULL,
+					  CLK_IS_ROOT, INTEL_QUARK_I2C_CLK_HZ);
+
+	quark_mfd->i2c_clk_lookup = i2c_clk_lookup;
+	quark_mfd->i2c_clk = i2c_clk;
+
+	ret = clk_register_clkdevs(i2c_clk, i2c_clk_lookup,
+				   INTEL_QUARK_I2C_NCLK);
+	if (ret)
+		dev_err(&pdev->dev, "Fixed clk register failed: %d\n", ret);
+
+	return ret;
+}
+
+static void intel_quark_unregister_i2c_clk(struct pci_dev *pdev)
+{
+	struct intel_quark_mfd *quark_mfd = dev_get_drvdata(&pdev->dev);
+
+	if (!quark_mfd->i2c_clk || !quark_mfd->i2c_clk_lookup)
+		return;
+
+	clkdev_drop(quark_mfd->i2c_clk_lookup);
+	clk_unregister(quark_mfd->i2c_clk);
+}
+
+static int intel_quark_i2c_setup(struct pci_dev *pdev, struct mfd_cell *cell)
+{
+	const char *board_name = dmi_get_system_info(DMI_BOARD_NAME);
+	const struct i2c_mode_info *info;
+	struct dw_i2c_platform_data *pdata;
+	struct resource *res = (struct resource *)cell->resources;
+	struct device *dev = &pdev->dev;
+
+	res[INTEL_QUARK_IORES_MEM].start =
+		pci_resource_start(pdev, MFD_I2C_BAR);
+	res[INTEL_QUARK_IORES_MEM].end =
+		pci_resource_end(pdev, MFD_I2C_BAR);
+
+	res[INTEL_QUARK_IORES_IRQ].start = pdev->irq;
+	res[INTEL_QUARK_IORES_IRQ].end = pdev->irq;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	/* Normal mode by default */
+	pdata->i2c_scl_freq = 100000;
+
+	if (board_name) {
+		for (info = platform_i2c_mode_info; info->name; info++) {
+			if (!strcmp(board_name, info->name)) {
+				pdata->i2c_scl_freq = info->i2c_scl_freq;
+				break;
+			}
+		}
+	}
+
+	cell->platform_data = pdata;
+	cell->pdata_size = sizeof(*pdata);
+
+	return 0;
+}
+
+static int intel_quark_gpio_setup(struct pci_dev *pdev, struct mfd_cell *cell)
+{
+	struct dwapb_platform_data *pdata;
+	struct resource *res = (struct resource *)cell->resources;
+	struct device *dev = &pdev->dev;
+
+	res[INTEL_QUARK_IORES_MEM].start =
+		pci_resource_start(pdev, MFD_GPIO_BAR);
+	res[INTEL_QUARK_IORES_MEM].end =
+		pci_resource_end(pdev, MFD_GPIO_BAR);
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	/* For intel quark x1000, it has only one port: portA */
+	pdata->nports = INTEL_QUARK_GPIO_NPORTS;
+	pdata->properties = devm_kcalloc(dev, pdata->nports,
+					 sizeof(*pdata->properties),
+					 GFP_KERNEL);
+	if (!pdata->properties)
+		return -ENOMEM;
+
+	/* Set the properties for portA */
+	pdata->properties->node		= NULL;
+	pdata->properties->name		= "intel-quark-x1000-gpio-portA";
+	pdata->properties->idx		= 0;
+	pdata->properties->ngpio	= INTEL_QUARK_MFD_NGPIO;
+	pdata->properties->gpio_base	= INTEL_QUARK_MFD_GPIO_BASE;
+	pdata->properties->irq		= pdev->irq;
+	pdata->properties->irq_shared	= true;
+
+	cell->platform_data = pdata;
+	cell->pdata_size = sizeof(*pdata);
+
+	return 0;
+}
+
+static int intel_quark_mfd_probe(struct pci_dev *pdev,
+				 const struct pci_device_id *id)
+{
+	struct intel_quark_mfd *quark_mfd;
+	int ret;
+
+	ret = pcim_enable_device(pdev);
+	if (ret)
+		return ret;
+
+	quark_mfd = devm_kzalloc(&pdev->dev, sizeof(*quark_mfd), GFP_KERNEL);
+	if (!quark_mfd)
+		return -ENOMEM;
+	quark_mfd->pdev = pdev;
+
+	ret = intel_quark_register_i2c_clk(quark_mfd);
+	if (ret)
+		return ret;
+
+	dev_set_drvdata(&pdev->dev, quark_mfd);
+
+	ret = intel_quark_i2c_setup(pdev, &intel_quark_mfd_cells[MFD_I2C_BAR]);
+	if (ret)
+		return ret;
+
+	ret = intel_quark_gpio_setup(pdev,
+				     &intel_quark_mfd_cells[MFD_GPIO_BAR]);
+	if (ret)
+		return ret;
+
+	return mfd_add_devices(&pdev->dev, 0, intel_quark_mfd_cells,
+			       ARRAY_SIZE(intel_quark_mfd_cells), NULL, 0,
+			       NULL);
+}
+
+static void intel_quark_mfd_remove(struct pci_dev *pdev)
+{
+	intel_quark_unregister_i2c_clk(pdev);
+	mfd_remove_devices(&pdev->dev);
+}
+
+static struct pci_driver intel_quark_mfd_driver = {
+	.name		= "intel_quark_mfd_i2c_gpio",
+	.id_table	= intel_quark_mfd_ids,
+	.probe		= intel_quark_mfd_probe,
+	.remove		= intel_quark_mfd_remove,
+};
+
+module_pci_driver(intel_quark_mfd_driver);
+
+MODULE_AUTHOR("Raymond Tan <raymond.tan@intel.com>");
+MODULE_DESCRIPTION("Intel Quark MFD PCI driver for I2C & GPIO");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/intel_soc_pmic_core.c b/drivers/mfd/intel_soc_pmic_core.c
index 80cef048b904..7b50b6b208a5 100644
--- a/drivers/mfd/intel_soc_pmic_core.c
+++ b/drivers/mfd/intel_soc_pmic_core.c
@@ -26,19 +26,14 @@
 #include <linux/mfd/intel_soc_pmic.h>
 #include "intel_soc_pmic_core.h"
 
-/*
- * On some boards the PMIC interrupt may come from a GPIO line.
- * Try to lookup the ACPI table and see if such connection exists. If not,
- * return -ENOENT and use the IRQ provided by I2C.
- */
 static int intel_soc_pmic_find_gpio_irq(struct device *dev)
 {
 	struct gpio_desc *desc;
 	int irq;
 
-	desc = devm_gpiod_get_index(dev, "intel_soc_pmic", 0);
+	desc = devm_gpiod_get_index(dev, "intel_soc_pmic", 0, GPIOD_IN);
 	if (IS_ERR(desc))
-		return -ENOENT;
+		return PTR_ERR(desc);
 
 	irq = gpiod_to_irq(desc);
 	if (irq < 0)
@@ -71,6 +66,11 @@ static int intel_soc_pmic_i2c_probe(struct i2c_client *i2c,
 
 	pmic->regmap = devm_regmap_init_i2c(i2c, config->regmap_config);
 
+	/*
+	 * On some boards the PMIC interrupt may come from a GPIO line. Try to
+	 * lookup the ACPI table for a such connection and setup a GPIO
+	 * interrupt if it exists. Otherwise use the IRQ provided by I2C
+	 */
 	irq = intel_soc_pmic_find_gpio_irq(dev);
 	pmic->irq = (irq < 0) ? i2c->irq : irq;
 
diff --git a/drivers/mfd/kempld-core.c b/drivers/mfd/kempld-core.c
index f38ec424872e..8057849d51ac 100644
--- a/drivers/mfd/kempld-core.c
+++ b/drivers/mfd/kempld-core.c
@@ -508,8 +508,15 @@ static struct dmi_system_id kempld_dmi_table[] __initdata = {
 		},
 		.driver_data = (void *)&kempld_platform_data_generic,
 		.callback = kempld_create_platform_device,
-	},
-	{
+	}, {
+		.ident = "CBL6",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "Kontron"),
+			DMI_MATCH(DMI_BOARD_NAME, "COMe-cBL6"),
+		},
+		.driver_data = (void *)&kempld_platform_data_generic,
+		.callback = kempld_create_platform_device,
+	}, {
 		.ident = "CCR2",
 		.matches = {
 			DMI_MATCH(DMI_BOARD_VENDOR, "Kontron"),
@@ -739,7 +746,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/lpc_ich.c b/drivers/mfd/lpc_ich.c
index f35d4280b2f7..12d960a60ec4 100644
--- a/drivers/mfd/lpc_ich.c
+++ b/drivers/mfd/lpc_ich.c
@@ -539,72 +539,7 @@ static struct lpc_ich_info lpc_chipset_info[] = {
  * functions that probably will be registered by other drivers.
  */
 static const struct pci_device_id lpc_ich_ids[] = {
-	{ PCI_VDEVICE(INTEL, 0x2410), LPC_ICH},
-	{ PCI_VDEVICE(INTEL, 0x2420), LPC_ICH0},
-	{ PCI_VDEVICE(INTEL, 0x2440), LPC_ICH2},
-	{ PCI_VDEVICE(INTEL, 0x244c), LPC_ICH2M},
-	{ PCI_VDEVICE(INTEL, 0x2480), LPC_ICH3},
-	{ PCI_VDEVICE(INTEL, 0x248c), LPC_ICH3M},
-	{ PCI_VDEVICE(INTEL, 0x24c0), LPC_ICH4},
-	{ PCI_VDEVICE(INTEL, 0x24cc), LPC_ICH4M},
-	{ PCI_VDEVICE(INTEL, 0x2450), LPC_CICH},
-	{ PCI_VDEVICE(INTEL, 0x24d0), LPC_ICH5},
-	{ PCI_VDEVICE(INTEL, 0x25a1), LPC_6300ESB},
-	{ PCI_VDEVICE(INTEL, 0x2640), LPC_ICH6},
-	{ PCI_VDEVICE(INTEL, 0x2641), LPC_ICH6M},
-	{ PCI_VDEVICE(INTEL, 0x2642), LPC_ICH6W},
-	{ PCI_VDEVICE(INTEL, 0x2670), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2671), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2672), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2673), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2674), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2675), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2676), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2677), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2678), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x2679), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267a), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267b), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267c), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267d), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267e), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x267f), LPC_631XESB},
-	{ PCI_VDEVICE(INTEL, 0x27b8), LPC_ICH7},
-	{ PCI_VDEVICE(INTEL, 0x27b0), LPC_ICH7DH},
-	{ PCI_VDEVICE(INTEL, 0x27b9), LPC_ICH7M},
-	{ PCI_VDEVICE(INTEL, 0x27bd), LPC_ICH7MDH},
-	{ PCI_VDEVICE(INTEL, 0x27bc), LPC_NM10},
-	{ PCI_VDEVICE(INTEL, 0x2810), LPC_ICH8},
-	{ PCI_VDEVICE(INTEL, 0x2812), LPC_ICH8DH},
-	{ PCI_VDEVICE(INTEL, 0x2814), LPC_ICH8DO},
-	{ PCI_VDEVICE(INTEL, 0x2815), LPC_ICH8M},
-	{ PCI_VDEVICE(INTEL, 0x2811), LPC_ICH8ME},
-	{ PCI_VDEVICE(INTEL, 0x2918), LPC_ICH9},
-	{ PCI_VDEVICE(INTEL, 0x2916), LPC_ICH9R},
-	{ PCI_VDEVICE(INTEL, 0x2912), LPC_ICH9DH},
-	{ PCI_VDEVICE(INTEL, 0x2914), LPC_ICH9DO},
-	{ PCI_VDEVICE(INTEL, 0x2919), LPC_ICH9M},
-	{ PCI_VDEVICE(INTEL, 0x2917), LPC_ICH9ME},
-	{ PCI_VDEVICE(INTEL, 0x3a18), LPC_ICH10},
-	{ PCI_VDEVICE(INTEL, 0x3a16), LPC_ICH10R},
-	{ PCI_VDEVICE(INTEL, 0x3a1a), LPC_ICH10D},
-	{ PCI_VDEVICE(INTEL, 0x3a14), LPC_ICH10DO},
-	{ PCI_VDEVICE(INTEL, 0x3b00), LPC_PCH},
-	{ PCI_VDEVICE(INTEL, 0x3b01), LPC_PCHM},
-	{ PCI_VDEVICE(INTEL, 0x3b02), LPC_P55},
-	{ PCI_VDEVICE(INTEL, 0x3b03), LPC_PM55},
-	{ PCI_VDEVICE(INTEL, 0x3b06), LPC_H55},
-	{ PCI_VDEVICE(INTEL, 0x3b07), LPC_QM57},
-	{ PCI_VDEVICE(INTEL, 0x3b08), LPC_H57},
-	{ PCI_VDEVICE(INTEL, 0x3b09), LPC_HM55},
-	{ PCI_VDEVICE(INTEL, 0x3b0a), LPC_Q57},
-	{ PCI_VDEVICE(INTEL, 0x3b0b), LPC_HM57},
-	{ PCI_VDEVICE(INTEL, 0x3b0d), LPC_PCHMSFF},
-	{ PCI_VDEVICE(INTEL, 0x3b0f), LPC_QS57},
-	{ PCI_VDEVICE(INTEL, 0x3b12), LPC_3400},
-	{ PCI_VDEVICE(INTEL, 0x3b14), LPC_3420},
-	{ PCI_VDEVICE(INTEL, 0x3b16), LPC_3450},
-	{ PCI_VDEVICE(INTEL, 0x5031), LPC_EP80579},
+	{ PCI_VDEVICE(INTEL, 0x0f1c), LPC_BAYTRAIL},
 	{ PCI_VDEVICE(INTEL, 0x1c41), LPC_CPT},
 	{ PCI_VDEVICE(INTEL, 0x1c42), LPC_CPTD},
 	{ PCI_VDEVICE(INTEL, 0x1c43), LPC_CPTM},
@@ -638,7 +573,6 @@ static const struct pci_device_id lpc_ich_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x1c5f), LPC_CPT},
 	{ PCI_VDEVICE(INTEL, 0x1d40), LPC_PBG},
 	{ PCI_VDEVICE(INTEL, 0x1d41), LPC_PBG},
-	{ PCI_VDEVICE(INTEL, 0x2310), LPC_DH89XXCC},
 	{ PCI_VDEVICE(INTEL, 0x1e40), LPC_PPT},
 	{ PCI_VDEVICE(INTEL, 0x1e41), LPC_PPT},
 	{ PCI_VDEVICE(INTEL, 0x1e42), LPC_PPT},
@@ -671,6 +605,79 @@ static const struct pci_device_id lpc_ich_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x1e5d), LPC_PPT},
 	{ PCI_VDEVICE(INTEL, 0x1e5e), LPC_PPT},
 	{ PCI_VDEVICE(INTEL, 0x1e5f), LPC_PPT},
+	{ PCI_VDEVICE(INTEL, 0x1f38), LPC_AVN},
+	{ PCI_VDEVICE(INTEL, 0x1f39), LPC_AVN},
+	{ PCI_VDEVICE(INTEL, 0x1f3a), LPC_AVN},
+	{ PCI_VDEVICE(INTEL, 0x1f3b), LPC_AVN},
+	{ PCI_VDEVICE(INTEL, 0x229c), LPC_BRASWELL},
+	{ PCI_VDEVICE(INTEL, 0x2310), LPC_DH89XXCC},
+	{ PCI_VDEVICE(INTEL, 0x2390), LPC_COLETO},
+	{ PCI_VDEVICE(INTEL, 0x2410), LPC_ICH},
+	{ PCI_VDEVICE(INTEL, 0x2420), LPC_ICH0},
+	{ PCI_VDEVICE(INTEL, 0x2440), LPC_ICH2},
+	{ PCI_VDEVICE(INTEL, 0x244c), LPC_ICH2M},
+	{ PCI_VDEVICE(INTEL, 0x2450), LPC_CICH},
+	{ PCI_VDEVICE(INTEL, 0x2480), LPC_ICH3},
+	{ PCI_VDEVICE(INTEL, 0x248c), LPC_ICH3M},
+	{ PCI_VDEVICE(INTEL, 0x24c0), LPC_ICH4},
+	{ PCI_VDEVICE(INTEL, 0x24cc), LPC_ICH4M},
+	{ PCI_VDEVICE(INTEL, 0x24d0), LPC_ICH5},
+	{ PCI_VDEVICE(INTEL, 0x25a1), LPC_6300ESB},
+	{ PCI_VDEVICE(INTEL, 0x2640), LPC_ICH6},
+	{ PCI_VDEVICE(INTEL, 0x2641), LPC_ICH6M},
+	{ PCI_VDEVICE(INTEL, 0x2642), LPC_ICH6W},
+	{ PCI_VDEVICE(INTEL, 0x2670), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2671), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2672), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2673), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2674), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2675), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2676), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2677), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2678), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x2679), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267a), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267b), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267c), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267d), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267e), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x267f), LPC_631XESB},
+	{ PCI_VDEVICE(INTEL, 0x27b0), LPC_ICH7DH},
+	{ PCI_VDEVICE(INTEL, 0x27b8), LPC_ICH7},
+	{ PCI_VDEVICE(INTEL, 0x27b9), LPC_ICH7M},
+	{ PCI_VDEVICE(INTEL, 0x27bc), LPC_NM10},
+	{ PCI_VDEVICE(INTEL, 0x27bd), LPC_ICH7MDH},
+	{ PCI_VDEVICE(INTEL, 0x2810), LPC_ICH8},
+	{ PCI_VDEVICE(INTEL, 0x2811), LPC_ICH8ME},
+	{ PCI_VDEVICE(INTEL, 0x2812), LPC_ICH8DH},
+	{ PCI_VDEVICE(INTEL, 0x2814), LPC_ICH8DO},
+	{ PCI_VDEVICE(INTEL, 0x2815), LPC_ICH8M},
+	{ PCI_VDEVICE(INTEL, 0x2912), LPC_ICH9DH},
+	{ PCI_VDEVICE(INTEL, 0x2914), LPC_ICH9DO},
+	{ PCI_VDEVICE(INTEL, 0x2916), LPC_ICH9R},
+	{ PCI_VDEVICE(INTEL, 0x2917), LPC_ICH9ME},
+	{ PCI_VDEVICE(INTEL, 0x2918), LPC_ICH9},
+	{ PCI_VDEVICE(INTEL, 0x2919), LPC_ICH9M},
+	{ PCI_VDEVICE(INTEL, 0x3a14), LPC_ICH10DO},
+	{ PCI_VDEVICE(INTEL, 0x3a16), LPC_ICH10R},
+	{ PCI_VDEVICE(INTEL, 0x3a18), LPC_ICH10},
+	{ PCI_VDEVICE(INTEL, 0x3a1a), LPC_ICH10D},
+	{ PCI_VDEVICE(INTEL, 0x3b00), LPC_PCH},
+	{ PCI_VDEVICE(INTEL, 0x3b01), LPC_PCHM},
+	{ PCI_VDEVICE(INTEL, 0x3b02), LPC_P55},
+	{ PCI_VDEVICE(INTEL, 0x3b03), LPC_PM55},
+	{ PCI_VDEVICE(INTEL, 0x3b06), LPC_H55},
+	{ PCI_VDEVICE(INTEL, 0x3b07), LPC_QM57},
+	{ PCI_VDEVICE(INTEL, 0x3b08), LPC_H57},
+	{ PCI_VDEVICE(INTEL, 0x3b09), LPC_HM55},
+	{ PCI_VDEVICE(INTEL, 0x3b0a), LPC_Q57},
+	{ PCI_VDEVICE(INTEL, 0x3b0b), LPC_HM57},
+	{ PCI_VDEVICE(INTEL, 0x3b0d), LPC_PCHMSFF},
+	{ PCI_VDEVICE(INTEL, 0x3b0f), LPC_QS57},
+	{ PCI_VDEVICE(INTEL, 0x3b12), LPC_3400},
+	{ PCI_VDEVICE(INTEL, 0x3b14), LPC_3420},
+	{ PCI_VDEVICE(INTEL, 0x3b16), LPC_3450},
+	{ PCI_VDEVICE(INTEL, 0x5031), LPC_EP80579},
 	{ PCI_VDEVICE(INTEL, 0x8c40), LPC_LPT},
 	{ PCI_VDEVICE(INTEL, 0x8c41), LPC_LPT},
 	{ PCI_VDEVICE(INTEL, 0x8c42), LPC_LPT},
@@ -703,14 +710,11 @@ static const struct pci_device_id lpc_ich_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x8c5d), LPC_LPT},
 	{ PCI_VDEVICE(INTEL, 0x8c5e), LPC_LPT},
 	{ PCI_VDEVICE(INTEL, 0x8c5f), LPC_LPT},
-	{ PCI_VDEVICE(INTEL, 0x9c40), LPC_LPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x9c41), LPC_LPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x9c42), LPC_LPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x9c43), LPC_LPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x9c44), LPC_LPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x9c45), LPC_LPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x9c46), LPC_LPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x9c47), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x8cc1), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc2), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc3), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc4), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc6), LPC_9S},
 	{ PCI_VDEVICE(INTEL, 0x8d40), LPC_WBG},
 	{ PCI_VDEVICE(INTEL, 0x8d41), LPC_WBG},
 	{ PCI_VDEVICE(INTEL, 0x8d42), LPC_WBG},
@@ -743,12 +747,14 @@ static const struct pci_device_id lpc_ich_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x8d5d), LPC_WBG},
 	{ PCI_VDEVICE(INTEL, 0x8d5e), LPC_WBG},
 	{ PCI_VDEVICE(INTEL, 0x8d5f), LPC_WBG},
-	{ PCI_VDEVICE(INTEL, 0x1f38), LPC_AVN},
-	{ PCI_VDEVICE(INTEL, 0x1f39), LPC_AVN},
-	{ PCI_VDEVICE(INTEL, 0x1f3a), LPC_AVN},
-	{ PCI_VDEVICE(INTEL, 0x1f3b), LPC_AVN},
-	{ PCI_VDEVICE(INTEL, 0x0f1c), LPC_BAYTRAIL},
-	{ PCI_VDEVICE(INTEL, 0x2390), LPC_COLETO},
+	{ PCI_VDEVICE(INTEL, 0x9c40), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x9c41), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x9c42), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x9c43), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x9c44), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x9c45), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x9c46), LPC_LPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x9c47), LPC_LPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc1), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc2), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc3), LPC_WPT_LP},
@@ -756,12 +762,6 @@ static const struct pci_device_id lpc_ich_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x9cc6), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc7), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc9), LPC_WPT_LP},
-	{ PCI_VDEVICE(INTEL, 0x229c), LPC_BRASWELL},
-	{ PCI_VDEVICE(INTEL, 0x8cc1), LPC_9S},
-	{ PCI_VDEVICE(INTEL, 0x8cc2), LPC_9S},
-	{ PCI_VDEVICE(INTEL, 0x8cc3), LPC_9S},
-	{ PCI_VDEVICE(INTEL, 0x8cc4), LPC_9S},
-	{ PCI_VDEVICE(INTEL, 0x8cc6), LPC_9S},
 	{ 0, },			/* End of list */
 };
 MODULE_DEVICE_TABLE(pci, lpc_ich_ids);
diff --git a/drivers/mfd/max77693.c b/drivers/mfd/max77693.c
index a159593e27a0..cb14afa97e6f 100644
--- a/drivers/mfd/max77693.c
+++ b/drivers/mfd/max77693.c
@@ -53,8 +53,8 @@ static const struct mfd_cell max77693_devs[] = {
 		.of_compatible = "maxim,max77693-haptic",
 	},
 	{
-		.name = "max77693-flash",
-		.of_compatible = "maxim,max77693-flash",
+		.name = "max77693-led",
+		.of_compatible = "maxim,max77693-led",
 	},
 };
 
diff --git a/drivers/mfd/max77843.c b/drivers/mfd/max77843.c
new file mode 100644
index 000000000000..a354ac677ec7
--- /dev/null
+++ b/drivers/mfd/max77843.c
@@ -0,0 +1,243 @@
+/*
+ * MFD core driver for the Maxim MAX77843
+ *
+ * Copyright (C) 2015 Samsung Electronics
+ * Author: Jaewon Kim <jaewon02.kim@samsung.com>
+ * Author: Beomho Seo <beomho.seo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max77843-private.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+
+static const struct mfd_cell max77843_devs[] = {
+	{
+		.name = "max77843-muic",
+		.of_compatible = "maxim,max77843-muic",
+	}, {
+		.name = "max77843-regulator",
+		.of_compatible = "maxim,max77843-regulator",
+	}, {
+		.name = "max77843-charger",
+		.of_compatible = "maxim,max77843-charger"
+	}, {
+		.name = "max77843-fuelgauge",
+		.of_compatible = "maxim,max77843-fuelgauge",
+	}, {
+		.name = "max77843-haptic",
+		.of_compatible = "maxim,max77843-haptic",
+	},
+};
+
+static const struct regmap_config max77843_charger_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= MAX77843_CHG_REG_END,
+};
+
+static const struct regmap_config max77843_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= MAX77843_SYS_REG_END,
+};
+
+static const struct regmap_irq max77843_irqs[] = {
+	/* TOPSYS interrupts */
+	{ .reg_offset = 0, .mask = MAX77843_SYS_IRQ_SYSUVLO_INT, },
+	{ .reg_offset = 0, .mask = MAX77843_SYS_IRQ_SYSOVLO_INT, },
+	{ .reg_offset = 0, .mask = MAX77843_SYS_IRQ_TSHDN_INT, },
+	{ .reg_offset = 0, .mask = MAX77843_SYS_IRQ_TM_INT, },
+};
+
+static const struct regmap_irq_chip max77843_irq_chip = {
+	.name		= "max77843",
+	.status_base	= MAX77843_SYS_REG_SYSINTSRC,
+	.mask_base	= MAX77843_SYS_REG_SYSINTMASK,
+	.mask_invert	= false,
+	.num_regs	= 1,
+	.irqs		= max77843_irqs,
+	.num_irqs	= ARRAY_SIZE(max77843_irqs),
+};
+
+/* Charger and Charger regulator use same regmap. */
+static int max77843_chg_init(struct max77843 *max77843)
+{
+	int ret;
+
+	max77843->i2c_chg = i2c_new_dummy(max77843->i2c->adapter, I2C_ADDR_CHG);
+	if (!max77843->i2c_chg) {
+		dev_err(&max77843->i2c->dev,
+				"Cannot allocate I2C device for Charger\n");
+		return PTR_ERR(max77843->i2c_chg);
+	}
+	i2c_set_clientdata(max77843->i2c_chg, max77843);
+
+	max77843->regmap_chg = devm_regmap_init_i2c(max77843->i2c_chg,
+			&max77843_charger_regmap_config);
+	if (IS_ERR(max77843->regmap_chg)) {
+		ret = PTR_ERR(max77843->regmap_chg);
+		goto err_chg_i2c;
+	}
+
+	return 0;
+
+err_chg_i2c:
+	i2c_unregister_device(max77843->i2c_chg);
+
+	return ret;
+}
+
+static int max77843_probe(struct i2c_client *i2c,
+			  const struct i2c_device_id *id)
+{
+	struct max77843 *max77843;
+	unsigned int reg_data;
+	int ret;
+
+	max77843 = devm_kzalloc(&i2c->dev, sizeof(*max77843), GFP_KERNEL);
+	if (!max77843)
+		return -ENOMEM;
+
+	i2c_set_clientdata(i2c, max77843);
+	max77843->dev = &i2c->dev;
+	max77843->i2c = i2c;
+	max77843->irq = i2c->irq;
+
+	max77843->regmap = devm_regmap_init_i2c(i2c,
+			&max77843_regmap_config);
+	if (IS_ERR(max77843->regmap)) {
+		dev_err(&i2c->dev, "Failed to allocate topsys register map\n");
+		return PTR_ERR(max77843->regmap);
+	}
+
+	ret = regmap_add_irq_chip(max77843->regmap, max77843->irq,
+			IRQF_TRIGGER_LOW | IRQF_ONESHOT | IRQF_SHARED,
+			0, &max77843_irq_chip, &max77843->irq_data);
+	if (ret) {
+		dev_err(&i2c->dev, "Failed to add TOPSYS IRQ chip\n");
+		return ret;
+	}
+
+	ret = regmap_read(max77843->regmap,
+			MAX77843_SYS_REG_PMICID, &reg_data);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "Failed to read PMIC ID\n");
+		goto err_pmic_id;
+	}
+	dev_info(&i2c->dev, "device ID: 0x%x\n", reg_data);
+
+	ret = max77843_chg_init(max77843);
+	if (ret) {
+		dev_err(&i2c->dev, "Failed to init Charger\n");
+		goto err_pmic_id;
+	}
+
+	ret = regmap_update_bits(max77843->regmap,
+				 MAX77843_SYS_REG_INTSRCMASK,
+				 MAX77843_INTSRC_MASK_MASK,
+				 (unsigned int)~MAX77843_INTSRC_MASK_MASK);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "Failed to unmask interrupt source\n");
+		goto err_pmic_id;
+	}
+
+	ret = mfd_add_devices(max77843->dev, -1, max77843_devs,
+			      ARRAY_SIZE(max77843_devs), NULL, 0, NULL);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "Failed to add mfd device\n");
+		goto err_pmic_id;
+	}
+
+	device_init_wakeup(max77843->dev, true);
+
+	return 0;
+
+err_pmic_id:
+	regmap_del_irq_chip(max77843->irq, max77843->irq_data);
+
+	return ret;
+}
+
+static int max77843_remove(struct i2c_client *i2c)
+{
+	struct max77843 *max77843 = i2c_get_clientdata(i2c);
+
+	mfd_remove_devices(max77843->dev);
+
+	regmap_del_irq_chip(max77843->irq, max77843->irq_data);
+
+	i2c_unregister_device(max77843->i2c_chg);
+
+	return 0;
+}
+
+static const struct of_device_id max77843_dt_match[] = {
+	{ .compatible = "maxim,max77843", },
+	{ },
+};
+
+static const struct i2c_device_id max77843_id[] = {
+	{ "max77843", },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, max77843_id);
+
+static int __maybe_unused max77843_suspend(struct device *dev)
+{
+	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct max77843 *max77843 = i2c_get_clientdata(i2c);
+
+	disable_irq(max77843->irq);
+	if (device_may_wakeup(dev))
+		enable_irq_wake(max77843->irq);
+
+	return 0;
+}
+
+static int __maybe_unused max77843_resume(struct device *dev)
+{
+	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct max77843 *max77843 = i2c_get_clientdata(i2c);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(max77843->irq);
+	enable_irq(max77843->irq);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(max77843_pm, max77843_suspend, max77843_resume);
+
+static struct i2c_driver max77843_i2c_driver = {
+	.driver	= {
+		.name = "max77843",
+		.pm = &max77843_pm,
+		.of_match_table = max77843_dt_match,
+	},
+	.probe = max77843_probe,
+	.remove = max77843_remove,
+	.id_table = max77843_id,
+};
+
+static int __init max77843_i2c_init(void)
+{
+	return i2c_add_driver(&max77843_i2c_driver);
+}
+subsys_initcall(max77843_i2c_init);
+
+static void __exit max77843_i2c_exit(void)
+{
+	i2c_del_driver(&max77843_i2c_driver);
+}
+module_exit(max77843_i2c_exit);
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index 64dde5d24b32..25fd7116493a 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -51,19 +51,19 @@
 void mc13xxx_lock(struct mc13xxx *mc13xxx)
 {
 	if (!mutex_trylock(&mc13xxx->lock)) {
-		dev_dbg(mc13xxx->dev, "wait for %s from %pf\n",
+		dev_dbg(mc13xxx->dev, "wait for %s from %ps\n",
 				__func__, __builtin_return_address(0));
 
 		mutex_lock(&mc13xxx->lock);
 	}
-	dev_dbg(mc13xxx->dev, "%s from %pf\n",
+	dev_dbg(mc13xxx->dev, "%s from %ps\n",
 			__func__, __builtin_return_address(0));
 }
 EXPORT_SYMBOL(mc13xxx_lock);
 
 void mc13xxx_unlock(struct mc13xxx *mc13xxx)
 {
-	dev_dbg(mc13xxx->dev, "%s from %pf\n",
+	dev_dbg(mc13xxx->dev, "%s from %ps\n",
 			__func__, __builtin_return_address(0));
 	mutex_unlock(&mc13xxx->lock);
 }
diff --git a/drivers/mfd/menelaus.c b/drivers/mfd/menelaus.c
index 9f01aef539dd..3ac36f5ccd3e 100644
--- a/drivers/mfd/menelaus.c
+++ b/drivers/mfd/menelaus.c
@@ -532,29 +532,6 @@ static const struct menelaus_vtg_value vcore_values[] = {
 	{ 1450, 18 },
 };
 
-int menelaus_set_vcore_sw(unsigned int mV)
-{
-	int val, ret;
-	struct i2c_client *c = the_menelaus->client;
-
-	val = menelaus_get_vtg_value(mV, vcore_values,
-				     ARRAY_SIZE(vcore_values));
-	if (val < 0)
-		return -EINVAL;
-
-	dev_dbg(&c->dev, "Setting VCORE to %d mV (val 0x%02x)\n", mV, val);
-
-	/* Set SW mode and the voltage in one go. */
-	mutex_lock(&the_menelaus->lock);
-	ret = menelaus_write_reg(MENELAUS_VCORE_CTRL1, val);
-	if (ret == 0)
-		the_menelaus->vcore_hw_mode = 0;
-	mutex_unlock(&the_menelaus->lock);
-	msleep(1);
-
-	return ret;
-}
-
 int menelaus_set_vcore_hw(unsigned int roof_mV, unsigned int floor_mV)
 {
 	int fval, rval, val, ret;
@@ -1239,7 +1216,7 @@ static int menelaus_probe(struct i2c_client *client,
 	err = menelaus_read_reg(MENELAUS_VCORE_CTRL1);
 	if (err < 0)
 		goto fail;
-	if (err & BIT(7))
+	if (err & VCORE_CTRL1_HW_NSW)
 		menelaus->vcore_hw_mode = 1;
 	else
 		menelaus->vcore_hw_mode = 0;
@@ -1259,7 +1236,7 @@ fail:
 	return err;
 }
 
-static int __exit menelaus_remove(struct i2c_client *client)
+static int menelaus_remove(struct i2c_client *client)
 {
 	struct menelaus_chip	*menelaus = i2c_get_clientdata(client);
 
@@ -1280,7 +1257,7 @@ static struct i2c_driver menelaus_i2c_driver = {
 		.name		= DRIVER_NAME,
 	},
 	.probe		= menelaus_probe,
-	.remove		= __exit_p(menelaus_remove),
+	.remove		= menelaus_remove,
 	.id_table	= menelaus_id,
 };
 
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index 2a87f69be53d..1aed3b7b8d9b 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -128,7 +128,7 @@ static int mfd_add_device(struct device *parent, int id,
 	int platform_id;
 	int r;
 
-	if (id < 0)
+	if (id == PLATFORM_DEVID_AUTO)
 		platform_id = id;
 	else
 		platform_id = id + cell->id;
diff --git a/drivers/mfd/mt6397-core.c b/drivers/mfd/mt6397-core.c
new file mode 100644
index 000000000000..09bc7804952a
--- /dev/null
+++ b/drivers/mfd/mt6397-core.c
@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Flora Fu, MediaTek
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+#include <linux/regmap.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/mt6397/core.h>
+#include <linux/mfd/mt6397/registers.h>
+
+static const struct mfd_cell mt6397_devs[] = {
+	{
+		.name = "mt6397-rtc",
+		.of_compatible = "mediatek,mt6397-rtc",
+	}, {
+		.name = "mt6397-regulator",
+		.of_compatible = "mediatek,mt6397-regulator",
+	}, {
+		.name = "mt6397-codec",
+		.of_compatible = "mediatek,mt6397-codec",
+	}, {
+		.name = "mt6397-clk",
+		.of_compatible = "mediatek,mt6397-clk",
+	},
+};
+
+static void mt6397_irq_lock(struct irq_data *data)
+{
+	struct mt6397_chip *mt6397 = irq_get_chip_data(data->irq);
+
+	mutex_lock(&mt6397->irqlock);
+}
+
+static void mt6397_irq_sync_unlock(struct irq_data *data)
+{
+	struct mt6397_chip *mt6397 = irq_get_chip_data(data->irq);
+
+	regmap_write(mt6397->regmap, MT6397_INT_CON0, mt6397->irq_masks_cur[0]);
+	regmap_write(mt6397->regmap, MT6397_INT_CON1, mt6397->irq_masks_cur[1]);
+
+	mutex_unlock(&mt6397->irqlock);
+}
+
+static void mt6397_irq_disable(struct irq_data *data)
+{
+	struct mt6397_chip *mt6397 = irq_get_chip_data(data->irq);
+	int shift = data->hwirq & 0xf;
+	int reg = data->hwirq >> 4;
+
+	mt6397->irq_masks_cur[reg] &= ~BIT(shift);
+}
+
+static void mt6397_irq_enable(struct irq_data *data)
+{
+	struct mt6397_chip *mt6397 = irq_get_chip_data(data->irq);
+	int shift = data->hwirq & 0xf;
+	int reg = data->hwirq >> 4;
+
+	mt6397->irq_masks_cur[reg] |= BIT(shift);
+}
+
+static struct irq_chip mt6397_irq_chip = {
+	.name = "mt6397-irq",
+	.irq_bus_lock = mt6397_irq_lock,
+	.irq_bus_sync_unlock = mt6397_irq_sync_unlock,
+	.irq_enable = mt6397_irq_enable,
+	.irq_disable = mt6397_irq_disable,
+};
+
+static void mt6397_irq_handle_reg(struct mt6397_chip *mt6397, int reg,
+		int irqbase)
+{
+	unsigned int status;
+	int i, irq, ret;
+
+	ret = regmap_read(mt6397->regmap, reg, &status);
+	if (ret) {
+		dev_err(mt6397->dev, "Failed to read irq status: %d\n", ret);
+		return;
+	}
+
+	for (i = 0; i < 16; i++) {
+		if (status & BIT(i)) {
+			irq = irq_find_mapping(mt6397->irq_domain, irqbase + i);
+			if (irq)
+				handle_nested_irq(irq);
+		}
+	}
+
+	regmap_write(mt6397->regmap, reg, status);
+}
+
+static irqreturn_t mt6397_irq_thread(int irq, void *data)
+{
+	struct mt6397_chip *mt6397 = data;
+
+	mt6397_irq_handle_reg(mt6397, MT6397_INT_STATUS0, 0);
+	mt6397_irq_handle_reg(mt6397, MT6397_INT_STATUS1, 16);
+
+	return IRQ_HANDLED;
+}
+
+static int mt6397_irq_domain_map(struct irq_domain *d, unsigned int irq,
+					irq_hw_number_t hw)
+{
+	struct mt6397_chip *mt6397 = d->host_data;
+
+	irq_set_chip_data(irq, mt6397);
+	irq_set_chip_and_handler(irq, &mt6397_irq_chip, handle_level_irq);
+	irq_set_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+	set_irq_flags(irq, IRQF_VALID);
+#else
+	irq_set_noprobe(irq);
+#endif
+
+	return 0;
+}
+
+static struct irq_domain_ops mt6397_irq_domain_ops = {
+	.map = mt6397_irq_domain_map,
+};
+
+static int mt6397_irq_init(struct mt6397_chip *mt6397)
+{
+	int ret;
+
+	mutex_init(&mt6397->irqlock);
+
+	/* Mask all interrupt sources */
+	regmap_write(mt6397->regmap, MT6397_INT_CON0, 0x0);
+	regmap_write(mt6397->regmap, MT6397_INT_CON1, 0x0);
+
+	mt6397->irq_domain = irq_domain_add_linear(mt6397->dev->of_node,
+		MT6397_IRQ_NR, &mt6397_irq_domain_ops, mt6397);
+	if (!mt6397->irq_domain) {
+		dev_err(mt6397->dev, "could not create irq domain\n");
+		return -ENOMEM;
+	}
+
+	ret = devm_request_threaded_irq(mt6397->dev, mt6397->irq, NULL,
+		mt6397_irq_thread, IRQF_ONESHOT, "mt6397-pmic", mt6397);
+	if (ret) {
+		dev_err(mt6397->dev, "failed to register irq=%d; err: %d\n",
+			mt6397->irq, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int mt6397_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct mt6397_chip *mt6397;
+
+	mt6397 = devm_kzalloc(&pdev->dev, sizeof(*mt6397), GFP_KERNEL);
+	if (!mt6397)
+		return -ENOMEM;
+
+	mt6397->dev = &pdev->dev;
+	/*
+	 * mt6397 MFD is child device of soc pmic wrapper.
+	 * Regmap is set from its parent.
+	 */
+	mt6397->regmap = dev_get_regmap(pdev->dev.parent, NULL);
+	if (!mt6397->regmap)
+		return -ENODEV;
+
+	platform_set_drvdata(pdev, mt6397);
+
+	mt6397->irq = platform_get_irq(pdev, 0);
+	if (mt6397->irq > 0) {
+		ret = mt6397_irq_init(mt6397);
+		if (ret)
+			return ret;
+	}
+
+	ret = mfd_add_devices(&pdev->dev, -1, mt6397_devs,
+			ARRAY_SIZE(mt6397_devs), NULL, 0, NULL);
+	if (ret)
+		dev_err(&pdev->dev, "failed to add child devices: %d\n", ret);
+
+	return ret;
+}
+
+static int mt6397_remove(struct platform_device *pdev)
+{
+	mfd_remove_devices(&pdev->dev);
+
+	return 0;
+}
+
+static const struct of_device_id mt6397_of_match[] = {
+	{ .compatible = "mediatek,mt6397" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mt6397_of_match);
+
+static struct platform_driver mt6397_driver = {
+	.probe = mt6397_probe,
+	.remove = mt6397_remove,
+	.driver = {
+		.name = "mt6397",
+		.of_match_table = of_match_ptr(mt6397_of_match),
+	},
+};
+
+module_platform_driver(mt6397_driver);
+
+MODULE_AUTHOR("Flora Fu, MediaTek");
+MODULE_DESCRIPTION("Driver for MediaTek MT6397 PMIC");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:mt6397");
diff --git a/drivers/mfd/qcom-spmi-pmic.c b/drivers/mfd/qcom-spmi-pmic.c
index 4b8beb2a1579..af6ac1c4b45c 100644
--- a/drivers/mfd/qcom-spmi-pmic.c
+++ b/drivers/mfd/qcom-spmi-pmic.c
@@ -17,6 +17,100 @@
 #include <linux/regmap.h>
 #include <linux/of_platform.h>
 
+#define PMIC_REV2		0x101
+#define PMIC_REV3		0x102
+#define PMIC_REV4		0x103
+#define PMIC_TYPE		0x104
+#define PMIC_SUBTYPE		0x105
+
+#define PMIC_TYPE_VALUE		0x51
+
+#define COMMON_SUBTYPE		0x00
+#define PM8941_SUBTYPE		0x01
+#define PM8841_SUBTYPE		0x02
+#define PM8019_SUBTYPE		0x03
+#define PM8226_SUBTYPE		0x04
+#define PM8110_SUBTYPE		0x05
+#define PMA8084_SUBTYPE		0x06
+#define PMI8962_SUBTYPE		0x07
+#define PMD9635_SUBTYPE		0x08
+#define PM8994_SUBTYPE		0x09
+#define PMI8994_SUBTYPE		0x0a
+#define PM8916_SUBTYPE		0x0b
+#define PM8004_SUBTYPE		0x0c
+#define PM8909_SUBTYPE		0x0d
+
+static const struct of_device_id pmic_spmi_id_table[] = {
+	{ .compatible = "qcom,spmi-pmic", .data = (void *)COMMON_SUBTYPE },
+	{ .compatible = "qcom,pm8941",    .data = (void *)PM8941_SUBTYPE },
+	{ .compatible = "qcom,pm8841",    .data = (void *)PM8841_SUBTYPE },
+	{ .compatible = "qcom,pm8019",    .data = (void *)PM8019_SUBTYPE },
+	{ .compatible = "qcom,pm8226",    .data = (void *)PM8226_SUBTYPE },
+	{ .compatible = "qcom,pm8110",    .data = (void *)PM8110_SUBTYPE },
+	{ .compatible = "qcom,pma8084",   .data = (void *)PMA8084_SUBTYPE },
+	{ .compatible = "qcom,pmi8962",   .data = (void *)PMI8962_SUBTYPE },
+	{ .compatible = "qcom,pmd9635",   .data = (void *)PMD9635_SUBTYPE },
+	{ .compatible = "qcom,pm8994",    .data = (void *)PM8994_SUBTYPE },
+	{ .compatible = "qcom,pmi8994",   .data = (void *)PMI8994_SUBTYPE },
+	{ .compatible = "qcom,pm8916",    .data = (void *)PM8916_SUBTYPE },
+	{ .compatible = "qcom,pm8004",    .data = (void *)PM8004_SUBTYPE },
+	{ .compatible = "qcom,pm8909",    .data = (void *)PM8909_SUBTYPE },
+	{ }
+};
+
+static void pmic_spmi_show_revid(struct regmap *map, struct device *dev)
+{
+	unsigned int rev2, minor, major, type, subtype;
+	const char *name = "unknown";
+	int ret, i;
+
+	ret = regmap_read(map, PMIC_TYPE, &type);
+	if (ret < 0)
+		return;
+
+	if (type != PMIC_TYPE_VALUE)
+		return;
+
+	ret = regmap_read(map, PMIC_SUBTYPE, &subtype);
+	if (ret < 0)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(pmic_spmi_id_table); i++) {
+		if (subtype == (unsigned long)pmic_spmi_id_table[i].data)
+			break;
+	}
+
+	if (i != ARRAY_SIZE(pmic_spmi_id_table))
+		name = pmic_spmi_id_table[i].compatible;
+
+	ret = regmap_read(map, PMIC_REV2, &rev2);
+	if (ret < 0)
+		return;
+
+	ret = regmap_read(map, PMIC_REV3, &minor);
+	if (ret < 0)
+		return;
+
+	ret = regmap_read(map, PMIC_REV4, &major);
+	if (ret < 0)
+		return;
+
+	/*
+	 * In early versions of PM8941 and PM8226, the major revision number
+	 * started incrementing from 0 (eg 0 = v1.0, 1 = v2.0).
+	 * Increment the major revision number here if the chip is an early
+	 * version of PM8941 or PM8226.
+	 */
+	if ((subtype == PM8941_SUBTYPE || subtype == PM8226_SUBTYPE) &&
+	    major < 0x02)
+		major++;
+
+	if (subtype == PM8110_SUBTYPE)
+		minor = rev2;
+
+	dev_dbg(dev, "%x: %s v%d.%d\n", subtype, name, major, minor);
+}
+
 static const struct regmap_config spmi_regmap_config = {
 	.reg_bits	= 16,
 	.val_bits	= 8,
@@ -33,6 +127,8 @@ static int pmic_spmi_probe(struct spmi_device *sdev)
 	if (IS_ERR(regmap))
 		return PTR_ERR(regmap);
 
+	pmic_spmi_show_revid(regmap, &sdev->dev);
+
 	return of_platform_populate(root, NULL, NULL, &sdev->dev);
 }
 
@@ -41,13 +137,6 @@ static void pmic_spmi_remove(struct spmi_device *sdev)
 	of_platform_depopulate(&sdev->dev);
 }
 
-static const struct of_device_id pmic_spmi_id_table[] = {
-	{ .compatible = "qcom,spmi-pmic" },
-	{ .compatible = "qcom,pm8941" },
-	{ .compatible = "qcom,pm8841" },
-	{ .compatible = "qcom,pma8084" },
-	{ }
-};
 MODULE_DEVICE_TABLE(of, pmic_spmi_id_table);
 
 static struct spmi_driver pmic_spmi_driver = {
diff --git a/drivers/mfd/qcom_rpm.c b/drivers/mfd/qcom_rpm.c
index f696328c2933..12e324319573 100644
--- a/drivers/mfd/qcom_rpm.c
+++ b/drivers/mfd/qcom_rpm.c
@@ -323,10 +323,51 @@ static const struct qcom_rpm_data msm8960_template = {
 	.n_resources = ARRAY_SIZE(msm8960_rpm_resource_table),
 };
 
+static const struct qcom_rpm_resource ipq806x_rpm_resource_table[] = {
+	[QCOM_RPM_CXO_CLK] =			{ 25, 9, 5, 1 },
+	[QCOM_RPM_PXO_CLK] =			{ 26, 10, 6, 1 },
+	[QCOM_RPM_APPS_FABRIC_CLK] =		{ 27, 11, 8, 1 },
+	[QCOM_RPM_SYS_FABRIC_CLK] =		{ 28, 12, 9, 1 },
+	[QCOM_RPM_NSS_FABRIC_0_CLK] =		{ 29, 13, 10, 1 },
+	[QCOM_RPM_DAYTONA_FABRIC_CLK] =		{ 30, 14, 11, 1 },
+	[QCOM_RPM_SFPB_CLK] =			{ 31, 15, 12, 1 },
+	[QCOM_RPM_CFPB_CLK] =			{ 32, 16, 13, 1 },
+	[QCOM_RPM_NSS_FABRIC_1_CLK] =		{ 33, 17, 14, 1 },
+	[QCOM_RPM_EBI1_CLK] =			{ 34, 18, 16, 1 },
+	[QCOM_RPM_APPS_FABRIC_HALT] =		{ 35, 19, 18, 2 },
+	[QCOM_RPM_APPS_FABRIC_MODE] =		{ 37, 20, 19, 3 },
+	[QCOM_RPM_APPS_FABRIC_IOCTL] =		{ 40, 21, 20, 1 },
+	[QCOM_RPM_APPS_FABRIC_ARB] =		{ 41, 22, 21, 12 },
+	[QCOM_RPM_SYS_FABRIC_HALT] =		{ 53, 23, 22, 2 },
+	[QCOM_RPM_SYS_FABRIC_MODE] =		{ 55, 24, 23, 3 },
+	[QCOM_RPM_SYS_FABRIC_IOCTL] =		{ 58, 25, 24, 1 },
+	[QCOM_RPM_SYS_FABRIC_ARB] =		{ 59, 26, 25, 30 },
+	[QCOM_RPM_MM_FABRIC_HALT] =		{ 89, 27, 26, 2 },
+	[QCOM_RPM_MM_FABRIC_MODE] =		{ 91, 28, 27, 3 },
+	[QCOM_RPM_MM_FABRIC_IOCTL] =		{ 94, 29, 28, 1 },
+	[QCOM_RPM_MM_FABRIC_ARB] =		{ 95, 30, 29, 2 },
+	[QCOM_RPM_CXO_BUFFERS] =		{ 209, 33, 31, 1 },
+	[QCOM_RPM_USB_OTG_SWITCH] =		{ 210, 34, 32, 1 },
+	[QCOM_RPM_HDMI_SWITCH] =		{ 211, 35, 33, 1 },
+	[QCOM_RPM_DDR_DMM] =			{ 212, 36, 34, 2 },
+	[QCOM_RPM_VDDMIN_GPIO] =		{ 215, 40, 39, 1 },
+	[QCOM_RPM_SMB208_S1a] =			{ 216, 41, 90, 2 },
+	[QCOM_RPM_SMB208_S1b] =			{ 218, 43, 91, 2 },
+	[QCOM_RPM_SMB208_S2a] =			{ 220, 45, 92, 2 },
+	[QCOM_RPM_SMB208_S2b] =			{ 222, 47, 93, 2 },
+};
+
+static const struct qcom_rpm_data ipq806x_template = {
+	.version = 3,
+	.resource_table = ipq806x_rpm_resource_table,
+	.n_resources = ARRAY_SIZE(ipq806x_rpm_resource_table),
+};
+
 static const struct of_device_id qcom_rpm_of_match[] = {
 	{ .compatible = "qcom,rpm-apq8064", .data = &apq8064_template },
 	{ .compatible = "qcom,rpm-msm8660", .data = &msm8660_template },
 	{ .compatible = "qcom,rpm-msm8960", .data = &msm8960_template },
+	{ .compatible = "qcom,rpm-ipq8064", .data = &ipq806x_template },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, qcom_rpm_of_match);
diff --git a/drivers/mfd/rk808.c b/drivers/mfd/rk808.c
index bd0215069875..4b1e4399754b 100644
--- a/drivers/mfd/rk808.c
+++ b/drivers/mfd/rk808.c
@@ -89,6 +89,7 @@ static const struct rk808_reg_data pre_init_reg[] = {
 	{ RK808_BOOST_CONFIG_REG, BOOST_ILMIN_MASK, BOOST_ILMIN_100MA },
 	{ RK808_BUCK1_CONFIG_REG, BUCK1_RATE_MASK,  BUCK_ILMIN_200MA },
 	{ RK808_BUCK2_CONFIG_REG, BUCK2_RATE_MASK,  BUCK_ILMIN_200MA },
+	{ RK808_DCDC_UV_ACT_REG,  BUCK_UV_ACT_MASK, BUCK_UV_ACT_DISABLE},
 	{ RK808_VB_MON_REG,       MASK_ALL,         VB_LO_ACT |
 						    VB_LO_SEL_3500MV },
 };
@@ -245,7 +246,7 @@ static int rk808_remove(struct i2c_client *client)
 	return 0;
 }
 
-static struct of_device_id rk808_of_match[] = {
+static const struct of_device_id rk808_of_match[] = {
 	{ .compatible = "rockchip,rk808" },
 	{ },
 };
diff --git a/drivers/mfd/rtl8411.c b/drivers/mfd/rtl8411.c
index fdd34c883d86..b3ae6592014a 100644
--- a/drivers/mfd/rtl8411.c
+++ b/drivers/mfd/rtl8411.c
@@ -53,7 +53,7 @@ static void rtl8411_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	u8 reg3 = 0;
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG1, &reg1);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg1);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg1);
 
 	if (!rtsx_vendor_setting_valid(reg1))
 		return;
@@ -65,7 +65,7 @@ static void rtl8411_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	pcr->card_drive_sel |= rtsx_reg_to_card_drive_sel(reg1);
 
 	rtsx_pci_read_config_byte(pcr, PCR_SETTING_REG3, &reg3);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG3, reg3);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG3, reg3);
 	pcr->sd30_drive_sel_3v3 = rtl8411_reg_to_sd30_drive_sel_3v3(reg3);
 }
 
@@ -74,7 +74,7 @@ static void rtl8411b_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	u32 reg = 0;
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG1, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
 
 	if (!rtsx_vendor_setting_valid(reg))
 		return;
@@ -260,9 +260,8 @@ static unsigned int rtl8411_cd_deglitch(struct rtsx_pcr *pcr)
 		rtsx_pci_write_register(pcr, CARD_PWR_CTL,
 				BPP_POWER_MASK, BPP_POWER_OFF);
 
-		dev_dbg(&(pcr->pci->dev),
-				"After CD deglitch, card_exist = 0x%x\n",
-				card_exist);
+		pcr_dbg(pcr, "After CD deglitch, card_exist = 0x%x\n",
+			card_exist);
 	}
 
 	if (card_exist & MS_EXIST) {
diff --git a/drivers/mfd/rts5209.c b/drivers/mfd/rts5209.c
index cb04174a8924..373e253c33df 100644
--- a/drivers/mfd/rts5209.c
+++ b/drivers/mfd/rts5209.c
@@ -38,7 +38,7 @@ static void rts5209_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	u32 reg;
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG1, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
 
 	if (rts5209_vendor_setting1_valid(reg)) {
 		if (rts5209_reg_check_ms_pmos(reg))
@@ -47,7 +47,7 @@ static void rts5209_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	}
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG2, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
 
 	if (rts5209_vendor_setting2_valid(reg)) {
 		pcr->sd30_drive_sel_1v8 =
diff --git a/drivers/mfd/rts5227.c b/drivers/mfd/rts5227.c
index 32407404d838..ce012d78ce2a 100644
--- a/drivers/mfd/rts5227.c
+++ b/drivers/mfd/rts5227.c
@@ -63,7 +63,7 @@ static void rts5227_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	u32 reg;
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG1, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
 
 	if (!rtsx_vendor_setting_valid(reg))
 		return;
@@ -74,7 +74,7 @@ static void rts5227_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	pcr->card_drive_sel |= rtsx_reg_to_card_drive_sel(reg);
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG2, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
 	pcr->sd30_drive_sel_3v3 = rtsx_reg_to_sd30_drive_sel_3v3(reg);
 	if (rtsx_reg_check_reverse_socket(reg))
 		pcr->flags |= PCR_REVERSE_SOCKET;
@@ -118,11 +118,9 @@ static int rts5227_extra_init_hw(struct rtsx_pcr *pcr)
 	rts5227_fill_driving(pcr, OUTPUT_3V3);
 	/* Configure force_clock_req */
 	if (pcr->flags & PCR_REVERSE_SOCKET)
-		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
-				AUTOLOAD_CFG_BASE + 3, 0xB8, 0xB8);
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PETXCFG, 0xB8, 0xB8);
 	else
-		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
-				AUTOLOAD_CFG_BASE + 3, 0xB8, 0x88);
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PETXCFG, 0xB8, 0x88);
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PM_CTRL3, 0x10, 0x00);
 
 	return rtsx_pci_send_cmd(pcr, 100);
@@ -132,7 +130,7 @@ static int rts5227_optimize_phy(struct rtsx_pcr *pcr)
 {
 	int err;
 
-	err = rtsx_gops_pm_reset(pcr);
+	err = rtsx_pci_write_register(pcr, PM_CTRL3, D3_DELINK_MODE_EN, 0x00);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/mfd/rts5229.c b/drivers/mfd/rts5229.c
index 6353f5df087a..ace45384ec8b 100644
--- a/drivers/mfd/rts5229.c
+++ b/drivers/mfd/rts5229.c
@@ -38,7 +38,7 @@ static void rts5229_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	u32 reg;
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG1, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
 
 	if (!rtsx_vendor_setting_valid(reg))
 		return;
@@ -50,7 +50,7 @@ static void rts5229_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	pcr->card_drive_sel |= rtsx_reg_to_card_drive_sel(reg);
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG2, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
 	pcr->sd30_drive_sel_3v3 =
 		map_sd_drive(rtsx_reg_to_sd30_drive_sel_3v3(reg));
 }
diff --git a/drivers/mfd/rts5249.c b/drivers/mfd/rts5249.c
index cf425cc959d5..eb2d5866f719 100644
--- a/drivers/mfd/rts5249.c
+++ b/drivers/mfd/rts5249.c
@@ -36,16 +36,16 @@ static u8 rts5249_get_ic_version(struct rtsx_pcr *pcr)
 static void rts5249_fill_driving(struct rtsx_pcr *pcr, u8 voltage)
 {
 	u8 driving_3v3[4][3] = {
-		{0x11, 0x11, 0x11},
+		{0x11, 0x11, 0x18},
 		{0x55, 0x55, 0x5C},
-		{0x99, 0x99, 0x92},
-		{0x99, 0x99, 0x92},
+		{0xFF, 0xFF, 0xFF},
+		{0x96, 0x96, 0x96},
 	};
 	u8 driving_1v8[4][3] = {
+		{0xC4, 0xC4, 0xC4},
 		{0x3C, 0x3C, 0x3C},
-		{0xB3, 0xB3, 0xB3},
 		{0xFE, 0xFE, 0xFE},
-		{0xC4, 0xC4, 0xC4},
+		{0xB3, 0xB3, 0xB3},
 	};
 	u8 (*driving)[3], drive_sel;
 
@@ -65,15 +65,17 @@ static void rts5249_fill_driving(struct rtsx_pcr *pcr, u8 voltage)
 			0xFF, driving[drive_sel][2]);
 }
 
-static void rts5249_fetch_vendor_settings(struct rtsx_pcr *pcr)
+static void rtsx_base_fetch_vendor_settings(struct rtsx_pcr *pcr)
 {
 	u32 reg;
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG1, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG1, reg);
 
-	if (!rtsx_vendor_setting_valid(reg))
+	if (!rtsx_vendor_setting_valid(reg)) {
+		pcr_dbg(pcr, "skip fetch vendor setting\n");
 		return;
+	}
 
 	pcr->aspm_en = rtsx_reg_to_aspm(reg);
 	pcr->sd30_drive_sel_1v8 = rtsx_reg_to_sd30_drive_sel_1v8(reg);
@@ -81,13 +83,13 @@ static void rts5249_fetch_vendor_settings(struct rtsx_pcr *pcr)
 	pcr->card_drive_sel |= rtsx_reg_to_card_drive_sel(reg);
 
 	rtsx_pci_read_config_dword(pcr, PCR_SETTING_REG2, &reg);
-	dev_dbg(&(pcr->pci->dev), "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
+	pcr_dbg(pcr, "Cfg 0x%x: 0x%x\n", PCR_SETTING_REG2, reg);
 	pcr->sd30_drive_sel_3v3 = rtsx_reg_to_sd30_drive_sel_3v3(reg);
 	if (rtsx_reg_check_reverse_socket(reg))
 		pcr->flags |= PCR_REVERSE_SOCKET;
 }
 
-static void rts5249_force_power_down(struct rtsx_pcr *pcr, u8 pm_state)
+static void rtsx_base_force_power_down(struct rtsx_pcr *pcr, u8 pm_state)
 {
 	/* Set relink_time to 0 */
 	rtsx_pci_write_register(pcr, AUTOLOAD_CFG_BASE + 1, 0xFF, 0);
@@ -95,7 +97,8 @@ static void rts5249_force_power_down(struct rtsx_pcr *pcr, u8 pm_state)
 	rtsx_pci_write_register(pcr, AUTOLOAD_CFG_BASE + 3, 0x01, 0);
 
 	if (pm_state == HOST_ENTER_S3)
-		rtsx_pci_write_register(pcr, PM_CTRL3, 0x10, 0x10);
+		rtsx_pci_write_register(pcr, pcr->reg_pm_ctrl3,
+			D3_DELINK_MODE_EN, D3_DELINK_MODE_EN);
 
 	rtsx_pci_write_register(pcr, FPDCTL, 0x03, 0x03);
 }
@@ -104,6 +107,8 @@ static int rts5249_extra_init_hw(struct rtsx_pcr *pcr)
 {
 	rtsx_pci_init_cmd(pcr);
 
+	/* Rest L1SUB Config */
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, L1SUB_CONFIG3, 0xFF, 0x00);
 	/* Configure GPIO as output */
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, GPIO_CTL, 0x02, 0x02);
 	/* Reset ASPM state to default value */
@@ -116,12 +121,9 @@ static int rts5249_extra_init_hw(struct rtsx_pcr *pcr)
 	/* Configure driving */
 	rts5249_fill_driving(pcr, OUTPUT_3V3);
 	if (pcr->flags & PCR_REVERSE_SOCKET)
-		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
-				AUTOLOAD_CFG_BASE + 3, 0xB0, 0xB0);
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PETXCFG, 0xB0, 0xB0);
 	else
-		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
-				AUTOLOAD_CFG_BASE + 3, 0xB0, 0x80);
-	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PM_CTRL3, 0x10, 0x00);
+		rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PETXCFG, 0xB0, 0x80);
 
 	return rtsx_pci_send_cmd(pcr, 100);
 }
@@ -130,15 +132,16 @@ static int rts5249_optimize_phy(struct rtsx_pcr *pcr)
 {
 	int err;
 
-	err = rtsx_gops_pm_reset(pcr);
+	err = rtsx_pci_write_register(pcr, PM_CTRL3, D3_DELINK_MODE_EN, 0x00);
 	if (err < 0)
 		return err;
 
-	err = rtsx_pci_write_phy_register(pcr, PHY_REG_REV,
-			PHY_REG_REV_RESV | PHY_REG_REV_RXIDLE_LATCHED |
-			PHY_REG_REV_P1_EN | PHY_REG_REV_RXIDLE_EN |
-			PHY_REG_REV_RX_PWST | PHY_REG_REV_CLKREQ_DLY_TIMER_1_0 |
-			PHY_REG_REV_STOP_CLKRD | PHY_REG_REV_STOP_CLKWR);
+	err = rtsx_pci_write_phy_register(pcr, PHY_REV,
+			PHY_REV_RESV | PHY_REV_RXIDLE_LATCHED |
+			PHY_REV_P1_EN | PHY_REV_RXIDLE_EN |
+			PHY_REV_CLKREQ_TX_EN | PHY_REV_RX_PWST |
+			PHY_REV_CLKREQ_DT_1_0 | PHY_REV_STOP_CLKRD |
+			PHY_REV_STOP_CLKWR);
 	if (err < 0)
 		return err;
 
@@ -149,19 +152,21 @@ static int rts5249_optimize_phy(struct rtsx_pcr *pcr)
 			PHY_BPCR_IB_FILTER | PHY_BPCR_CMIRROR_EN);
 	if (err < 0)
 		return err;
+
 	err = rtsx_pci_write_phy_register(pcr, PHY_PCR,
 			PHY_PCR_FORCE_CODE | PHY_PCR_OOBS_CALI_50 |
 			PHY_PCR_OOBS_VCM_08 | PHY_PCR_OOBS_SEN_90 |
-			PHY_PCR_RSSI_EN);
+			PHY_PCR_RSSI_EN | PHY_PCR_RX10K);
 	if (err < 0)
 		return err;
+
 	err = rtsx_pci_write_phy_register(pcr, PHY_RCR2,
 			PHY_RCR2_EMPHASE_EN | PHY_RCR2_NADJR |
-			PHY_RCR2_CDR_CP_10 | PHY_RCR2_CDR_SR_2 |
-			PHY_RCR2_FREQSEL_12 | PHY_RCR2_CPADJEN |
-			PHY_RCR2_CDR_SC_8 | PHY_RCR2_CALIB_LATE);
+			PHY_RCR2_CDR_SR_2 | PHY_RCR2_FREQSEL_12 |
+			PHY_RCR2_CDR_SC_12P | PHY_RCR2_CALIB_LATE);
 	if (err < 0)
 		return err;
+
 	err = rtsx_pci_write_phy_register(pcr, PHY_FLD4,
 			PHY_FLD4_FLDEN_SEL | PHY_FLD4_REQ_REF |
 			PHY_FLD4_RXAMP_OFF | PHY_FLD4_REQ_ADDA |
@@ -169,11 +174,12 @@ static int rts5249_optimize_phy(struct rtsx_pcr *pcr)
 			PHY_FLD4_BER_CHK_EN);
 	if (err < 0)
 		return err;
-	err = rtsx_pci_write_phy_register(pcr, PHY_RDR, PHY_RDR_RXDSEL_1_9);
+	err = rtsx_pci_write_phy_register(pcr, PHY_RDR,
+			PHY_RDR_RXDSEL_1_9 | PHY_SSC_AUTO_PWD);
 	if (err < 0)
 		return err;
 	err = rtsx_pci_write_phy_register(pcr, PHY_RCR1,
-			PHY_RCR1_ADP_TIME | PHY_RCR1_VCO_COARSE);
+			PHY_RCR1_ADP_TIME_4 | PHY_RCR1_VCO_COARSE);
 	if (err < 0)
 		return err;
 	err = rtsx_pci_write_phy_register(pcr, PHY_FLD3,
@@ -181,33 +187,34 @@ static int rts5249_optimize_phy(struct rtsx_pcr *pcr)
 			PHY_FLD3_RXDELINK);
 	if (err < 0)
 		return err;
+
 	return rtsx_pci_write_phy_register(pcr, PHY_TUNE,
 			PHY_TUNE_TUNEREF_1_0 | PHY_TUNE_VBGSEL_1252 |
 			PHY_TUNE_SDBUS_33 | PHY_TUNE_TUNED18 |
-			PHY_TUNE_TUNED12);
+			PHY_TUNE_TUNED12 | PHY_TUNE_TUNEA12);
 }
 
-static int rts5249_turn_on_led(struct rtsx_pcr *pcr)
+static int rtsx_base_turn_on_led(struct rtsx_pcr *pcr)
 {
 	return rtsx_pci_write_register(pcr, GPIO_CTL, 0x02, 0x02);
 }
 
-static int rts5249_turn_off_led(struct rtsx_pcr *pcr)
+static int rtsx_base_turn_off_led(struct rtsx_pcr *pcr)
 {
 	return rtsx_pci_write_register(pcr, GPIO_CTL, 0x02, 0x00);
 }
 
-static int rts5249_enable_auto_blink(struct rtsx_pcr *pcr)
+static int rtsx_base_enable_auto_blink(struct rtsx_pcr *pcr)
 {
 	return rtsx_pci_write_register(pcr, OLT_LED_CTL, 0x08, 0x08);
 }
 
-static int rts5249_disable_auto_blink(struct rtsx_pcr *pcr)
+static int rtsx_base_disable_auto_blink(struct rtsx_pcr *pcr)
 {
 	return rtsx_pci_write_register(pcr, OLT_LED_CTL, 0x08, 0x00);
 }
 
-static int rts5249_card_power_on(struct rtsx_pcr *pcr, int card)
+static int rtsx_base_card_power_on(struct rtsx_pcr *pcr, int card)
 {
 	int err;
 
@@ -234,7 +241,7 @@ static int rts5249_card_power_on(struct rtsx_pcr *pcr, int card)
 	return 0;
 }
 
-static int rts5249_card_power_off(struct rtsx_pcr *pcr, int card)
+static int rtsx_base_card_power_off(struct rtsx_pcr *pcr, int card)
 {
 	rtsx_pci_init_cmd(pcr);
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_PWR_CTL,
@@ -244,22 +251,35 @@ static int rts5249_card_power_off(struct rtsx_pcr *pcr, int card)
 	return rtsx_pci_send_cmd(pcr, 100);
 }
 
-static int rts5249_switch_output_voltage(struct rtsx_pcr *pcr, u8 voltage)
+static int rtsx_base_switch_output_voltage(struct rtsx_pcr *pcr, u8 voltage)
 {
 	int err;
+	u16 append;
 
-	if (voltage == OUTPUT_3V3) {
-		err = rtsx_pci_write_phy_register(pcr, PHY_TUNE, 0x4FC0 | 0x24);
+	switch (voltage) {
+	case OUTPUT_3V3:
+		err = rtsx_pci_update_phy(pcr, PHY_TUNE, PHY_TUNE_VOLTAGE_MASK,
+			PHY_TUNE_VOLTAGE_3V3);
 		if (err < 0)
 			return err;
-	} else if (voltage == OUTPUT_1V8) {
-		err = rtsx_pci_write_phy_register(pcr, PHY_BACR, 0x3C02);
+		break;
+	case OUTPUT_1V8:
+		append = PHY_TUNE_D18_1V8;
+		if (CHK_PCI_PID(pcr, 0x5249)) {
+			err = rtsx_pci_update_phy(pcr, PHY_BACR,
+				PHY_BACR_BASIC_MASK, 0);
+			if (err < 0)
+				return err;
+			append = PHY_TUNE_D18_1V7;
+		}
+
+		err = rtsx_pci_update_phy(pcr, PHY_TUNE, PHY_TUNE_VOLTAGE_MASK,
+			append);
 		if (err < 0)
 			return err;
-		err = rtsx_pci_write_phy_register(pcr, PHY_TUNE, 0x4C40 | 0x24);
-		if (err < 0)
-			return err;
-	} else {
+		break;
+	default:
+		pcr_dbg(pcr, "unknown output voltage %d\n", voltage);
 		return -EINVAL;
 	}
 
@@ -270,17 +290,17 @@ static int rts5249_switch_output_voltage(struct rtsx_pcr *pcr, u8 voltage)
 }
 
 static const struct pcr_ops rts5249_pcr_ops = {
-	.fetch_vendor_settings = rts5249_fetch_vendor_settings,
+	.fetch_vendor_settings = rtsx_base_fetch_vendor_settings,
 	.extra_init_hw = rts5249_extra_init_hw,
 	.optimize_phy = rts5249_optimize_phy,
-	.turn_on_led = rts5249_turn_on_led,
-	.turn_off_led = rts5249_turn_off_led,
-	.enable_auto_blink = rts5249_enable_auto_blink,
-	.disable_auto_blink = rts5249_disable_auto_blink,
-	.card_power_on = rts5249_card_power_on,
-	.card_power_off = rts5249_card_power_off,
-	.switch_output_voltage = rts5249_switch_output_voltage,
-	.force_power_down = rts5249_force_power_down,
+	.turn_on_led = rtsx_base_turn_on_led,
+	.turn_off_led = rtsx_base_turn_off_led,
+	.enable_auto_blink = rtsx_base_enable_auto_blink,
+	.disable_auto_blink = rtsx_base_disable_auto_blink,
+	.card_power_on = rtsx_base_card_power_on,
+	.card_power_off = rtsx_base_card_power_off,
+	.switch_output_voltage = rtsx_base_switch_output_voltage,
+	.force_power_down = rtsx_base_force_power_down,
 };
 
 /* SD Pull Control Enable:
@@ -343,7 +363,7 @@ void rts5249_init_params(struct rtsx_pcr *pcr)
 
 	pcr->flags = 0;
 	pcr->card_drive_sel = RTSX_CARD_DRIVE_DEFAULT;
-	pcr->sd30_drive_sel_1v8 = CFG_DRIVER_TYPE_C;
+	pcr->sd30_drive_sel_1v8 = CFG_DRIVER_TYPE_B;
 	pcr->sd30_drive_sel_3v3 = CFG_DRIVER_TYPE_B;
 	pcr->aspm_en = ASPM_L1_EN;
 	pcr->tx_initial_phase = SET_CLOCK_PHASE(1, 29, 16);
@@ -354,4 +374,219 @@ void rts5249_init_params(struct rtsx_pcr *pcr)
 	pcr->sd_pull_ctl_disable_tbl = rts5249_sd_pull_ctl_disable_tbl;
 	pcr->ms_pull_ctl_enable_tbl = rts5249_ms_pull_ctl_enable_tbl;
 	pcr->ms_pull_ctl_disable_tbl = rts5249_ms_pull_ctl_disable_tbl;
+
+	pcr->reg_pm_ctrl3 = PM_CTRL3;
+}
+
+static int rts524a_write_phy(struct rtsx_pcr *pcr, u8 addr, u16 val)
+{
+	addr = addr & 0x80 ? (addr & 0x7F) | 0x40 : addr;
+
+	return __rtsx_pci_write_phy_register(pcr, addr, val);
+}
+
+static int rts524a_read_phy(struct rtsx_pcr *pcr, u8 addr, u16 *val)
+{
+	addr = addr & 0x80 ? (addr & 0x7F) | 0x40 : addr;
+
+	return __rtsx_pci_read_phy_register(pcr, addr, val);
 }
+
+static int rts524a_optimize_phy(struct rtsx_pcr *pcr)
+{
+	int err;
+
+	err = rtsx_pci_write_register(pcr, RTS524A_PM_CTRL3,
+		D3_DELINK_MODE_EN, 0x00);
+	if (err < 0)
+		return err;
+
+	rtsx_pci_write_phy_register(pcr, PHY_PCR,
+		PHY_PCR_FORCE_CODE | PHY_PCR_OOBS_CALI_50 |
+		PHY_PCR_OOBS_VCM_08 | PHY_PCR_OOBS_SEN_90 | PHY_PCR_RSSI_EN);
+	rtsx_pci_write_phy_register(pcr, PHY_SSCCR3,
+		PHY_SSCCR3_STEP_IN | PHY_SSCCR3_CHECK_DELAY);
+
+	if (is_version(pcr, 0x524A, IC_VER_A)) {
+		rtsx_pci_write_phy_register(pcr, PHY_SSCCR3,
+			PHY_SSCCR3_STEP_IN | PHY_SSCCR3_CHECK_DELAY);
+		rtsx_pci_write_phy_register(pcr, PHY_SSCCR2,
+			PHY_SSCCR2_PLL_NCODE | PHY_SSCCR2_TIME0 |
+			PHY_SSCCR2_TIME2_WIDTH);
+		rtsx_pci_write_phy_register(pcr, PHY_ANA1A,
+			PHY_ANA1A_TXR_LOOPBACK | PHY_ANA1A_RXT_BIST |
+			PHY_ANA1A_TXR_BIST | PHY_ANA1A_REV);
+		rtsx_pci_write_phy_register(pcr, PHY_ANA1D,
+			PHY_ANA1D_DEBUG_ADDR);
+		rtsx_pci_write_phy_register(pcr, PHY_DIG1E,
+			PHY_DIG1E_REV | PHY_DIG1E_D0_X_D1 |
+			PHY_DIG1E_RX_ON_HOST | PHY_DIG1E_RCLK_REF_HOST |
+			PHY_DIG1E_RCLK_TX_EN_KEEP |
+			PHY_DIG1E_RCLK_TX_TERM_KEEP |
+			PHY_DIG1E_RCLK_RX_EIDLE_ON | PHY_DIG1E_TX_TERM_KEEP |
+			PHY_DIG1E_RX_TERM_KEEP | PHY_DIG1E_TX_EN_KEEP |
+			PHY_DIG1E_RX_EN_KEEP);
+	}
+
+	rtsx_pci_write_phy_register(pcr, PHY_ANA08,
+		PHY_ANA08_RX_EQ_DCGAIN | PHY_ANA08_SEL_RX_EN |
+		PHY_ANA08_RX_EQ_VAL | PHY_ANA08_SCP | PHY_ANA08_SEL_IPI);
+
+	return 0;
+}
+
+static int rts524a_extra_init_hw(struct rtsx_pcr *pcr)
+{
+	rts5249_extra_init_hw(pcr);
+
+	rtsx_pci_write_register(pcr, FUNC_FORCE_CTL,
+		FORCE_ASPM_L1_EN, FORCE_ASPM_L1_EN);
+	rtsx_pci_write_register(pcr, PM_EVENT_DEBUG, PME_DEBUG_0, PME_DEBUG_0);
+	rtsx_pci_write_register(pcr, LDO_VCC_CFG1, LDO_VCC_LMT_EN,
+		LDO_VCC_LMT_EN);
+	rtsx_pci_write_register(pcr, PCLK_CTL, PCLK_MODE_SEL, PCLK_MODE_SEL);
+	if (is_version(pcr, 0x524A, IC_VER_A)) {
+		rtsx_pci_write_register(pcr, LDO_DV18_CFG,
+			LDO_DV18_SR_MASK, LDO_DV18_SR_DF);
+		rtsx_pci_write_register(pcr, LDO_VCC_CFG1,
+			LDO_VCC_REF_TUNE_MASK, LDO_VCC_REF_1V2);
+		rtsx_pci_write_register(pcr, LDO_VIO_CFG,
+			LDO_VIO_REF_TUNE_MASK, LDO_VIO_REF_1V2);
+		rtsx_pci_write_register(pcr, LDO_VIO_CFG,
+			LDO_VIO_SR_MASK, LDO_VIO_SR_DF);
+		rtsx_pci_write_register(pcr, LDO_DV12S_CFG,
+			LDO_REF12_TUNE_MASK, LDO_REF12_TUNE_DF);
+		rtsx_pci_write_register(pcr, SD40_LDO_CTL1,
+			SD40_VIO_TUNE_MASK, SD40_VIO_TUNE_1V7);
+	}
+
+	return 0;
+}
+
+static const struct pcr_ops rts524a_pcr_ops = {
+	.write_phy = rts524a_write_phy,
+	.read_phy = rts524a_read_phy,
+	.fetch_vendor_settings = rtsx_base_fetch_vendor_settings,
+	.extra_init_hw = rts524a_extra_init_hw,
+	.optimize_phy = rts524a_optimize_phy,
+	.turn_on_led = rtsx_base_turn_on_led,
+	.turn_off_led = rtsx_base_turn_off_led,
+	.enable_auto_blink = rtsx_base_enable_auto_blink,
+	.disable_auto_blink = rtsx_base_disable_auto_blink,
+	.card_power_on = rtsx_base_card_power_on,
+	.card_power_off = rtsx_base_card_power_off,
+	.switch_output_voltage = rtsx_base_switch_output_voltage,
+	.force_power_down = rtsx_base_force_power_down,
+};
+
+void rts524a_init_params(struct rtsx_pcr *pcr)
+{
+	rts5249_init_params(pcr);
+
+	pcr->reg_pm_ctrl3 = RTS524A_PM_CTRL3;
+	pcr->ops = &rts524a_pcr_ops;
+}
+
+static int rts525a_card_power_on(struct rtsx_pcr *pcr, int card)
+{
+	rtsx_pci_write_register(pcr, LDO_VCC_CFG1,
+		LDO_VCC_TUNE_MASK, LDO_VCC_3V3);
+	return rtsx_base_card_power_on(pcr, card);
+}
+
+static int rts525a_switch_output_voltage(struct rtsx_pcr *pcr, u8 voltage)
+{
+	switch (voltage) {
+	case OUTPUT_3V3:
+		rtsx_pci_write_register(pcr, LDO_CONFIG2,
+			LDO_D3318_MASK, LDO_D3318_33V);
+		rtsx_pci_write_register(pcr, SD_PAD_CTL, SD_IO_USING_1V8, 0);
+		break;
+	case OUTPUT_1V8:
+		rtsx_pci_write_register(pcr, LDO_CONFIG2,
+			LDO_D3318_MASK, LDO_D3318_18V);
+		rtsx_pci_write_register(pcr, SD_PAD_CTL, SD_IO_USING_1V8,
+			SD_IO_USING_1V8);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	rtsx_pci_init_cmd(pcr);
+	rts5249_fill_driving(pcr, voltage);
+	return rtsx_pci_send_cmd(pcr, 100);
+}
+
+static int rts525a_optimize_phy(struct rtsx_pcr *pcr)
+{
+	int err;
+
+	err = rtsx_pci_write_register(pcr, RTS524A_PM_CTRL3,
+		D3_DELINK_MODE_EN, 0x00);
+	if (err < 0)
+		return err;
+
+	rtsx_pci_write_phy_register(pcr, _PHY_FLD0,
+		_PHY_FLD0_CLK_REQ_20C | _PHY_FLD0_RX_IDLE_EN |
+		_PHY_FLD0_BIT_ERR_RSTN | _PHY_FLD0_BER_COUNT |
+		_PHY_FLD0_BER_TIMER | _PHY_FLD0_CHECK_EN);
+
+	rtsx_pci_write_phy_register(pcr, _PHY_ANA03,
+		_PHY_ANA03_TIMER_MAX | _PHY_ANA03_OOBS_DEB_EN |
+		_PHY_CMU_DEBUG_EN);
+
+	if (is_version(pcr, 0x525A, IC_VER_A))
+		rtsx_pci_write_phy_register(pcr, _PHY_REV0,
+			_PHY_REV0_FILTER_OUT | _PHY_REV0_CDR_BYPASS_PFD |
+			_PHY_REV0_CDR_RX_IDLE_BYPASS);
+
+	return 0;
+}
+
+static int rts525a_extra_init_hw(struct rtsx_pcr *pcr)
+{
+	rts5249_extra_init_hw(pcr);
+
+	rtsx_pci_write_register(pcr, PCLK_CTL, PCLK_MODE_SEL, PCLK_MODE_SEL);
+	if (is_version(pcr, 0x525A, IC_VER_A)) {
+		rtsx_pci_write_register(pcr, L1SUB_CONFIG2,
+			L1SUB_AUTO_CFG, L1SUB_AUTO_CFG);
+		rtsx_pci_write_register(pcr, RREF_CFG,
+			RREF_VBGSEL_MASK, RREF_VBGSEL_1V25);
+		rtsx_pci_write_register(pcr, LDO_VIO_CFG,
+			LDO_VIO_TUNE_MASK, LDO_VIO_1V7);
+		rtsx_pci_write_register(pcr, LDO_DV12S_CFG,
+			LDO_D12_TUNE_MASK, LDO_D12_TUNE_DF);
+		rtsx_pci_write_register(pcr, LDO_AV12S_CFG,
+			LDO_AV12S_TUNE_MASK, LDO_AV12S_TUNE_DF);
+		rtsx_pci_write_register(pcr, LDO_VCC_CFG0,
+			LDO_VCC_LMTVTH_MASK, LDO_VCC_LMTVTH_2A);
+		rtsx_pci_write_register(pcr, OOBS_CONFIG,
+			OOBS_AUTOK_DIS | OOBS_VAL_MASK, 0x89);
+	}
+
+	return 0;
+}
+
+static const struct pcr_ops rts525a_pcr_ops = {
+	.fetch_vendor_settings = rtsx_base_fetch_vendor_settings,
+	.extra_init_hw = rts525a_extra_init_hw,
+	.optimize_phy = rts525a_optimize_phy,
+	.turn_on_led = rtsx_base_turn_on_led,
+	.turn_off_led = rtsx_base_turn_off_led,
+	.enable_auto_blink = rtsx_base_enable_auto_blink,
+	.disable_auto_blink = rtsx_base_disable_auto_blink,
+	.card_power_on = rts525a_card_power_on,
+	.card_power_off = rtsx_base_card_power_off,
+	.switch_output_voltage = rts525a_switch_output_voltage,
+	.force_power_down = rtsx_base_force_power_down,
+};
+
+void rts525a_init_params(struct rtsx_pcr *pcr)
+{
+	rts5249_init_params(pcr);
+
+	pcr->reg_pm_ctrl3 = RTS524A_PM_CTRL3;
+	pcr->ops = &rts525a_pcr_ops;
+}
+
diff --git a/drivers/mfd/rtsx_gops.c b/drivers/mfd/rtsx_gops.c
deleted file mode 100644
index b1a98c678593..000000000000
--- a/drivers/mfd/rtsx_gops.c
+++ /dev/null
@@ -1,37 +0,0 @@
-/* Driver for Realtek PCI-Express card reader
- *
- * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation; either version 2, or (at your option) any
- * later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, see <http://www.gnu.org/licenses/>.
- *
- * Author:
- *   Micky Ching <micky_ching@realsil.com.cn>
- */
-
-#include <linux/mfd/rtsx_pci.h>
-#include "rtsx_pcr.h"
-
-int rtsx_gops_pm_reset(struct rtsx_pcr *pcr)
-{
-	int err;
-
-	/* init aspm */
-	rtsx_pci_write_register(pcr, ASPM_FORCE_CTL, 0xFF, 0x00);
-	err = rtsx_pci_update_cfg_byte(pcr, LCTLR, ~LCTLR_ASPM_CTL_MASK, 0x00);
-	if (err < 0)
-		return err;
-
-	/* reset PM_CTRL3 before send buffer cmd */
-	return rtsx_pci_write_register(pcr, PM_CTRL3, D3_DELINK_MODE_EN, 0x00);
-}
diff --git a/drivers/mfd/rtsx_pcr.c b/drivers/mfd/rtsx_pcr.c
index 30f7ca89a0e6..a66540a49079 100644
--- a/drivers/mfd/rtsx_pcr.c
+++ b/drivers/mfd/rtsx_pcr.c
@@ -58,11 +58,25 @@ static const struct pci_device_id rtsx_pci_ids[] = {
 	{ PCI_DEVICE(0x10EC, 0x5249), PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ PCI_DEVICE(0x10EC, 0x5287), PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ PCI_DEVICE(0x10EC, 0x5286), PCI_CLASS_OTHERS << 16, 0xFF0000 },
+	{ PCI_DEVICE(0x10EC, 0x524A), PCI_CLASS_OTHERS << 16, 0xFF0000 },
+	{ PCI_DEVICE(0x10EC, 0x525A), PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ 0, }
 };
 
 MODULE_DEVICE_TABLE(pci, rtsx_pci_ids);
 
+static inline void rtsx_pci_enable_aspm(struct rtsx_pcr *pcr)
+{
+	rtsx_pci_update_cfg_byte(pcr, pcr->pcie_cap + PCI_EXP_LNKCTL,
+		0xFC, pcr->aspm_en);
+}
+
+static inline void rtsx_pci_disable_aspm(struct rtsx_pcr *pcr)
+{
+	rtsx_pci_update_cfg_byte(pcr, pcr->pcie_cap + PCI_EXP_LNKCTL,
+		0xFC, 0);
+}
+
 void rtsx_pci_start_run(struct rtsx_pcr *pcr)
 {
 	/* If pci device removed, don't queue idle work any more */
@@ -75,7 +89,7 @@ void rtsx_pci_start_run(struct rtsx_pcr *pcr)
 			pcr->ops->enable_auto_blink(pcr);
 
 		if (pcr->aspm_en)
-			rtsx_pci_write_config_byte(pcr, LCTLR, 0);
+			rtsx_pci_disable_aspm(pcr);
 	}
 
 	mod_delayed_work(system_wq, &pcr->idle_work, msecs_to_jiffies(200));
@@ -130,7 +144,7 @@ int rtsx_pci_read_register(struct rtsx_pcr *pcr, u16 addr, u8 *data)
 }
 EXPORT_SYMBOL_GPL(rtsx_pci_read_register);
 
-int rtsx_pci_write_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 val)
+int __rtsx_pci_write_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 val)
 {
 	int err, i, finished = 0;
 	u8 tmp;
@@ -162,9 +176,17 @@ int rtsx_pci_write_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 val)
 
 	return 0;
 }
+
+int rtsx_pci_write_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 val)
+{
+	if (pcr->ops->write_phy)
+		return pcr->ops->write_phy(pcr, addr, val);
+
+	return __rtsx_pci_write_phy_register(pcr, addr, val);
+}
 EXPORT_SYMBOL_GPL(rtsx_pci_write_phy_register);
 
-int rtsx_pci_read_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 *val)
+int __rtsx_pci_read_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 *val)
 {
 	int err, i, finished = 0;
 	u16 data;
@@ -210,6 +232,14 @@ int rtsx_pci_read_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 *val)
 
 	return 0;
 }
+
+int rtsx_pci_read_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 *val)
+{
+	if (pcr->ops->read_phy)
+		return pcr->ops->read_phy(pcr, addr, val);
+
+	return __rtsx_pci_read_phy_register(pcr, addr, val);
+}
 EXPORT_SYMBOL_GPL(rtsx_pci_read_phy_register);
 
 void rtsx_pci_stop_cmd(struct rtsx_pcr *pcr)
@@ -286,8 +316,7 @@ int rtsx_pci_send_cmd(struct rtsx_pcr *pcr, int timeout)
 	timeleft = wait_for_completion_interruptible_timeout(
 			&trans_done, msecs_to_jiffies(timeout));
 	if (timeleft <= 0) {
-		dev_dbg(&(pcr->pci->dev), "Timeout (%s %d)\n",
-				__func__, __LINE__);
+		pcr_dbg(pcr, "Timeout (%s %d)\n", __func__, __LINE__);
 		err = -ETIMEDOUT;
 		goto finish_send_cmd;
 	}
@@ -323,8 +352,7 @@ static void rtsx_pci_add_sg_tbl(struct rtsx_pcr *pcr,
 	u64 val;
 	u8 option = SG_VALID | SG_TRANS_DATA;
 
-	dev_dbg(&(pcr->pci->dev), "DMA addr: 0x%x, Len: 0x%x\n",
-			(unsigned int)addr, len);
+	pcr_dbg(pcr, "DMA addr: 0x%x, Len: 0x%x\n", (unsigned int)addr, len);
 
 	if (end)
 		option |= SG_END;
@@ -339,11 +367,11 @@ int rtsx_pci_transfer_data(struct rtsx_pcr *pcr, struct scatterlist *sglist,
 {
 	int err = 0, count;
 
-	dev_dbg(&(pcr->pci->dev), "--> %s: num_sg = %d\n", __func__, num_sg);
+	pcr_dbg(pcr, "--> %s: num_sg = %d\n", __func__, num_sg);
 	count = rtsx_pci_dma_map_sg(pcr, sglist, num_sg, read);
 	if (count < 1)
 		return -EINVAL;
-	dev_dbg(&(pcr->pci->dev), "DMA mapping count: %d\n", count);
+	pcr_dbg(pcr, "DMA mapping count: %d\n", count);
 
 	err = rtsx_pci_dma_transfer(pcr, sglist, count, read, timeout);
 
@@ -417,8 +445,7 @@ int rtsx_pci_dma_transfer(struct rtsx_pcr *pcr, struct scatterlist *sglist,
 	timeleft = wait_for_completion_interruptible_timeout(
 			&trans_done, msecs_to_jiffies(timeout));
 	if (timeleft <= 0) {
-		dev_dbg(&(pcr->pci->dev), "Timeout (%s %d)\n",
-				__func__, __LINE__);
+		pcr_dbg(pcr, "Timeout (%s %d)\n", __func__, __LINE__);
 		err = -ETIMEDOUT;
 		goto out;
 	}
@@ -592,7 +619,7 @@ static void rtsx_pci_enable_bus_int(struct rtsx_pcr *pcr)
 	/* Enable Bus Interrupt */
 	rtsx_pci_writel(pcr, RTSX_BIER, pcr->bier);
 
-	dev_dbg(&(pcr->pci->dev), "RTSX_BIER: 0x%08x\n", pcr->bier);
+	pcr_dbg(pcr, "RTSX_BIER: 0x%08x\n", pcr->bier);
 }
 
 static inline u8 double_ssc_depth(u8 depth)
@@ -638,14 +665,13 @@ int rtsx_pci_switch_clock(struct rtsx_pcr *pcr, unsigned int card_clock,
 		return err;
 
 	card_clock /= 1000000;
-	dev_dbg(&(pcr->pci->dev), "Switch card clock to %dMHz\n", card_clock);
+	pcr_dbg(pcr, "Switch card clock to %dMHz\n", card_clock);
 
 	clk = card_clock;
 	if (!initial_mode && double_clk)
 		clk = card_clock * 2;
-	dev_dbg(&(pcr->pci->dev),
-			"Internal SSC clock: %dMHz (cur_clock = %d)\n",
-			clk, pcr->cur_clock);
+	pcr_dbg(pcr, "Internal SSC clock: %dMHz (cur_clock = %d)\n",
+		clk, pcr->cur_clock);
 
 	if (clk == pcr->cur_clock)
 		return 0;
@@ -674,14 +700,14 @@ int rtsx_pci_switch_clock(struct rtsx_pcr *pcr, unsigned int card_clock,
 		}
 		div++;
 	}
-	dev_dbg(&(pcr->pci->dev), "n = %d, div = %d\n", n, div);
+	pcr_dbg(pcr, "n = %d, div = %d\n", n, div);
 
 	ssc_depth = depth[ssc_depth];
 	if (double_clk)
 		ssc_depth = double_ssc_depth(ssc_depth);
 
 	ssc_depth = revise_ssc_depth(ssc_depth, div);
-	dev_dbg(&(pcr->pci->dev), "ssc_depth = %d\n", ssc_depth);
+	pcr_dbg(pcr, "ssc_depth = %d\n", ssc_depth);
 
 	rtsx_pci_init_cmd(pcr);
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CLK_CTL,
@@ -803,13 +829,13 @@ static void rtsx_pci_card_detect(struct work_struct *work)
 	dwork = to_delayed_work(work);
 	pcr = container_of(dwork, struct rtsx_pcr, carddet_work);
 
-	dev_dbg(&(pcr->pci->dev), "--> %s\n", __func__);
+	pcr_dbg(pcr, "--> %s\n", __func__);
 
 	mutex_lock(&pcr->pcr_mutex);
 	spin_lock_irqsave(&pcr->lock, flags);
 
 	irq_status = rtsx_pci_readl(pcr, RTSX_BIPR);
-	dev_dbg(&(pcr->pci->dev), "irq_status: 0x%08x\n", irq_status);
+	pcr_dbg(pcr, "irq_status: 0x%08x\n", irq_status);
 
 	irq_status &= CARD_EXIST;
 	card_inserted = pcr->card_inserted & irq_status;
@@ -820,9 +846,8 @@ static void rtsx_pci_card_detect(struct work_struct *work)
 	spin_unlock_irqrestore(&pcr->lock, flags);
 
 	if (card_inserted || card_removed) {
-		dev_dbg(&(pcr->pci->dev),
-				"card_inserted: 0x%x, card_removed: 0x%x\n",
-				card_inserted, card_removed);
+		pcr_dbg(pcr, "card_inserted: 0x%x, card_removed: 0x%x\n",
+			card_inserted, card_removed);
 
 		if (pcr->ops->cd_deglitch)
 			card_inserted = pcr->ops->cd_deglitch(pcr);
@@ -930,7 +955,7 @@ static void rtsx_pci_idle_work(struct work_struct *work)
 	struct delayed_work *dwork = to_delayed_work(work);
 	struct rtsx_pcr *pcr = container_of(dwork, struct rtsx_pcr, idle_work);
 
-	dev_dbg(&(pcr->pci->dev), "--> %s\n", __func__);
+	pcr_dbg(pcr, "--> %s\n", __func__);
 
 	mutex_lock(&pcr->pcr_mutex);
 
@@ -942,7 +967,7 @@ static void rtsx_pci_idle_work(struct work_struct *work)
 		pcr->ops->turn_off_led(pcr);
 
 	if (pcr->aspm_en)
-		rtsx_pci_write_config_byte(pcr, LCTLR, pcr->aspm_en);
+		rtsx_pci_enable_aspm(pcr);
 
 	mutex_unlock(&pcr->pcr_mutex);
 }
@@ -968,6 +993,7 @@ static int rtsx_pci_init_hw(struct rtsx_pcr *pcr)
 {
 	int err;
 
+	pcr->pcie_cap = pci_find_capability(pcr->pci, PCI_CAP_ID_EXP);
 	rtsx_pci_writel(pcr, RTSX_HCBAR, pcr->host_cmds_addr);
 
 	rtsx_pci_enable_bus_int(pcr);
@@ -980,6 +1006,7 @@ static int rtsx_pci_init_hw(struct rtsx_pcr *pcr)
 	/* Wait SSC power stable */
 	udelay(200);
 
+	rtsx_pci_disable_aspm(pcr);
 	if (pcr->ops->optimize_phy) {
 		err = pcr->ops->optimize_phy(pcr);
 		if (err < 0)
@@ -1028,10 +1055,8 @@ static int rtsx_pci_init_hw(struct rtsx_pcr *pcr)
 	if (err < 0)
 		return err;
 
-	rtsx_pci_write_config_byte(pcr, LCTLR, 0);
-
 	/* Enable clk_request_n to enable clock power management */
-	rtsx_pci_write_config_byte(pcr, 0x81, 1);
+	rtsx_pci_write_config_byte(pcr, pcr->pcie_cap + PCI_EXP_LNKCTL + 1, 1);
 	/* Enter L1 when host tx idle */
 	rtsx_pci_write_config_byte(pcr, 0x70F, 0x5B);
 
@@ -1081,6 +1106,14 @@ static int rtsx_pci_init_chip(struct rtsx_pcr *pcr)
 		rts5249_init_params(pcr);
 		break;
 
+	case 0x524A:
+		rts524a_init_params(pcr);
+		break;
+
+	case 0x525A:
+		rts525a_init_params(pcr);
+		break;
+
 	case 0x5287:
 		rtl8411b_init_params(pcr);
 		break;
@@ -1090,7 +1123,7 @@ static int rtsx_pci_init_chip(struct rtsx_pcr *pcr)
 		break;
 	}
 
-	dev_dbg(&(pcr->pci->dev), "PID: 0x%04x, IC version: 0x%02x\n",
+	pcr_dbg(pcr, "PID: 0x%04x, IC version: 0x%02x\n",
 			PCI_PID(pcr), pcr->ic_version);
 
 	pcr->slots = kcalloc(pcr->num_slots, sizeof(struct rtsx_slot),
@@ -1101,14 +1134,14 @@ static int rtsx_pci_init_chip(struct rtsx_pcr *pcr)
 	if (pcr->ops->fetch_vendor_settings)
 		pcr->ops->fetch_vendor_settings(pcr);
 
-	dev_dbg(&(pcr->pci->dev), "pcr->aspm_en = 0x%x\n", pcr->aspm_en);
-	dev_dbg(&(pcr->pci->dev), "pcr->sd30_drive_sel_1v8 = 0x%x\n",
+	pcr_dbg(pcr, "pcr->aspm_en = 0x%x\n", pcr->aspm_en);
+	pcr_dbg(pcr, "pcr->sd30_drive_sel_1v8 = 0x%x\n",
 			pcr->sd30_drive_sel_1v8);
-	dev_dbg(&(pcr->pci->dev), "pcr->sd30_drive_sel_3v3 = 0x%x\n",
+	pcr_dbg(pcr, "pcr->sd30_drive_sel_3v3 = 0x%x\n",
 			pcr->sd30_drive_sel_3v3);
-	dev_dbg(&(pcr->pci->dev), "pcr->card_drive_sel = 0x%x\n",
+	pcr_dbg(pcr, "pcr->card_drive_sel = 0x%x\n",
 			pcr->card_drive_sel);
-	dev_dbg(&(pcr->pci->dev), "pcr->flags = 0x%x\n", pcr->flags);
+	pcr_dbg(pcr, "pcr->flags = 0x%x\n", pcr->flags);
 
 	pcr->state = PDEV_STAT_IDLE;
 	err = rtsx_pci_init_hw(pcr);
@@ -1126,7 +1159,7 @@ static int rtsx_pci_probe(struct pci_dev *pcidev,
 	struct rtsx_pcr *pcr;
 	struct pcr_handle *handle;
 	u32 base, len;
-	int ret, i;
+	int ret, i, bar = 0;
 
 	dev_dbg(&(pcidev->dev),
 		": Realtek PCI-E Card Reader found at %s [%04x:%04x] (rev %x)\n",
@@ -1171,8 +1204,10 @@ static int rtsx_pci_probe(struct pci_dev *pcidev,
 	pcr->pci = pcidev;
 	dev_set_drvdata(&pcidev->dev, handle);
 
-	len = pci_resource_len(pcidev, 0);
-	base = pci_resource_start(pcidev, 0);
+	if (CHK_PCI_PID(pcr, 0x525A))
+		bar = 1;
+	len = pci_resource_len(pcidev, bar);
+	base = pci_resource_start(pcidev, bar);
 	pcr->remap_addr = ioremap_nocache(base, len);
 	if (!pcr->remap_addr) {
 		ret = -ENOMEM;
diff --git a/drivers/mfd/rtsx_pcr.h b/drivers/mfd/rtsx_pcr.h
index fe2bbb67defc..ce48842570d7 100644
--- a/drivers/mfd/rtsx_pcr.h
+++ b/drivers/mfd/rtsx_pcr.h
@@ -27,12 +27,20 @@
 #define MIN_DIV_N_PCR		80
 #define MAX_DIV_N_PCR		208
 
+#define RTS524A_PME_FORCE_CTL		0xFF78
+#define RTS524A_PM_CTRL3		0xFF7E
+
+int __rtsx_pci_write_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 val);
+int __rtsx_pci_read_phy_register(struct rtsx_pcr *pcr, u8 addr, u16 *val);
+
 void rts5209_init_params(struct rtsx_pcr *pcr);
 void rts5229_init_params(struct rtsx_pcr *pcr);
 void rtl8411_init_params(struct rtsx_pcr *pcr);
 void rtl8402_init_params(struct rtsx_pcr *pcr);
 void rts5227_init_params(struct rtsx_pcr *pcr);
 void rts5249_init_params(struct rtsx_pcr *pcr);
+void rts524a_init_params(struct rtsx_pcr *pcr);
+void rts525a_init_params(struct rtsx_pcr *pcr);
 void rtl8411b_init_params(struct rtsx_pcr *pcr);
 
 static inline u8 map_sd_drive(int idx)
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/mfd/sec-core.c b/drivers/mfd/sec-core.c
index 0a7bc43db4e4..4a69afb425ad 100644
--- a/drivers/mfd/sec-core.c
+++ b/drivers/mfd/sec-core.c
@@ -69,6 +69,8 @@ static const struct mfd_cell s2mps11_devs[] = {
 	{
 		.name = "s2mps11-pmic",
 	}, {
+		.name = "s2mps14-rtc",
+	}, {
 		.name = "s2mps11-clk",
 		.of_compatible = "samsung,s2mps11-clk",
 	}
@@ -267,10 +269,8 @@ static struct sec_platform_data *sec_pmic_i2c_parse_dt_pdata(
 	struct sec_platform_data *pd;
 
 	pd = devm_kzalloc(dev, sizeof(*pd), GFP_KERNEL);
-	if (!pd) {
-		dev_err(dev, "could not allocate memory for pdata\n");
+	if (!pd)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	/*
 	 * ToDo: the 'wakeup' member in the platform data is more of a linux
@@ -333,7 +333,6 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 	}
 	if (pdata) {
 		sec_pmic->device_type = pdata->device_type;
-		sec_pmic->ono = pdata->ono;
 		sec_pmic->irq_base = pdata->irq_base;
 		sec_pmic->wakeup = pdata->wakeup;
 		sec_pmic->pdata = pdata;
diff --git a/drivers/mfd/sec-irq.c b/drivers/mfd/sec-irq.c
index ba86a918c2da..806fa8dbb22d 100644
--- a/drivers/mfd/sec-irq.c
+++ b/drivers/mfd/sec-irq.c
@@ -61,14 +61,14 @@ static const struct regmap_irq s2mps11_irqs[] = {
 		.reg_offset = 1,
 		.mask = S2MPS11_IRQ_RTC60S_MASK,
 	},
-	[S2MPS11_IRQ_RTCA0] = {
-		.reg_offset = 1,
-		.mask = S2MPS11_IRQ_RTCA0_MASK,
-	},
 	[S2MPS11_IRQ_RTCA1] = {
 		.reg_offset = 1,
 		.mask = S2MPS11_IRQ_RTCA1_MASK,
 	},
+	[S2MPS11_IRQ_RTCA0] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_RTCA0_MASK,
+	},
 	[S2MPS11_IRQ_SMPL] = {
 		.reg_offset = 1,
 		.mask = S2MPS11_IRQ_SMPL_MASK,
@@ -484,6 +484,12 @@ int sec_irq_init(struct sec_pmic_dev *sec_pmic)
 		return ret;
 	}
 
+	/*
+	 * The rtc-s5m driver requests S2MPS14_IRQ_RTCA0 also for S2MPS11
+	 * so the interrupt number must be consistent.
+	 */
+	BUILD_BUG_ON(((enum s2mps14_irq)S2MPS11_IRQ_RTCA0) != S2MPS14_IRQ_RTCA0);
+
 	return 0;
 }
 
diff --git a/drivers/mfd/si476x-i2c.c b/drivers/mfd/si476x-i2c.c
index 0e4a76daf187..7f87c62d91b3 100644
--- a/drivers/mfd/si476x-i2c.c
+++ b/drivers/mfd/si476x-i2c.c
@@ -766,7 +766,7 @@ static int si476x_core_probe(struct i2c_client *client,
 			   sizeof(struct v4l2_rds_data),
 			   GFP_KERNEL);
 	if (rval) {
-		dev_err(&client->dev, "Could not alloate the FIFO\n");
+		dev_err(&client->dev, "Could not allocate the FIFO\n");
 		goto free_gpio;
 	}
 	mutex_init(&core->rds_drainer_status_lock);
diff --git a/drivers/mfd/sky81452.c b/drivers/mfd/sky81452.c
new file mode 100644
index 000000000000..b0c9b0415650
--- /dev/null
+++ b/drivers/mfd/sky81452.c
@@ -0,0 +1,108 @@
+/*
+ * sky81452.c	SKY81452 MFD driver
+ *
+ * Copyright 2014 Skyworks Solutions Inc.
+ * Author : Gyungoh Yoo <jack.yoo@skyworksinc.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/sky81452.h>
+
+static const struct regmap_config sky81452_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+static int sky81452_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	const struct sky81452_platform_data *pdata = dev_get_platdata(dev);
+	struct mfd_cell cells[2];
+	struct regmap *regmap;
+	int ret;
+
+	if (!pdata) {
+		pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+		if (!pdata)
+			return -ENOMEM;
+	}
+
+	regmap = devm_regmap_init_i2c(client, &sky81452_config);
+	if (IS_ERR(regmap)) {
+		dev_err(dev, "failed to initialize.err=%ld\n", PTR_ERR(regmap));
+		return PTR_ERR(regmap);
+	}
+
+	i2c_set_clientdata(client, regmap);
+
+	memset(cells, 0, sizeof(cells));
+	cells[0].name = "sky81452-backlight";
+	cells[0].of_compatible = "skyworks,sky81452-backlight";
+	cells[0].platform_data = pdata->bl_pdata;
+	cells[0].pdata_size = sizeof(*pdata->bl_pdata);
+	cells[1].name = "sky81452-regulator";
+	cells[1].platform_data = pdata->regulator_init_data;
+	cells[1].pdata_size = sizeof(*pdata->regulator_init_data);
+
+	ret = mfd_add_devices(dev, -1, cells, ARRAY_SIZE(cells), NULL, 0, NULL);
+	if (ret)
+		dev_err(dev, "failed to add child devices. err=%d\n", ret);
+
+	return ret;
+}
+
+static int sky81452_remove(struct i2c_client *client)
+{
+	mfd_remove_devices(&client->dev);
+	return 0;
+}
+
+static const struct i2c_device_id sky81452_ids[] = {
+	{ "sky81452" },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, sky81452_ids);
+
+#ifdef CONFIG_OF
+static const struct of_device_id sky81452_of_match[] = {
+	{ .compatible = "skyworks,sky81452", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sky81452_of_match);
+#endif
+
+static struct i2c_driver sky81452_driver = {
+	.driver = {
+		.name = "sky81452",
+		.of_match_table = of_match_ptr(sky81452_of_match),
+	},
+	.probe = sky81452_probe,
+	.remove = sky81452_remove,
+	.id_table = sky81452_ids,
+};
+
+module_i2c_driver(sky81452_driver);
+
+MODULE_DESCRIPTION("Skyworks SKY81452 MFD driver");
+MODULE_AUTHOR("Gyungoh Yoo <jack.yoo@skyworksinc.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/tc3589x.c b/drivers/mfd/tc3589x.c
index aacb3720065c..cf356395c9e9 100644
--- a/drivers/mfd/tc3589x.c
+++ b/drivers/mfd/tc3589x.c
@@ -318,7 +318,6 @@ static int tc3589x_device_init(struct tc3589x *tc3589x)
 	return ret;
 }
 
-#ifdef CONFIG_OF
 static const struct of_device_id tc3589x_match[] = {
 	/* Legacy compatible string */
 	{ .compatible = "tc3589x", .data = (void *) TC3589X_UNKNOWN },
@@ -359,14 +358,6 @@ tc3589x_of_probe(struct device *dev, enum tc3589x_version *version)
 
 	return pdata;
 }
-#else
-static inline struct tc3589x_platform_data *
-tc3589x_of_probe(struct device *dev, enum tc3589x_version *version)
-{
-	dev_err(dev, "no device tree support\n");
-	return ERR_PTR(-ENODEV);
-}
-#endif
 
 static int tc3589x_probe(struct i2c_client *i2c,
 				   const struct i2c_device_id *id)
diff --git a/drivers/mfd/ti_am335x_tscadc.c b/drivers/mfd/ti_am335x_tscadc.c
index 467c80e1c4ae..e4e4b22eebc9 100644
--- a/drivers/mfd/ti_am335x_tscadc.c
+++ b/drivers/mfd/ti_am335x_tscadc.c
@@ -68,12 +68,6 @@ static void am335x_tscadc_need_adc(struct ti_tscadc_dev *tsadc)
 	DEFINE_WAIT(wait);
 	u32 reg;
 
-	/*
-	 * disable TSC steps so it does not run while the ADC is using it. If
-	 * write 0 while it is running (it just started or was already running)
-	 * then it completes all steps that were enabled and stops then.
-	 */
-	tscadc_writel(tsadc, REG_SE, 0);
 	reg = tscadc_readl(tsadc, REG_ADCFSM);
 	if (reg & SEQ_STATUS) {
 		tsadc->adc_waiting = true;
@@ -86,8 +80,12 @@ static void am335x_tscadc_need_adc(struct ti_tscadc_dev *tsadc)
 		spin_lock_irq(&tsadc->reg_lock);
 		finish_wait(&tsadc->reg_se_wait, &wait);
 
+		/*
+		 * Sequencer should either be idle or
+		 * busy applying the charge step.
+		 */
 		reg = tscadc_readl(tsadc, REG_ADCFSM);
-		WARN_ON(reg & SEQ_STATUS);
+		WARN_ON((reg & SEQ_STATUS) && !(reg & CHARGE_STEP));
 		tsadc->adc_waiting = false;
 	}
 	tsadc->adc_in_use = true;
@@ -96,7 +94,6 @@ static void am335x_tscadc_need_adc(struct ti_tscadc_dev *tsadc)
 void am335x_tsc_se_set_once(struct ti_tscadc_dev *tsadc, u32 val)
 {
 	spin_lock_irq(&tsadc->reg_lock);
-	tsadc->reg_se_cache |= val;
 	am335x_tscadc_need_adc(tsadc);
 
 	tscadc_writel(tsadc, REG_SE, val);
diff --git a/drivers/mfd/tps65010.c b/drivers/mfd/tps65010.c
index 743fb524fc8a..448f0a182dc4 100644
--- a/drivers/mfd/tps65010.c
+++ b/drivers/mfd/tps65010.c
@@ -515,7 +515,7 @@ static int tps65010_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static struct tps65010 *the_tps;
 
-static int __exit tps65010_remove(struct i2c_client *client)
+static int tps65010_remove(struct i2c_client *client)
 {
 	struct tps65010		*tps = i2c_get_clientdata(client);
 	struct tps65010_board	*board = dev_get_platdata(&client->dev);
@@ -684,7 +684,7 @@ static struct i2c_driver tps65010_driver = {
 		.name	= "tps65010",
 	},
 	.probe	= tps65010_probe,
-	.remove	= __exit_p(tps65010_remove),
+	.remove	= tps65010_remove,
 	.id_table = tps65010_id,
 };
 
diff --git a/drivers/mfd/twl4030-power.c b/drivers/mfd/twl4030-power.c
index 393509246037..f440aed61305 100644
--- a/drivers/mfd/twl4030-power.c
+++ b/drivers/mfd/twl4030-power.c
@@ -829,7 +829,7 @@ static struct twl4030_power_data osc_off_idle = {
 	.board_config		= osc_off_rconfig,
 };
 
-static struct of_device_id twl4030_power_of_match[] = {
+static const struct of_device_id twl4030_power_of_match[] = {
 	{
 		.compatible = "ti,twl4030-power",
 	},
diff --git a/drivers/mfd/twl6040.c b/drivers/mfd/twl6040.c
index f71ee3dbc2a2..c5265c1262c5 100644
--- a/drivers/mfd/twl6040.c
+++ b/drivers/mfd/twl6040.c
@@ -814,4 +814,3 @@ MODULE_DESCRIPTION("TWL6040 MFD");
 MODULE_AUTHOR("Misael Lopez Cruz <misael.lopez@ti.com>");
 MODULE_AUTHOR("Jorge Eduardo Candelaria <jorge.candelaria@ti.com>");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:twl6040");
diff --git a/drivers/mfd/vexpress-sysreg.c b/drivers/mfd/vexpress-sysreg.c
index 8f43ab8fd2d6..3e628df9280c 100644
--- a/drivers/mfd/vexpress-sysreg.c
+++ b/drivers/mfd/vexpress-sysreg.c
@@ -47,71 +47,26 @@
 #define SYS_HBI_MASK		0xfff
 #define SYS_PROCIDx_HBI_SHIFT	0
 
-#define SYS_MCI_CARDIN		(1 << 0)
-#define SYS_MCI_WPROT		(1 << 1)
-
 #define SYS_MISC_MASTERSITE	(1 << 14)
 
-
-static void __iomem *__vexpress_sysreg_base;
-
-static void __iomem *vexpress_sysreg_base(void)
+void vexpress_flags_set(u32 data)
 {
-	if (!__vexpress_sysreg_base) {
+	static void __iomem *base;
+
+	if (!base) {
 		struct device_node *node = of_find_compatible_node(NULL, NULL,
 				"arm,vexpress-sysreg");
 
-		__vexpress_sysreg_base = of_iomap(node, 0);
+		base = of_iomap(node, 0);
 	}
 
-	WARN_ON(!__vexpress_sysreg_base);
-
-	return __vexpress_sysreg_base;
-}
-
-
-static int vexpress_sysreg_get_master(void)
-{
-	if (readl(vexpress_sysreg_base() + SYS_MISC) & SYS_MISC_MASTERSITE)
-		return VEXPRESS_SITE_DB2;
-
-	return VEXPRESS_SITE_DB1;
-}
-
-void vexpress_flags_set(u32 data)
-{
-	writel(~0, vexpress_sysreg_base() + SYS_FLAGSCLR);
-	writel(data, vexpress_sysreg_base() + SYS_FLAGSSET);
-}
-
-unsigned int vexpress_get_mci_cardin(struct device *dev)
-{
-	return readl(vexpress_sysreg_base() + SYS_MCI) & SYS_MCI_CARDIN;
-}
-
-u32 vexpress_get_procid(int site)
-{
-	if (site == VEXPRESS_SITE_MASTER)
-		site = vexpress_sysreg_get_master();
+	if (WARN_ON(!base))
+		return;
 
-	return readl(vexpress_sysreg_base() + (site == VEXPRESS_SITE_DB1 ?
-			SYS_PROCID0 : SYS_PROCID1));
+	writel(~0, base + SYS_FLAGSCLR);
+	writel(data, base + SYS_FLAGSSET);
 }
 
-void __iomem *vexpress_get_24mhz_clock_base(void)
-{
-	return vexpress_sysreg_base() + SYS_24MHZ;
-}
-
-
-void __init vexpress_sysreg_early_init(void __iomem *base)
-{
-	__vexpress_sysreg_base = base;
-
-	vexpress_config_set_master(vexpress_sysreg_get_master());
-}
-
-
 /* The sysreg block is just a random collection of various functions... */
 
 static struct syscon_platform_data vexpress_sysreg_sys_id_pdata = {
@@ -210,6 +165,7 @@ static int vexpress_sysreg_probe(struct platform_device *pdev)
 	struct resource *mem;
 	void __iomem *base;
 	struct bgpio_chip *mmc_gpio_chip;
+	int master;
 	u32 dt_hbi;
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -220,11 +176,14 @@ static int vexpress_sysreg_probe(struct platform_device *pdev)
 	if (!base)
 		return -ENOMEM;
 
-	vexpress_config_set_master(vexpress_sysreg_get_master());
+	master = readl(base + SYS_MISC) & SYS_MISC_MASTERSITE ?
+			VEXPRESS_SITE_DB2 : VEXPRESS_SITE_DB1;
+	vexpress_config_set_master(master);
 
 	/* Confirm board type against DT property, if available */
 	if (of_property_read_u32(of_root, "arm,hbi", &dt_hbi) == 0) {
-		u32 id = vexpress_get_procid(VEXPRESS_SITE_MASTER);
+		u32 id = readl(base + (master == VEXPRESS_SITE_DB1 ?
+				 SYS_PROCID0 : SYS_PROCID1));
 		u32 hbi = (id >> SYS_PROCIDx_HBI_SHIFT) & SYS_HBI_MASK;
 
 		if (WARN_ON(dt_hbi != hbi))
diff --git a/drivers/mfd/wm5102-tables.c b/drivers/mfd/wm5102-tables.c
index b326a82017ee..aeae6ec123b3 100644
--- a/drivers/mfd/wm5102-tables.c
+++ b/drivers/mfd/wm5102-tables.c
@@ -1172,9 +1172,6 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_DAC_DIGITAL_VOLUME_3L:
 	case ARIZONA_DAC_VOLUME_LIMIT_3L:
 	case ARIZONA_NOISE_GATE_SELECT_3L:
-	case ARIZONA_OUTPUT_PATH_CONFIG_3R:
-	case ARIZONA_DAC_DIGITAL_VOLUME_3R:
-	case ARIZONA_DAC_VOLUME_LIMIT_3R:
 	case ARIZONA_OUTPUT_PATH_CONFIG_4L:
 	case ARIZONA_DAC_DIGITAL_VOLUME_4L:
 	case ARIZONA_OUT_VOLUME_4L:
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/amthif.c b/drivers/misc/mei/amthif.c
index c4cb9a984a5f..40ea639fa413 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -19,7 +19,6 @@
 #include <linux/errno.h>
 #include <linux/types.h>
 #include <linux/fcntl.h>
-#include <linux/aio.h>
 #include <linux/ioctl.h>
 #include <linux/cdev.h>
 #include <linux/list.h>
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 3c019c0e60eb..47680c84801c 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -22,7 +22,6 @@
 #include <linux/errno.h>
 #include <linux/types.h>
 #include <linux/fcntl.h>
-#include <linux/aio.h>
 #include <linux/poll.h>
 #include <linux/init.h>
 #include <linux/ioctl.h>
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index bd3039ab8f98..af44ee26075d 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -21,7 +21,6 @@
 #include <linux/errno.h>
 #include <linux/types.h>
 #include <linux/fcntl.h>
-#include <linux/aio.h>
 #include <linux/pci.h>
 #include <linux/poll.h>
 #include <linux/ioctl.h>
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..f36c76f8b232 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;
@@ -1750,7 +1758,7 @@ static int mmc_runtime_suspend(struct mmc_host *host)
 
 	err = _mmc_suspend(host, true);
 	if (err)
-		pr_err("%s: error %d doing aggessive suspend\n",
+		pr_err("%s: error %d doing aggressive suspend\n",
 			mmc_hostname(host), err);
 
 	return err;
@@ -1768,7 +1776,7 @@ static int mmc_runtime_resume(struct mmc_host *host)
 
 	err = _mmc_resume(host);
 	if (err)
-		pr_err("%s: error %d doing aggessive resume\n",
+		pr_err("%s: error %d doing aggressive resume\n",
 			mmc_hostname(host), err);
 
 	return 0;
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/sd.c b/drivers/mmc/core/sd.c
index ad4d43eae99d..31a9ef256d06 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -1157,7 +1157,7 @@ static int mmc_sd_runtime_suspend(struct mmc_host *host)
 
 	err = _mmc_sd_suspend(host);
 	if (err)
-		pr_err("%s: error %d doing aggessive suspend\n",
+		pr_err("%s: error %d doing aggressive suspend\n",
 			mmc_hostname(host), err);
 
 	return err;
@@ -1175,7 +1175,7 @@ static int mmc_sd_runtime_resume(struct mmc_host *host)
 
 	err = _mmc_sd_resume(host);
 	if (err)
-		pr_err("%s: error %d doing aggessive resume\n",
+		pr_err("%s: error %d doing aggressive resume\n",
 			mmc_hostname(host), err);
 
 	return 0;
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/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index 27f272ed502a..43fa16b5f510 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -1105,7 +1105,7 @@ int gpmi_is_ready(struct gpmi_nand_data *this, unsigned chip)
 		mask = MX28_BF_GPMI_STAT_READY_BUSY(1 << chip);
 		reg = readl(r->gpmi_regs + HW_GPMI_STAT);
 	} else
-		dev_err(this->dev, "unknow arch.\n");
+		dev_err(this->dev, "unknown arch.\n");
 	return reg & mask;
 }
 
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/ubi/attach.c b/drivers/mtd/ubi/attach.c
index 9d2e16f3150a..68eea5befaf1 100644
--- a/drivers/mtd/ubi/attach.c
+++ b/drivers/mtd/ubi/attach.c
@@ -410,7 +410,7 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 		second_is_newer = !second_is_newer;
 	} else {
 		dbg_bld("PEB %d CRC is OK", pnum);
-		bitflips = !!err;
+		bitflips |= !!err;
 	}
 	mutex_unlock(&ubi->buf_mutex);
 
@@ -1301,6 +1301,30 @@ out_ech:
 	return err;
 }
 
+static struct ubi_attach_info *alloc_ai(void)
+{
+	struct ubi_attach_info *ai;
+
+	ai = kzalloc(sizeof(struct ubi_attach_info), GFP_KERNEL);
+	if (!ai)
+		return ai;
+
+	INIT_LIST_HEAD(&ai->corr);
+	INIT_LIST_HEAD(&ai->free);
+	INIT_LIST_HEAD(&ai->erase);
+	INIT_LIST_HEAD(&ai->alien);
+	ai->volumes = RB_ROOT;
+	ai->aeb_slab_cache = kmem_cache_create("ubi_aeb_slab_cache",
+					       sizeof(struct ubi_ainf_peb),
+					       0, 0, NULL);
+	if (!ai->aeb_slab_cache) {
+		kfree(ai);
+		ai = NULL;
+	}
+
+	return ai;
+}
+
 #ifdef CONFIG_MTD_UBI_FASTMAP
 
 /**
@@ -1313,7 +1337,7 @@ out_ech:
  * UBI_NO_FASTMAP denotes that no fastmap was found.
  * UBI_BAD_FASTMAP denotes that the found fastmap was invalid.
  */
-static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info *ai)
+static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info **ai)
 {
 	int err, pnum, fm_anchor = -1;
 	unsigned long long max_sqnum = 0;
@@ -1334,7 +1358,7 @@ static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		cond_resched();
 
 		dbg_gen("process PEB %d", pnum);
-		err = scan_peb(ubi, ai, pnum, &vol_id, &sqnum);
+		err = scan_peb(ubi, *ai, pnum, &vol_id, &sqnum);
 		if (err < 0)
 			goto out_vidh;
 
@@ -1350,7 +1374,12 @@ static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	if (fm_anchor < 0)
 		return UBI_NO_FASTMAP;
 
-	return ubi_scan_fastmap(ubi, ai, fm_anchor);
+	destroy_ai(*ai);
+	*ai = alloc_ai();
+	if (!*ai)
+		return -ENOMEM;
+
+	return ubi_scan_fastmap(ubi, *ai, fm_anchor);
 
 out_vidh:
 	ubi_free_vid_hdr(ubi, vidh);
@@ -1362,30 +1391,6 @@ out:
 
 #endif
 
-static struct ubi_attach_info *alloc_ai(const char *slab_name)
-{
-	struct ubi_attach_info *ai;
-
-	ai = kzalloc(sizeof(struct ubi_attach_info), GFP_KERNEL);
-	if (!ai)
-		return ai;
-
-	INIT_LIST_HEAD(&ai->corr);
-	INIT_LIST_HEAD(&ai->free);
-	INIT_LIST_HEAD(&ai->erase);
-	INIT_LIST_HEAD(&ai->alien);
-	ai->volumes = RB_ROOT;
-	ai->aeb_slab_cache = kmem_cache_create(slab_name,
-					       sizeof(struct ubi_ainf_peb),
-					       0, 0, NULL);
-	if (!ai->aeb_slab_cache) {
-		kfree(ai);
-		ai = NULL;
-	}
-
-	return ai;
-}
-
 /**
  * ubi_attach - attach an MTD device.
  * @ubi: UBI device descriptor
@@ -1399,7 +1404,7 @@ int ubi_attach(struct ubi_device *ubi, int force_scan)
 	int err;
 	struct ubi_attach_info *ai;
 
-	ai = alloc_ai("ubi_aeb_slab_cache");
+	ai = alloc_ai();
 	if (!ai)
 		return -ENOMEM;
 
@@ -1413,11 +1418,11 @@ int ubi_attach(struct ubi_device *ubi, int force_scan)
 	if (force_scan)
 		err = scan_all(ubi, ai, 0);
 	else {
-		err = scan_fast(ubi, ai);
-		if (err > 0) {
+		err = scan_fast(ubi, &ai);
+		if (err > 0 || mtd_is_eccerr(err)) {
 			if (err != UBI_NO_FASTMAP) {
 				destroy_ai(ai);
-				ai = alloc_ai("ubi_aeb_slab_cache2");
+				ai = alloc_ai();
 				if (!ai)
 					return -ENOMEM;
 
@@ -1453,10 +1458,10 @@ int ubi_attach(struct ubi_device *ubi, int force_scan)
 		goto out_wl;
 
 #ifdef CONFIG_MTD_UBI_FASTMAP
-	if (ubi->fm && ubi_dbg_chk_gen(ubi)) {
+	if (ubi->fm && ubi_dbg_chk_fastmap(ubi)) {
 		struct ubi_attach_info *scan_ai;
 
-		scan_ai = alloc_ai("ubi_ckh_aeb_slab_cache");
+		scan_ai = alloc_ai();
 		if (!scan_ai) {
 			err = -ENOMEM;
 			goto out_wl;
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index ba01a8d22d28..9690cf9aaef5 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -81,6 +81,7 @@ static struct mtd_dev_param __initdata mtd_dev_param[UBI_MAX_DEVICES];
 #ifdef CONFIG_MTD_UBI_FASTMAP
 /* UBI module parameter to enable fastmap automatically on non-fastmap images */
 static bool fm_autoconvert;
+static bool fm_debug;
 #endif
 /* Root UBI "class" object (corresponds to '/<sysfs>/class/ubi/') */
 struct class *ubi_class;
@@ -154,23 +155,22 @@ static struct device_attribute dev_mtd_num =
  */
 int ubi_volume_notify(struct ubi_device *ubi, struct ubi_volume *vol, int ntype)
 {
+	int ret;
 	struct ubi_notification nt;
 
 	ubi_do_get_device_info(ubi, &nt.di);
 	ubi_do_get_volume_info(ubi, vol, &nt.vi);
 
-#ifdef CONFIG_MTD_UBI_FASTMAP
 	switch (ntype) {
 	case UBI_VOLUME_ADDED:
 	case UBI_VOLUME_REMOVED:
 	case UBI_VOLUME_RESIZED:
 	case UBI_VOLUME_RENAMED:
-		if (ubi_update_fastmap(ubi)) {
-			ubi_err(ubi, "Unable to update fastmap!");
-			ubi_ro_mode(ubi);
-		}
+		ret = ubi_update_fastmap(ubi);
+		if (ret)
+			ubi_msg(ubi, "Unable to write a new fastmap: %i", ret);
 	}
-#endif
+
 	return blocking_notifier_call_chain(&ubi_notifiers, ntype, &nt);
 }
 
@@ -950,8 +950,10 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	if (ubi->fm_pool.max_size < UBI_FM_MIN_POOL_SIZE)
 		ubi->fm_pool.max_size = UBI_FM_MIN_POOL_SIZE;
 
-	ubi->fm_wl_pool.max_size = UBI_FM_WL_POOL_SIZE;
+	ubi->fm_wl_pool.max_size = ubi->fm_pool.max_size / 2;
 	ubi->fm_disabled = !fm_autoconvert;
+	if (fm_debug)
+		ubi_enable_dbg_chk_fastmap(ubi);
 
 	if (!ubi->fm_disabled && (int)mtd_div_by_eb(ubi->mtd->size, ubi->mtd)
 	    <= UBI_FM_MAX_START) {
@@ -970,8 +972,8 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	mutex_init(&ubi->ckvol_mutex);
 	mutex_init(&ubi->device_mutex);
 	spin_lock_init(&ubi->volumes_lock);
-	mutex_init(&ubi->fm_mutex);
-	init_rwsem(&ubi->fm_sem);
+	init_rwsem(&ubi->fm_protect);
+	init_rwsem(&ubi->fm_eba_sem);
 
 	ubi_msg(ubi, "attaching mtd%d", mtd->index);
 
@@ -1115,8 +1117,11 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 	ubi_msg(ubi, "detaching mtd%d", ubi->mtd->index);
 #ifdef CONFIG_MTD_UBI_FASTMAP
 	/* If we don't write a new fastmap at detach time we lose all
-	 * EC updates that have been made since the last written fastmap. */
-	ubi_update_fastmap(ubi);
+	 * EC updates that have been made since the last written fastmap.
+	 * In case of fastmap debugging we omit the update to simulate an
+	 * unclean shutdown. */
+	if (!ubi_dbg_chk_fastmap(ubi))
+		ubi_update_fastmap(ubi);
 #endif
 	/*
 	 * Before freeing anything, we have to stop the background thread to
@@ -1501,6 +1506,8 @@ MODULE_PARM_DESC(mtd, "MTD devices to attach. Parameter format: mtd=<name|num|pa
 #ifdef CONFIG_MTD_UBI_FASTMAP
 module_param(fm_autoconvert, bool, 0644);
 MODULE_PARM_DESC(fm_autoconvert, "Set this parameter to enable fastmap automatically on images without a fastmap.");
+module_param(fm_debug, bool, 0);
+MODULE_PARM_DESC(fm_debug, "Set this parameter to enable fastmap debugging by default. Warning, this will make fastmap slow!");
 #endif
 MODULE_VERSION(__stringify(UBI_VERSION));
 MODULE_DESCRIPTION("UBI - Unsorted Block Images");
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index d647e504f9b1..d16fccf79179 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -455,7 +455,7 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 		/* Validate the request */
 		err = -EINVAL;
 		if (req.lnum < 0 || req.lnum >= vol->reserved_pebs ||
-		    req.bytes < 0 || req.lnum >= vol->usable_leb_size)
+		    req.bytes < 0 || req.bytes > vol->usable_leb_size)
 			break;
 
 		err = get_exclusive(desc);
diff --git a/drivers/mtd/ubi/debug.c b/drivers/mtd/ubi/debug.c
index 7335c9ff9d99..b077e43b5ba9 100644
--- a/drivers/mtd/ubi/debug.c
+++ b/drivers/mtd/ubi/debug.c
@@ -263,7 +263,7 @@ static ssize_t dfs_file_read(struct file *file, char __user *user_buf,
 	struct dentry *dent = file->f_path.dentry;
 	struct ubi_device *ubi;
 	struct ubi_debug_info *d;
-	char buf[3];
+	char buf[8];
 	int val;
 
 	ubi = ubi_get_device(ubi_num);
@@ -275,12 +275,30 @@ static ssize_t dfs_file_read(struct file *file, char __user *user_buf,
 		val = d->chk_gen;
 	else if (dent == d->dfs_chk_io)
 		val = d->chk_io;
+	else if (dent == d->dfs_chk_fastmap)
+		val = d->chk_fastmap;
 	else if (dent == d->dfs_disable_bgt)
 		val = d->disable_bgt;
 	else if (dent == d->dfs_emulate_bitflips)
 		val = d->emulate_bitflips;
 	else if (dent == d->dfs_emulate_io_failures)
 		val = d->emulate_io_failures;
+	else if (dent == d->dfs_emulate_power_cut) {
+		snprintf(buf, sizeof(buf), "%u\n", d->emulate_power_cut);
+		count = simple_read_from_buffer(user_buf, count, ppos,
+						buf, strlen(buf));
+		goto out;
+	} else if (dent == d->dfs_power_cut_min) {
+		snprintf(buf, sizeof(buf), "%u\n", d->power_cut_min);
+		count = simple_read_from_buffer(user_buf, count, ppos,
+						buf, strlen(buf));
+		goto out;
+	} else if (dent == d->dfs_power_cut_max) {
+		snprintf(buf, sizeof(buf), "%u\n", d->power_cut_max);
+		count = simple_read_from_buffer(user_buf, count, ppos,
+						buf, strlen(buf));
+		goto out;
+	}
 	else {
 		count = -EINVAL;
 		goto out;
@@ -309,7 +327,7 @@ static ssize_t dfs_file_write(struct file *file, const char __user *user_buf,
 	struct ubi_device *ubi;
 	struct ubi_debug_info *d;
 	size_t buf_size;
-	char buf[8];
+	char buf[8] = {0};
 	int val;
 
 	ubi = ubi_get_device(ubi_num);
@@ -323,6 +341,21 @@ static ssize_t dfs_file_write(struct file *file, const char __user *user_buf,
 		goto out;
 	}
 
+	if (dent == d->dfs_power_cut_min) {
+		if (kstrtouint(buf, 0, &d->power_cut_min) != 0)
+			count = -EINVAL;
+		goto out;
+	} else if (dent == d->dfs_power_cut_max) {
+		if (kstrtouint(buf, 0, &d->power_cut_max) != 0)
+			count = -EINVAL;
+		goto out;
+	} else if (dent == d->dfs_emulate_power_cut) {
+		if (kstrtoint(buf, 0, &val) != 0)
+			count = -EINVAL;
+		d->emulate_power_cut = val;
+		goto out;
+	}
+
 	if (buf[0] == '1')
 		val = 1;
 	else if (buf[0] == '0')
@@ -336,6 +369,8 @@ static ssize_t dfs_file_write(struct file *file, const char __user *user_buf,
 		d->chk_gen = val;
 	else if (dent == d->dfs_chk_io)
 		d->chk_io = val;
+	else if (dent == d->dfs_chk_fastmap)
+		d->chk_fastmap = val;
 	else if (dent == d->dfs_disable_bgt)
 		d->disable_bgt = val;
 	else if (dent == d->dfs_emulate_bitflips)
@@ -406,6 +441,13 @@ int ubi_debugfs_init_dev(struct ubi_device *ubi)
 		goto out_remove;
 	d->dfs_chk_io = dent;
 
+	fname = "chk_fastmap";
+	dent = debugfs_create_file(fname, S_IWUSR, d->dfs_dir, (void *)ubi_num,
+				   &dfs_fops);
+	if (IS_ERR_OR_NULL(dent))
+		goto out_remove;
+	d->dfs_chk_fastmap = dent;
+
 	fname = "tst_disable_bgt";
 	dent = debugfs_create_file(fname, S_IWUSR, d->dfs_dir, (void *)ubi_num,
 				   &dfs_fops);
@@ -427,6 +469,27 @@ int ubi_debugfs_init_dev(struct ubi_device *ubi)
 		goto out_remove;
 	d->dfs_emulate_io_failures = dent;
 
+	fname = "tst_emulate_power_cut";
+	dent = debugfs_create_file(fname, S_IWUSR, d->dfs_dir, (void *)ubi_num,
+				   &dfs_fops);
+	if (IS_ERR_OR_NULL(dent))
+		goto out_remove;
+	d->dfs_emulate_power_cut = dent;
+
+	fname = "tst_emulate_power_cut_min";
+	dent = debugfs_create_file(fname, S_IWUSR, d->dfs_dir, (void *)ubi_num,
+				   &dfs_fops);
+	if (IS_ERR_OR_NULL(dent))
+		goto out_remove;
+	d->dfs_power_cut_min = dent;
+
+	fname = "tst_emulate_power_cut_max";
+	dent = debugfs_create_file(fname, S_IWUSR, d->dfs_dir, (void *)ubi_num,
+				   &dfs_fops);
+	if (IS_ERR_OR_NULL(dent))
+		goto out_remove;
+	d->dfs_power_cut_max = dent;
+
 	return 0;
 
 out_remove:
@@ -447,3 +510,36 @@ void ubi_debugfs_exit_dev(struct ubi_device *ubi)
 	if (IS_ENABLED(CONFIG_DEBUG_FS))
 		debugfs_remove_recursive(ubi->dbg.dfs_dir);
 }
+
+/**
+ * ubi_dbg_power_cut - emulate a power cut if it is time to do so
+ * @ubi: UBI device description object
+ * @caller: Flags set to indicate from where the function is being called
+ *
+ * Returns non-zero if a power cut was emulated, zero if not.
+ */
+int ubi_dbg_power_cut(struct ubi_device *ubi, int caller)
+{
+	unsigned int range;
+
+	if ((ubi->dbg.emulate_power_cut & caller) == 0)
+		return 0;
+
+	if (ubi->dbg.power_cut_counter == 0) {
+		ubi->dbg.power_cut_counter = ubi->dbg.power_cut_min;
+
+		if (ubi->dbg.power_cut_max > ubi->dbg.power_cut_min) {
+			range = ubi->dbg.power_cut_max - ubi->dbg.power_cut_min;
+			ubi->dbg.power_cut_counter += prandom_u32() % range;
+		}
+		return 0;
+	}
+
+	ubi->dbg.power_cut_counter--;
+	if (ubi->dbg.power_cut_counter)
+		return 0;
+
+	ubi_msg(ubi, "XXXXXXXXXXXXXXX emulating a power cut XXXXXXXXXXXXXXXX");
+	ubi_ro_mode(ubi);
+	return 1;
+}
diff --git a/drivers/mtd/ubi/debug.h b/drivers/mtd/ubi/debug.h
index cba89fcd1587..eb8985e5c178 100644
--- a/drivers/mtd/ubi/debug.h
+++ b/drivers/mtd/ubi/debug.h
@@ -127,4 +127,16 @@ static inline int ubi_dbg_chk_gen(const struct ubi_device *ubi)
 {
 	return ubi->dbg.chk_gen;
 }
+
+static inline int ubi_dbg_chk_fastmap(const struct ubi_device *ubi)
+{
+	return ubi->dbg.chk_fastmap;
+}
+
+static inline void ubi_enable_dbg_chk_fastmap(struct ubi_device *ubi)
+{
+	ubi->dbg.chk_fastmap = 1;
+}
+
+int ubi_dbg_power_cut(struct ubi_device *ubi, int caller);
 #endif /* !__UBI_DEBUG_H__ */
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index da4c79259f67..51bca035cd83 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -340,9 +340,9 @@ int ubi_eba_unmap_leb(struct ubi_device *ubi, struct ubi_volume *vol,
 
 	dbg_eba("erase LEB %d:%d, PEB %d", vol_id, lnum, pnum);
 
-	down_read(&ubi->fm_sem);
+	down_read(&ubi->fm_eba_sem);
 	vol->eba_tbl[lnum] = UBI_LEB_UNMAPPED;
-	up_read(&ubi->fm_sem);
+	up_read(&ubi->fm_eba_sem);
 	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 0);
 
 out_unlock:
@@ -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)
@@ -566,6 +567,7 @@ retry:
 	new_pnum = ubi_wl_get_peb(ubi);
 	if (new_pnum < 0) {
 		ubi_free_vid_hdr(ubi, vid_hdr);
+		up_read(&ubi->fm_eba_sem);
 		return new_pnum;
 	}
 
@@ -576,13 +578,16 @@ retry:
 	if (err && err != UBI_IO_BITFLIPS) {
 		if (err > 0)
 			err = -EIO;
+		up_read(&ubi->fm_eba_sem);
 		goto out_put;
 	}
 
 	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	err = ubi_io_write_vid_hdr(ubi, new_pnum, vid_hdr);
-	if (err)
+	if (err) {
+		up_read(&ubi->fm_eba_sem);
 		goto write_error;
+	}
 
 	data_size = offset + len;
 	mutex_lock(&ubi->buf_mutex);
@@ -591,8 +596,10 @@ retry:
 	/* Read everything before the area where the write failure happened */
 	if (offset > 0) {
 		err = ubi_io_read_data(ubi, ubi->peb_buf, pnum, 0, offset);
-		if (err && err != UBI_IO_BITFLIPS)
+		if (err && err != UBI_IO_BITFLIPS) {
+			up_read(&ubi->fm_eba_sem);
 			goto out_unlock;
+		}
 	}
 
 	memcpy(ubi->peb_buf + offset, buf, len);
@@ -600,15 +607,15 @@ retry:
 	err = ubi_io_write_data(ubi, ubi->peb_buf, new_pnum, 0, data_size);
 	if (err) {
 		mutex_unlock(&ubi->buf_mutex);
+		up_read(&ubi->fm_eba_sem);
 		goto write_error;
 	}
 
 	mutex_unlock(&ubi->buf_mutex);
 	ubi_free_vid_hdr(ubi, vid_hdr);
 
-	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = new_pnum;
-	up_read(&ubi->fm_sem);
+	up_read(&ubi->fm_eba_sem);
 	ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 
 	ubi_msg(ubi, "data was successfully recovered");
@@ -703,6 +710,7 @@ retry:
 	if (pnum < 0) {
 		ubi_free_vid_hdr(ubi, vid_hdr);
 		leb_write_unlock(ubi, vol_id, lnum);
+		up_read(&ubi->fm_eba_sem);
 		return pnum;
 	}
 
@@ -713,6 +721,7 @@ retry:
 	if (err) {
 		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
 			 vol_id, lnum, pnum);
+		up_read(&ubi->fm_eba_sem);
 		goto write_error;
 	}
 
@@ -721,13 +730,13 @@ retry:
 		if (err) {
 			ubi_warn(ubi, "failed to write %d bytes at offset %d of LEB %d:%d, PEB %d",
 				 len, offset, vol_id, lnum, pnum);
+			up_read(&ubi->fm_eba_sem);
 			goto write_error;
 		}
 	}
 
-	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = pnum;
-	up_read(&ubi->fm_sem);
+	up_read(&ubi->fm_eba_sem);
 
 	leb_write_unlock(ubi, vol_id, lnum);
 	ubi_free_vid_hdr(ubi, vid_hdr);
@@ -824,6 +833,7 @@ retry:
 	if (pnum < 0) {
 		ubi_free_vid_hdr(ubi, vid_hdr);
 		leb_write_unlock(ubi, vol_id, lnum);
+		up_read(&ubi->fm_eba_sem);
 		return pnum;
 	}
 
@@ -834,6 +844,7 @@ retry:
 	if (err) {
 		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
 			 vol_id, lnum, pnum);
+		up_read(&ubi->fm_eba_sem);
 		goto write_error;
 	}
 
@@ -841,13 +852,13 @@ retry:
 	if (err) {
 		ubi_warn(ubi, "failed to write %d bytes of data to PEB %d",
 			 len, pnum);
+		up_read(&ubi->fm_eba_sem);
 		goto write_error;
 	}
 
 	ubi_assert(vol->eba_tbl[lnum] < 0);
-	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = pnum;
-	up_read(&ubi->fm_sem);
+	up_read(&ubi->fm_eba_sem);
 
 	leb_write_unlock(ubi, vol_id, lnum);
 	ubi_free_vid_hdr(ubi, vid_hdr);
@@ -899,7 +910,7 @@ write_error:
 int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 			      int lnum, const void *buf, int len)
 {
-	int err, pnum, tries = 0, vol_id = vol->vol_id;
+	int err, pnum, old_pnum, tries = 0, vol_id = vol->vol_id;
 	struct ubi_vid_hdr *vid_hdr;
 	uint32_t crc;
 
@@ -942,6 +953,7 @@ retry:
 	pnum = ubi_wl_get_peb(ubi);
 	if (pnum < 0) {
 		err = pnum;
+		up_read(&ubi->fm_eba_sem);
 		goto out_leb_unlock;
 	}
 
@@ -952,6 +964,7 @@ retry:
 	if (err) {
 		ubi_warn(ubi, "failed to write VID header to LEB %d:%d, PEB %d",
 			 vol_id, lnum, pnum);
+		up_read(&ubi->fm_eba_sem);
 		goto write_error;
 	}
 
@@ -959,19 +972,20 @@ retry:
 	if (err) {
 		ubi_warn(ubi, "failed to write %d bytes of data to PEB %d",
 			 len, pnum);
+		up_read(&ubi->fm_eba_sem);
 		goto write_error;
 	}
 
-	if (vol->eba_tbl[lnum] >= 0) {
-		err = ubi_wl_put_peb(ubi, vol_id, lnum, vol->eba_tbl[lnum], 0);
+	old_pnum = vol->eba_tbl[lnum];
+	vol->eba_tbl[lnum] = pnum;
+	up_read(&ubi->fm_eba_sem);
+
+	if (old_pnum >= 0) {
+		err = ubi_wl_put_peb(ubi, vol_id, lnum, old_pnum, 0);
 		if (err)
 			goto out_leb_unlock;
 	}
 
-	down_read(&ubi->fm_sem);
-	vol->eba_tbl[lnum] = pnum;
-	up_read(&ubi->fm_sem);
-
 out_leb_unlock:
 	leb_write_unlock(ubi, vol_id, lnum);
 out_mutex:
@@ -1217,9 +1231,9 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	}
 
 	ubi_assert(vol->eba_tbl[lnum] == from);
-	down_read(&ubi->fm_sem);
+	down_read(&ubi->fm_eba_sem);
 	vol->eba_tbl[lnum] = to;
-	up_read(&ubi->fm_sem);
+	up_read(&ubi->fm_eba_sem);
 
 out_unlock_buf:
 	mutex_unlock(&ubi->buf_mutex);
@@ -1418,7 +1432,8 @@ int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 				 * during re-size.
 				 */
 				ubi_move_aeb_to_list(av, aeb, &ai->erase);
-			vol->eba_tbl[aeb->lnum] = aeb->pnum;
+			else
+				vol->eba_tbl[aeb->lnum] = aeb->pnum;
 		}
 	}
 
diff --git a/drivers/mtd/ubi/fastmap-wl.c b/drivers/mtd/ubi/fastmap-wl.c
new file mode 100644
index 000000000000..b2a665398bca
--- /dev/null
+++ b/drivers/mtd/ubi/fastmap-wl.c
@@ -0,0 +1,362 @@
+/*
+ * Copyright (c) 2012 Linutronix GmbH
+ * Copyright (c) 2014 sigma star gmbh
+ * Author: Richard Weinberger <richard@nod.at>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
+ * the GNU General Public License for more details.
+ *
+ */
+
+/**
+ * update_fastmap_work_fn - calls ubi_update_fastmap from a work queue
+ * @wrk: the work description object
+ */
+static void update_fastmap_work_fn(struct work_struct *wrk)
+{
+	struct ubi_device *ubi = container_of(wrk, struct ubi_device, fm_work);
+
+	ubi_update_fastmap(ubi);
+	spin_lock(&ubi->wl_lock);
+	ubi->fm_work_scheduled = 0;
+	spin_unlock(&ubi->wl_lock);
+}
+
+/**
+ * find_anchor_wl_entry - find wear-leveling entry to used as anchor PEB.
+ * @root: the RB-tree where to look for
+ */
+static struct ubi_wl_entry *find_anchor_wl_entry(struct rb_root *root)
+{
+	struct rb_node *p;
+	struct ubi_wl_entry *e, *victim = NULL;
+	int max_ec = UBI_MAX_ERASECOUNTER;
+
+	ubi_rb_for_each_entry(p, e, root, u.rb) {
+		if (e->pnum < UBI_FM_MAX_START && e->ec < max_ec) {
+			victim = e;
+			max_ec = e->ec;
+		}
+	}
+
+	return victim;
+}
+
+/**
+ * return_unused_pool_pebs - returns unused PEB to the free tree.
+ * @ubi: UBI device description object
+ * @pool: fastmap pool description object
+ */
+static void return_unused_pool_pebs(struct ubi_device *ubi,
+				    struct ubi_fm_pool *pool)
+{
+	int i;
+	struct ubi_wl_entry *e;
+
+	for (i = pool->used; i < pool->size; i++) {
+		e = ubi->lookuptbl[pool->pebs[i]];
+		wl_tree_add(e, &ubi->free);
+		ubi->free_count++;
+	}
+}
+
+static int anchor_pebs_avalible(struct rb_root *root)
+{
+	struct rb_node *p;
+	struct ubi_wl_entry *e;
+
+	ubi_rb_for_each_entry(p, e, root, u.rb)
+		if (e->pnum < UBI_FM_MAX_START)
+			return 1;
+
+	return 0;
+}
+
+/**
+ * ubi_wl_get_fm_peb - find a physical erase block with a given maximal number.
+ * @ubi: UBI device description object
+ * @anchor: This PEB will be used as anchor PEB by fastmap
+ *
+ * The function returns a physical erase block with a given maximal number
+ * and removes it from the wl subsystem.
+ * Must be called with wl_lock held!
+ */
+struct ubi_wl_entry *ubi_wl_get_fm_peb(struct ubi_device *ubi, int anchor)
+{
+	struct ubi_wl_entry *e = NULL;
+
+	if (!ubi->free.rb_node || (ubi->free_count - ubi->beb_rsvd_pebs < 1))
+		goto out;
+
+	if (anchor)
+		e = find_anchor_wl_entry(&ubi->free);
+	else
+		e = find_mean_wl_entry(ubi, &ubi->free);
+
+	if (!e)
+		goto out;
+
+	self_check_in_wl_tree(ubi, e, &ubi->free);
+
+	/* remove it from the free list,
+	 * the wl subsystem does no longer know this erase block */
+	rb_erase(&e->u.rb, &ubi->free);
+	ubi->free_count--;
+out:
+	return e;
+}
+
+/**
+ * ubi_refill_pools - refills all fastmap PEB pools.
+ * @ubi: UBI device description object
+ */
+void ubi_refill_pools(struct ubi_device *ubi)
+{
+	struct ubi_fm_pool *wl_pool = &ubi->fm_wl_pool;
+	struct ubi_fm_pool *pool = &ubi->fm_pool;
+	struct ubi_wl_entry *e;
+	int enough;
+
+	spin_lock(&ubi->wl_lock);
+
+	return_unused_pool_pebs(ubi, wl_pool);
+	return_unused_pool_pebs(ubi, pool);
+
+	wl_pool->size = 0;
+	pool->size = 0;
+
+	for (;;) {
+		enough = 0;
+		if (pool->size < pool->max_size) {
+			if (!ubi->free.rb_node)
+				break;
+
+			e = wl_get_wle(ubi);
+			if (!e)
+				break;
+
+			pool->pebs[pool->size] = e->pnum;
+			pool->size++;
+		} else
+			enough++;
+
+		if (wl_pool->size < wl_pool->max_size) {
+			if (!ubi->free.rb_node ||
+			   (ubi->free_count - ubi->beb_rsvd_pebs < 5))
+				break;
+
+			e = find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
+			self_check_in_wl_tree(ubi, e, &ubi->free);
+			rb_erase(&e->u.rb, &ubi->free);
+			ubi->free_count--;
+
+			wl_pool->pebs[wl_pool->size] = e->pnum;
+			wl_pool->size++;
+		} else
+			enough++;
+
+		if (enough == 2)
+			break;
+	}
+
+	wl_pool->used = 0;
+	pool->used = 0;
+
+	spin_unlock(&ubi->wl_lock);
+}
+
+/**
+ * ubi_wl_get_peb - get a physical eraseblock.
+ * @ubi: UBI device description object
+ *
+ * This function returns a physical eraseblock in case of success and a
+ * negative error code in case of failure.
+ * Returns with ubi->fm_eba_sem held in read mode!
+ */
+int ubi_wl_get_peb(struct ubi_device *ubi)
+{
+	int ret, retried = 0;
+	struct ubi_fm_pool *pool = &ubi->fm_pool;
+	struct ubi_fm_pool *wl_pool = &ubi->fm_wl_pool;
+
+again:
+	down_read(&ubi->fm_eba_sem);
+	spin_lock(&ubi->wl_lock);
+
+	/* We check here also for the WL pool because at this point we can
+	 * refill the WL pool synchronous. */
+	if (pool->used == pool->size || wl_pool->used == wl_pool->size) {
+		spin_unlock(&ubi->wl_lock);
+		up_read(&ubi->fm_eba_sem);
+		ret = ubi_update_fastmap(ubi);
+		if (ret) {
+			ubi_msg(ubi, "Unable to write a new fastmap: %i", ret);
+			down_read(&ubi->fm_eba_sem);
+			return -ENOSPC;
+		}
+		down_read(&ubi->fm_eba_sem);
+		spin_lock(&ubi->wl_lock);
+	}
+
+	if (pool->used == pool->size) {
+		spin_unlock(&ubi->wl_lock);
+		if (retried) {
+			ubi_err(ubi, "Unable to get a free PEB from user WL pool");
+			ret = -ENOSPC;
+			goto out;
+		}
+		retried = 1;
+		up_read(&ubi->fm_eba_sem);
+		goto again;
+	}
+
+	ubi_assert(pool->used < pool->size);
+	ret = pool->pebs[pool->used++];
+	prot_queue_add(ubi, ubi->lookuptbl[ret]);
+	spin_unlock(&ubi->wl_lock);
+out:
+	return ret;
+}
+
+/* get_peb_for_wl - returns a PEB to be used internally by the WL sub-system.
+ *
+ * @ubi: UBI device description object
+ */
+static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
+{
+	struct ubi_fm_pool *pool = &ubi->fm_wl_pool;
+	int pnum;
+
+	if (pool->used == pool->size) {
+		/* We cannot update the fastmap here because this
+		 * function is called in atomic context.
+		 * Let's fail here and refill/update it as soon as possible. */
+		if (!ubi->fm_work_scheduled) {
+			ubi->fm_work_scheduled = 1;
+			schedule_work(&ubi->fm_work);
+		}
+		return NULL;
+	}
+
+	pnum = pool->pebs[pool->used++];
+	return ubi->lookuptbl[pnum];
+}
+
+/**
+ * ubi_ensure_anchor_pebs - schedule wear-leveling to produce an anchor PEB.
+ * @ubi: UBI device description object
+ */
+int ubi_ensure_anchor_pebs(struct ubi_device *ubi)
+{
+	struct ubi_work *wrk;
+
+	spin_lock(&ubi->wl_lock);
+	if (ubi->wl_scheduled) {
+		spin_unlock(&ubi->wl_lock);
+		return 0;
+	}
+	ubi->wl_scheduled = 1;
+	spin_unlock(&ubi->wl_lock);
+
+	wrk = kmalloc(sizeof(struct ubi_work), GFP_NOFS);
+	if (!wrk) {
+		spin_lock(&ubi->wl_lock);
+		ubi->wl_scheduled = 0;
+		spin_unlock(&ubi->wl_lock);
+		return -ENOMEM;
+	}
+
+	wrk->anchor = 1;
+	wrk->func = &wear_leveling_worker;
+	schedule_ubi_work(ubi, wrk);
+	return 0;
+}
+
+/**
+ * ubi_wl_put_fm_peb - returns a PEB used in a fastmap to the wear-leveling
+ * sub-system.
+ * see: ubi_wl_put_peb()
+ *
+ * @ubi: UBI device description object
+ * @fm_e: physical eraseblock to return
+ * @lnum: the last used logical eraseblock number for the PEB
+ * @torture: if this physical eraseblock has to be tortured
+ */
+int ubi_wl_put_fm_peb(struct ubi_device *ubi, struct ubi_wl_entry *fm_e,
+		      int lnum, int torture)
+{
+	struct ubi_wl_entry *e;
+	int vol_id, pnum = fm_e->pnum;
+
+	dbg_wl("PEB %d", pnum);
+
+	ubi_assert(pnum >= 0);
+	ubi_assert(pnum < ubi->peb_count);
+
+	spin_lock(&ubi->wl_lock);
+	e = ubi->lookuptbl[pnum];
+
+	/* This can happen if we recovered from a fastmap the very
+	 * first time and writing now a new one. In this case the wl system
+	 * has never seen any PEB used by the original fastmap.
+	 */
+	if (!e) {
+		e = fm_e;
+		ubi_assert(e->ec >= 0);
+		ubi->lookuptbl[pnum] = e;
+	}
+
+	spin_unlock(&ubi->wl_lock);
+
+	vol_id = lnum ? UBI_FM_DATA_VOLUME_ID : UBI_FM_SB_VOLUME_ID;
+	return schedule_erase(ubi, e, vol_id, lnum, torture);
+}
+
+/**
+ * ubi_is_erase_work - checks whether a work is erase work.
+ * @wrk: The work object to be checked
+ */
+int ubi_is_erase_work(struct ubi_work *wrk)
+{
+	return wrk->func == erase_worker;
+}
+
+static void ubi_fastmap_close(struct ubi_device *ubi)
+{
+	int i;
+
+	flush_work(&ubi->fm_work);
+	return_unused_pool_pebs(ubi, &ubi->fm_pool);
+	return_unused_pool_pebs(ubi, &ubi->fm_wl_pool);
+
+	if (ubi->fm) {
+		for (i = 0; i < ubi->fm->used_blocks; i++)
+			kfree(ubi->fm->e[i]);
+	}
+	kfree(ubi->fm);
+}
+
+/**
+ * may_reserve_for_fm - tests whether a PEB shall be reserved for fastmap.
+ * See find_mean_wl_entry()
+ *
+ * @ubi: UBI device description object
+ * @e: physical eraseblock to return
+ * @root: RB tree to test against.
+ */
+static struct ubi_wl_entry *may_reserve_for_fm(struct ubi_device *ubi,
+					   struct ubi_wl_entry *e,
+					   struct rb_root *root) {
+	if (e && !ubi->fm_disabled && !ubi->fm &&
+	    e->pnum < UBI_FM_MAX_START)
+		e = rb_entry(rb_next(root->rb_node),
+			     struct ubi_wl_entry, u.rb);
+
+	return e;
+}
diff --git a/drivers/mtd/ubi/fastmap.c b/drivers/mtd/ubi/fastmap.c
index db3defdfc3c0..02a6de2f53ee 100644
--- a/drivers/mtd/ubi/fastmap.c
+++ b/drivers/mtd/ubi/fastmap.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2012 Linutronix GmbH
+ * Copyright (c) 2014 sigma star gmbh
  * Author: Richard Weinberger <richard@nod.at>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,6 +18,69 @@
 #include "ubi.h"
 
 /**
+ * init_seen - allocate memory for used for debugging.
+ * @ubi: UBI device description object
+ */
+static inline int *init_seen(struct ubi_device *ubi)
+{
+	int *ret;
+
+	if (!ubi_dbg_chk_fastmap(ubi))
+		return NULL;
+
+	ret = kcalloc(ubi->peb_count, sizeof(int), GFP_KERNEL);
+	if (!ret)
+		return ERR_PTR(-ENOMEM);
+
+	return ret;
+}
+
+/**
+ * free_seen - free the seen logic integer array.
+ * @seen: integer array of @ubi->peb_count size
+ */
+static inline void free_seen(int *seen)
+{
+	kfree(seen);
+}
+
+/**
+ * set_seen - mark a PEB as seen.
+ * @ubi: UBI device description object
+ * @pnum: The PEB to be makred as seen
+ * @seen: integer array of @ubi->peb_count size
+ */
+static inline void set_seen(struct ubi_device *ubi, int pnum, int *seen)
+{
+	if (!ubi_dbg_chk_fastmap(ubi) || !seen)
+		return;
+
+	seen[pnum] = 1;
+}
+
+/**
+ * self_check_seen - check whether all PEB have been seen by fastmap.
+ * @ubi: UBI device description object
+ * @seen: integer array of @ubi->peb_count size
+ */
+static int self_check_seen(struct ubi_device *ubi, int *seen)
+{
+	int pnum, ret = 0;
+
+	if (!ubi_dbg_chk_fastmap(ubi) || !seen)
+		return 0;
+
+	for (pnum = 0; pnum < ubi->peb_count; pnum++) {
+		if (!seen[pnum] && ubi->lookuptbl[pnum]) {
+			ubi_err(ubi, "self-check failed for PEB %d, fastmap didn't see it", pnum);
+			ret = -EINVAL;
+		}
+	}
+
+	return ret;
+}
+
+/**
  * ubi_calc_fm_size - calculates the fastmap size in bytes for an UBI device.
  * @ubi: UBI device description object
  */
@@ -136,14 +200,15 @@ static struct ubi_ainf_volume *add_vol(struct ubi_attach_info *ai, int vol_id,
 	if (!av)
 		goto out;
 
-	av->highest_lnum = av->leb_count = 0;
+	av->highest_lnum = av->leb_count = av->used_ebs = 0;
 	av->vol_id = vol_id;
-	av->used_ebs = used_ebs;
 	av->data_pad = data_pad;
 	av->last_data_size = last_eb_bytes;
 	av->compat = 0;
 	av->vol_type = vol_type;
 	av->root = RB_ROOT;
+	if (av->vol_type == UBI_STATIC_VOLUME)
+		av->used_ebs = used_ebs;
 
 	dbg_bld("found volume (ID %i)", vol_id);
 
@@ -362,6 +427,7 @@ static void unmap_peb(struct ubi_attach_info *ai, int pnum)
 			aeb = rb_entry(node2, struct ubi_ainf_peb, u.rb);
 			if (aeb->pnum == pnum) {
 				rb_erase(&aeb->u.rb, &av->root);
+				av->leb_count--;
 				kmem_cache_free(ai->aeb_slab_cache, aeb);
 				return;
 			}
@@ -376,7 +442,6 @@ static void unmap_peb(struct ubi_attach_info *ai, int pnum)
  * @pebs: an array of all PEB numbers in the to be scanned pool
  * @pool_size: size of the pool (number of entries in @pebs)
  * @max_sqnum: pointer to the maximal sequence number
- * @eba_orphans: list of PEBs which need to be scanned
  * @free: list of PEBs which are most likely free (and go into @ai->free)
  *
  * Returns 0 on success, if the pool is unusable UBI_BAD_FASTMAP is returned.
@@ -384,12 +449,12 @@ static void unmap_peb(struct ubi_attach_info *ai, int pnum)
  */
 static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		     int *pebs, int pool_size, unsigned long long *max_sqnum,
-		     struct list_head *eba_orphans, struct list_head *free)
+		     struct list_head *free)
 {
 	struct ubi_vid_hdr *vh;
 	struct ubi_ec_hdr *ech;
-	struct ubi_ainf_peb *new_aeb, *tmp_aeb;
-	int i, pnum, err, found_orphan, ret = 0;
+	struct ubi_ainf_peb *new_aeb;
+	int i, pnum, err, ret = 0;
 
 	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
 	if (!ech)
@@ -457,18 +522,6 @@ static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			if (err == UBI_IO_BITFLIPS)
 				scrub = 1;
 
-			found_orphan = 0;
-			list_for_each_entry(tmp_aeb, eba_orphans, u.list) {
-				if (tmp_aeb->pnum == pnum) {
-					found_orphan = 1;
-					break;
-				}
-			}
-			if (found_orphan) {
-				list_del(&tmp_aeb->u.list);
-				kmem_cache_free(ai->aeb_slab_cache, tmp_aeb);
-			}
-
 			new_aeb = kmem_cache_alloc(ai->aeb_slab_cache,
 						   GFP_KERNEL);
 			if (!new_aeb) {
@@ -543,10 +596,9 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 			      struct ubi_attach_info *ai,
 			      struct ubi_fastmap_layout *fm)
 {
-	struct list_head used, eba_orphans, free;
+	struct list_head used, free;
 	struct ubi_ainf_volume *av;
 	struct ubi_ainf_peb *aeb, *tmp_aeb, *_tmp_aeb;
-	struct ubi_ec_hdr *ech;
 	struct ubi_fm_sb *fmsb;
 	struct ubi_fm_hdr *fmhdr;
 	struct ubi_fm_scan_pool *fmpl1, *fmpl2;
@@ -560,22 +612,8 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 
 	INIT_LIST_HEAD(&used);
 	INIT_LIST_HEAD(&free);
-	INIT_LIST_HEAD(&eba_orphans);
-	INIT_LIST_HEAD(&ai->corr);
-	INIT_LIST_HEAD(&ai->free);
-	INIT_LIST_HEAD(&ai->erase);
-	INIT_LIST_HEAD(&ai->alien);
-	ai->volumes = RB_ROOT;
 	ai->min_ec = UBI_MAX_ERASECOUNTER;
 
-	ai->aeb_slab_cache = kmem_cache_create("ubi_ainf_peb_slab",
-					       sizeof(struct ubi_ainf_peb),
-					       0, 0, NULL);
-	if (!ai->aeb_slab_cache) {
-		ret = -ENOMEM;
-		goto fail;
-	}
-
 	fmsb = (struct ubi_fm_sb *)(fm_raw);
 	ai->max_sqnum = fmsb->sqnum;
 	fm_pos += sizeof(struct ubi_fm_sb);
@@ -741,28 +779,9 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 				}
 			}
 
-			/* This can happen if a PEB is already in an EBA known
-			 * by this fastmap but the PEB itself is not in the used
-			 * list.
-			 * In this case the PEB can be within the fastmap pool
-			 * or while writing the fastmap it was in the protection
-			 * queue.
-			 */
 			if (!aeb) {
-				aeb = kmem_cache_alloc(ai->aeb_slab_cache,
-						       GFP_KERNEL);
-				if (!aeb) {
-					ret = -ENOMEM;
-
-					goto fail;
-				}
-
-				aeb->lnum = j;
-				aeb->pnum = be32_to_cpu(fm_eba->pnum[j]);
-				aeb->ec = -1;
-				aeb->scrub = aeb->copy_flag = aeb->sqnum = 0;
-				list_add_tail(&aeb->u.list, &eba_orphans);
-				continue;
+				ubi_err(ubi, "PEB %i is in EBA but not in used list", pnum);
+				goto fail_bad;
 			}
 
 			aeb->lnum = j;
@@ -775,49 +794,13 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 			dbg_bld("inserting PEB:%i (LEB %i) to vol %i",
 				aeb->pnum, aeb->lnum, av->vol_id);
 		}
-
-		ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
-		if (!ech) {
-			ret = -ENOMEM;
-			goto fail;
-		}
-
-		list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &eba_orphans,
-					 u.list) {
-			int err;
-
-			if (ubi_io_is_bad(ubi, tmp_aeb->pnum)) {
-				ubi_err(ubi, "bad PEB in fastmap EBA orphan list");
-				ret = UBI_BAD_FASTMAP;
-				kfree(ech);
-				goto fail;
-			}
-
-			err = ubi_io_read_ec_hdr(ubi, tmp_aeb->pnum, ech, 0);
-			if (err && err != UBI_IO_BITFLIPS) {
-				ubi_err(ubi, "unable to read EC header! PEB:%i err:%i",
-					tmp_aeb->pnum, err);
-				ret = err > 0 ? UBI_BAD_FASTMAP : err;
-				kfree(ech);
-
-				goto fail;
-			} else if (err == UBI_IO_BITFLIPS)
-				tmp_aeb->scrub = 1;
-
-			tmp_aeb->ec = be64_to_cpu(ech->ec);
-			assign_aeb_to_av(ai, tmp_aeb, av);
-		}
-
-		kfree(ech);
 	}
 
-	ret = scan_pool(ubi, ai, fmpl1->pebs, pool_size, &max_sqnum,
-			&eba_orphans, &free);
+	ret = scan_pool(ubi, ai, fmpl1->pebs, pool_size, &max_sqnum, &free);
 	if (ret)
 		goto fail;
 
-	ret = scan_pool(ubi, ai, fmpl2->pebs, wl_pool_size, &max_sqnum,
-			&eba_orphans, &free);
+	ret = scan_pool(ubi, ai, fmpl2->pebs, wl_pool_size, &max_sqnum, &free);
 	if (ret)
 		goto fail;
 
@@ -827,8 +810,9 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 	list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &free, u.list)
 		list_move_tail(&tmp_aeb->u.list, &ai->free);
 
-	ubi_assert(list_empty(&used));
-	ubi_assert(list_empty(&eba_orphans));
+	list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &used, u.list)
+		list_move_tail(&tmp_aeb->u.list, &ai->erase);
+
 	ubi_assert(list_empty(&free));
 
 	/*
@@ -850,10 +834,6 @@ fail:
 		list_del(&tmp_aeb->u.list);
 		kmem_cache_free(ai->aeb_slab_cache, tmp_aeb);
 	}
-	list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &eba_orphans, u.list) {
-		list_del(&tmp_aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, tmp_aeb);
-	}
 	list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &free, u.list) {
 		list_del(&tmp_aeb->u.list);
 		kmem_cache_free(ai->aeb_slab_cache, tmp_aeb);
@@ -884,7 +864,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	__be32 crc, tmp_crc;
 	unsigned long long sqnum = 0;
 
-	mutex_lock(&ubi->fm_mutex);
+	down_write(&ubi->fm_protect);
 	memset(ubi->fm_buf, 0, ubi->fm_size);
 
 	fmsb = kmalloc(sizeof(*fmsb), GFP_KERNEL);
@@ -1075,7 +1055,7 @@ int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	ubi_free_vid_hdr(ubi, vh);
 	kfree(ech);
 out:
-	mutex_unlock(&ubi->fm_mutex);
+	up_write(&ubi->fm_protect);
 	if (ret == UBI_BAD_FASTMAP)
 		ubi_err(ubi, "Attach by fastmap failed, doing a full scan!");
 	return ret;
@@ -1107,13 +1087,14 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	struct ubi_fm_ec *fec;
 	struct ubi_fm_volhdr *fvh;
 	struct ubi_fm_eba *feba;
-	struct rb_node *node;
 	struct ubi_wl_entry *wl_e;
 	struct ubi_volume *vol;
 	struct ubi_vid_hdr *avhdr, *dvhdr;
 	struct ubi_work *ubi_wrk;
+	struct rb_node *tmp_rb;
 	int ret, i, j, free_peb_count, used_peb_count, vol_count;
 	int scrub_peb_count, erase_peb_count;
+	int *seen_pebs = NULL;
 
 	fm_raw = ubi->fm_buf;
 	memset(ubi->fm_buf, 0, ubi->fm_size);
@@ -1130,6 +1111,12 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 		goto out_kfree;
 	}
 
+	seen_pebs = init_seen(ubi);
+	if (IS_ERR(seen_pebs)) {
+		ret = PTR_ERR(seen_pebs);
+		goto out_kfree;
+	}
+
 	spin_lock(&ubi->volumes_lock);
 	spin_lock(&ubi->wl_lock);
 
@@ -1160,8 +1147,10 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	fmpl1->size = cpu_to_be16(ubi->fm_pool.size);
 	fmpl1->max_size = cpu_to_be16(ubi->fm_pool.max_size);
 
-	for (i = 0; i < ubi->fm_pool.size; i++)
+	for (i = 0; i < ubi->fm_pool.size; i++) {
 		fmpl1->pebs[i] = cpu_to_be32(ubi->fm_pool.pebs[i]);
+		set_seen(ubi, ubi->fm_pool.pebs[i], seen_pebs);
+	}
 
 	fmpl2 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
 	fm_pos += sizeof(*fmpl2);
@@ -1169,14 +1158,16 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	fmpl2->size = cpu_to_be16(ubi->fm_wl_pool.size);
 	fmpl2->max_size = cpu_to_be16(ubi->fm_wl_pool.max_size);
 
-	for (i = 0; i < ubi->fm_wl_pool.size; i++)
+	for (i = 0; i < ubi->fm_wl_pool.size; i++) {
 		fmpl2->pebs[i] = cpu_to_be32(ubi->fm_wl_pool.pebs[i]);
+		set_seen(ubi, ubi->fm_wl_pool.pebs[i], seen_pebs);
+	}
 
-	for (node = rb_first(&ubi->free); node; node = rb_next(node)) {
-		wl_e = rb_entry(node, struct ubi_wl_entry, u.rb);
+	ubi_for_each_free_peb(ubi, wl_e, tmp_rb) {
 		fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
 
 		fec->pnum = cpu_to_be32(wl_e->pnum);
+		set_seen(ubi, wl_e->pnum, seen_pebs);
 		fec->ec = cpu_to_be32(wl_e->ec);
 
 		free_peb_count++;
@@ -1185,11 +1176,11 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	}
 	fmh->free_peb_count = cpu_to_be32(free_peb_count);
 
-	for (node = rb_first(&ubi->used); node; node = rb_next(node)) {
-		wl_e = rb_entry(node, struct ubi_wl_entry, u.rb);
+	ubi_for_each_used_peb(ubi, wl_e, tmp_rb) {
 		fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
 
 		fec->pnum = cpu_to_be32(wl_e->pnum);
+		set_seen(ubi, wl_e->pnum, seen_pebs);
 		fec->ec = cpu_to_be32(wl_e->ec);
 
 		used_peb_count++;
@@ -1197,25 +1188,24 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 		ubi_assert(fm_pos <= ubi->fm_size);
 	}
 
-	for (i = 0; i < UBI_PROT_QUEUE_LEN; i++) {
-		list_for_each_entry(wl_e, &ubi->pq[i], u.list) {
-			fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+	ubi_for_each_protected_peb(ubi, i, wl_e) {
+		fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
 
-			fec->pnum = cpu_to_be32(wl_e->pnum);
-			fec->ec = cpu_to_be32(wl_e->ec);
+		fec->pnum = cpu_to_be32(wl_e->pnum);
+		set_seen(ubi, wl_e->pnum, seen_pebs);
+		fec->ec = cpu_to_be32(wl_e->ec);
 
-			used_peb_count++;
-			fm_pos += sizeof(*fec);
-			ubi_assert(fm_pos <= ubi->fm_size);
-		}
+		used_peb_count++;
+		fm_pos += sizeof(*fec);
+		ubi_assert(fm_pos <= ubi->fm_size);
 	}
 	fmh->used_peb_count = cpu_to_be32(used_peb_count);
 
-	for (node = rb_first(&ubi->scrub); node; node = rb_next(node)) {
-		wl_e = rb_entry(node, struct ubi_wl_entry, u.rb);
+	ubi_for_each_scrub_peb(ubi, wl_e, tmp_rb) {
 		fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
 
 		fec->pnum = cpu_to_be32(wl_e->pnum);
+		set_seen(ubi, wl_e->pnum, seen_pebs);
 		fec->ec = cpu_to_be32(wl_e->ec);
 
 		scrub_peb_count++;
@@ -1233,6 +1223,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 			fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
 
 			fec->pnum = cpu_to_be32(wl_e->pnum);
+			set_seen(ubi, wl_e->pnum, seen_pebs);
 			fec->ec = cpu_to_be32(wl_e->ec);
 
 			erase_peb_count++;
@@ -1292,6 +1283,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 
 	for (i = 0; i < new_fm->used_blocks; i++) {
 		fmsb->block_loc[i] = cpu_to_be32(new_fm->e[i]->pnum);
+		set_seen(ubi, new_fm->e[i]->pnum, seen_pebs);
 		fmsb->block_ec[i] = cpu_to_be32(new_fm->e[i]->ec);
 	}
 
@@ -1325,11 +1317,13 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	ubi_assert(new_fm);
 	ubi->fm = new_fm;
 
+	ret = self_check_seen(ubi, seen_pebs);
 	dbg_bld("fastmap written!");
 
 out_kfree:
 	ubi_free_vid_hdr(ubi, avhdr);
 	ubi_free_vid_hdr(ubi, dvhdr);
+	free_seen(seen_pebs);
 out:
 	return ret;
 }
@@ -1384,31 +1378,87 @@ out:
 /**
  * invalidate_fastmap - destroys a fastmap.
  * @ubi: UBI device object
- * @fm: the fastmap to be destroyed
  *
+ * This function ensures that upon next UBI attach a full scan
+ * is issued. We need this if UBI is about to write a new fastmap
+ * but is unable to do so. In this case we have two options:
+ * a) Make sure that the current fastmap will not be usued upon
+ * attach time and contine or b) fall back to RO mode to have the
+ * current fastmap in a valid state.
  * Returns 0 on success, < 0 indicates an internal error.
  */
-static int invalidate_fastmap(struct ubi_device *ubi,
-			      struct ubi_fastmap_layout *fm)
+static int invalidate_fastmap(struct ubi_device *ubi)
 {
 	int ret;
-	struct ubi_vid_hdr *vh;
+	struct ubi_fastmap_layout *fm;
+	struct ubi_wl_entry *e;
+	struct ubi_vid_hdr *vh = NULL;
 
-	ret = erase_block(ubi, fm->e[0]->pnum);
-	if (ret < 0)
-		return ret;
+	if (!ubi->fm)
+		return 0;
+
+	ubi->fm = NULL;
+
+	ret = -ENOMEM;
+	fm = kzalloc(sizeof(*fm), GFP_KERNEL);
+	if (!fm)
+		goto out;
 
 	vh = new_fm_vhdr(ubi, UBI_FM_SB_VOLUME_ID);
 	if (!vh)
-		return -ENOMEM;
+		goto out_free_fm;
 
-	/* deleting the current fastmap SB is not enough, an old SB may exist,
-	 * so create a (corrupted) SB such that fastmap will find it and fall
-	 * back to scanning mode in any case */
+	ret = -ENOSPC;
+	e = ubi_wl_get_fm_peb(ubi, 1);
+	if (!e)
+		goto out_free_fm;
+
+	/*
+	 * Create fake fastmap such that UBI will fall back
+	 * to scanning mode.
+	 */
 	vh->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
-	ret = ubi_io_write_vid_hdr(ubi, fm->e[0]->pnum, vh);
+	ret = ubi_io_write_vid_hdr(ubi, e->pnum, vh);
+	if (ret < 0) {
+		ubi_wl_put_fm_peb(ubi, e, 0, 0);
+		goto out_free_fm;
+	}
+
+	fm->used_blocks = 1;
+	fm->e[0] = e;
+
+	ubi->fm = fm;
 
+out:
+	ubi_free_vid_hdr(ubi, vh);
 	return ret;
+
+out_free_fm:
+	kfree(fm);
+	goto out;
+}
+
+/**
+ * return_fm_pebs - returns all PEBs used by a fastmap back to the
+ * WL sub-system.
+ * @ubi: UBI device object
+ * @fm: fastmap layout object
+ */
+static void return_fm_pebs(struct ubi_device *ubi,
+			   struct ubi_fastmap_layout *fm)
+{
+	int i;
+
+	if (!fm)
+		return;
+
+	for (i = 0; i < fm->used_blocks; i++) {
+		if (fm->e[i]) {
+			ubi_wl_put_fm_peb(ubi, fm->e[i], i,
+					  fm->to_be_tortured[i]);
+			fm->e[i] = NULL;
+		}
+	}
 }
 
 /**
@@ -1420,45 +1470,32 @@ static int invalidate_fastmap(struct ubi_device *ubi,
  */
 int ubi_update_fastmap(struct ubi_device *ubi)
 {
-	int ret, i;
+	int ret, i, j;
 	struct ubi_fastmap_layout *new_fm, *old_fm;
 	struct ubi_wl_entry *tmp_e;
 
-	mutex_lock(&ubi->fm_mutex);
+	down_write(&ubi->fm_protect);
 
 	ubi_refill_pools(ubi);
 
 	if (ubi->ro_mode || ubi->fm_disabled) {
-		mutex_unlock(&ubi->fm_mutex);
+		up_write(&ubi->fm_protect);
 		return 0;
 	}
 
 	ret = ubi_ensure_anchor_pebs(ubi);
 	if (ret) {
-		mutex_unlock(&ubi->fm_mutex);
+		up_write(&ubi->fm_protect);
 		return ret;
 	}
 
 	new_fm = kzalloc(sizeof(*new_fm), GFP_KERNEL);
 	if (!new_fm) {
-		mutex_unlock(&ubi->fm_mutex);
+		up_write(&ubi->fm_protect);
 		return -ENOMEM;
 	}
 
 	new_fm->used_blocks = ubi->fm_size / ubi->leb_size;
-
-	for (i = 0; i < new_fm->used_blocks; i++) {
-		new_fm->e[i] = kmem_cache_alloc(ubi_wl_entry_slab, GFP_KERNEL);
-		if (!new_fm->e[i]) {
-			while (i--)
-				kfree(new_fm->e[i]);
-
-			kfree(new_fm);
-			mutex_unlock(&ubi->fm_mutex);
-			return -ENOMEM;
-		}
-	}
-
 	old_fm = ubi->fm;
 	ubi->fm = NULL;
 
@@ -1473,37 +1510,49 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 		tmp_e = ubi_wl_get_fm_peb(ubi, 0);
 		spin_unlock(&ubi->wl_lock);
 
-		if (!tmp_e && !old_fm) {
-			int j;
-			ubi_err(ubi, "could not get any free erase block");
-
-			for (j = 1; j < i; j++)
-				ubi_wl_put_fm_peb(ubi, new_fm->e[j], j, 0);
-
-			ret = -ENOSPC;
-			goto err;
-		} else if (!tmp_e && old_fm) {
-			ret = erase_block(ubi, old_fm->e[i]->pnum);
-			if (ret < 0) {
-				int j;
-
-				for (j = 1; j < i; j++)
-					ubi_wl_put_fm_peb(ubi, new_fm->e[j],
-							  j, 0);
+		if (!tmp_e) {
+			if (old_fm && old_fm->e[i]) {
+				ret = erase_block(ubi, old_fm->e[i]->pnum);
+				if (ret < 0) {
+					ubi_err(ubi, "could not erase old fastmap PEB");
+
+					for (j = 1; j < i; j++) {
+						ubi_wl_put_fm_peb(ubi, new_fm->e[j],
+								  j, 0);
+						new_fm->e[j] = NULL;
+					}
+					goto err;
+				}
+				new_fm->e[i] = old_fm->e[i];
+				old_fm->e[i] = NULL;
+			} else {
+				ubi_err(ubi, "could not get any free erase block");
+
+				for (j = 1; j < i; j++) {
+					ubi_wl_put_fm_peb(ubi, new_fm->e[j], j, 0);
+					new_fm->e[j] = NULL;
+				}
 
-				ubi_err(ubi, "could not erase old fastmap PEB");
+				ret = -ENOSPC;
 				goto err;
 			}
-
-			new_fm->e[i]->pnum = old_fm->e[i]->pnum;
-			new_fm->e[i]->ec = old_fm->e[i]->ec;
 		} else {
-			new_fm->e[i]->pnum = tmp_e->pnum;
-			new_fm->e[i]->ec = tmp_e->ec;
+			new_fm->e[i] = tmp_e;
 
-			if (old_fm)
+			if (old_fm && old_fm->e[i]) {
 				ubi_wl_put_fm_peb(ubi, old_fm->e[i], i,
 						  old_fm->to_be_tortured[i]);
+				old_fm->e[i] = NULL;
+			}
+		}
+	}
+
+	/* Old fastmap is larger than the new one */
+	if (old_fm && new_fm->used_blocks < old_fm->used_blocks) {
+		for (i = new_fm->used_blocks; i < old_fm->used_blocks; i++) {
+			ubi_wl_put_fm_peb(ubi, old_fm->e[i], i,
+					  old_fm->to_be_tortured[i]);
+			old_fm->e[i] = NULL;
 		}
 	}
 
@@ -1516,67 +1565,67 @@ int ubi_update_fastmap(struct ubi_device *ubi)
 		if (!tmp_e) {
 			ret = erase_block(ubi, old_fm->e[0]->pnum);
 			if (ret < 0) {
-				int i;
 				ubi_err(ubi, "could not erase old anchor PEB");
 
-				for (i = 1; i < new_fm->used_blocks; i++)
+				for (i = 1; i < new_fm->used_blocks; i++) {
 					ubi_wl_put_fm_peb(ubi, new_fm->e[i],
 							  i, 0);
+					new_fm->e[i] = NULL;
+				}
 				goto err;
 			}
-
-			new_fm->e[0]->pnum = old_fm->e[0]->pnum;
+			new_fm->e[0] = old_fm->e[0];
 			new_fm->e[0]->ec = ret;
+			old_fm->e[0] = NULL;
 		} else {
 			/* we've got a new anchor PEB, return the old one */
 			ubi_wl_put_fm_peb(ubi, old_fm->e[0], 0,
 					  old_fm->to_be_tortured[0]);
-
-			new_fm->e[0]->pnum = tmp_e->pnum;
-			new_fm->e[0]->ec = tmp_e->ec;
+			new_fm->e[0] = tmp_e;
+			old_fm->e[0] = NULL;
 		}
 	} else {
 		if (!tmp_e) {
-			int i;
 			ubi_err(ubi, "could not find any anchor PEB");
 
-			for (i = 1; i < new_fm->used_blocks; i++)
+			for (i = 1; i < new_fm->used_blocks; i++) {
 				ubi_wl_put_fm_peb(ubi, new_fm->e[i], i, 0);
+				new_fm->e[i] = NULL;
+			}
 
 			ret = -ENOSPC;
 			goto err;
 		}
-
-		new_fm->e[0]->pnum = tmp_e->pnum;
-		new_fm->e[0]->ec = tmp_e->ec;
+		new_fm->e[0] = tmp_e;
 	}
 
 	down_write(&ubi->work_sem);
-	down_write(&ubi->fm_sem);
+	down_write(&ubi->fm_eba_sem);
 	ret = ubi_write_fastmap(ubi, new_fm);
-	up_write(&ubi->fm_sem);
+	up_write(&ubi->fm_eba_sem);
 	up_write(&ubi->work_sem);
 
 	if (ret)
 		goto err;
 
 out_unlock:
-	mutex_unlock(&ubi->fm_mutex);
+	up_write(&ubi->fm_protect);
 	kfree(old_fm);
 	return ret;
 
 err:
-	kfree(new_fm);
-
 	ubi_warn(ubi, "Unable to write new fastmap, err=%i", ret);
 
-	ret = 0;
-	if (old_fm) {
-		ret = invalidate_fastmap(ubi, old_fm);
-		if (ret < 0)
-			ubi_err(ubi, "Unable to invalidiate current fastmap!");
-		else if (ret)
-			ret = 0;
+	ret = invalidate_fastmap(ubi);
+	if (ret < 0) {
+		ubi_err(ubi, "Unable to invalidiate current fastmap!");
+		ubi_ro_mode(ubi);
+	} else {
+		return_fm_pebs(ubi, old_fm);
+		return_fm_pebs(ubi, new_fm);
+		ret = 0;
 	}
+
+	kfree(new_fm);
 	goto out_unlock;
 }
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index ed0bcb35472f..5bbd1f094f4e 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -859,6 +859,9 @@ int ubi_io_write_ec_hdr(struct ubi_device *ubi, int pnum,
 	if (err)
 		return err;
 
+	if (ubi_dbg_power_cut(ubi, POWER_CUT_EC_WRITE))
+		return -EROFS;
+
 	err = ubi_io_write(ubi, ec_hdr, pnum, 0, ubi->ec_hdr_alsize);
 	return err;
 }
@@ -1106,6 +1109,9 @@ int ubi_io_write_vid_hdr(struct ubi_device *ubi, int pnum,
 	if (err)
 		return err;
 
+	if (ubi_dbg_power_cut(ubi, POWER_CUT_VID_WRITE))
+		return -EROFS;
+
 	p = (char *)vid_hdr - ubi->vid_hdr_shift;
 	err = ubi_io_write(ubi, p, pnum, ubi->vid_hdr_aloffset,
 			   ubi->vid_hdr_alsize);
diff --git a/drivers/mtd/ubi/ubi-media.h b/drivers/mtd/ubi/ubi-media.h
index ac2b24d1783d..d0d072e7ccd2 100644
--- a/drivers/mtd/ubi/ubi-media.h
+++ b/drivers/mtd/ubi/ubi-media.h
@@ -403,8 +403,6 @@ struct ubi_vtbl_record {
 #define UBI_FM_MIN_POOL_SIZE	8
 #define UBI_FM_MAX_POOL_SIZE	256
 
-#define UBI_FM_WL_POOL_SIZE	25
-
 /**
  * struct ubi_fm_sb - UBI fastmap super block
  * @magic: fastmap super block magic number (%UBI_FM_SB_MAGIC)
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index c5be82d9d345..c998212fc680 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -151,6 +151,17 @@ enum {
 	UBI_BAD_FASTMAP,
 };
 
+/*
+ * Flags for emulate_power_cut in ubi_debug_info
+ *
+ * POWER_CUT_EC_WRITE: Emulate a power cut when writing an EC header
+ * POWER_CUT_VID_WRITE: Emulate a power cut when writing a VID header
+ */
+enum {
+	POWER_CUT_EC_WRITE = 0x01,
+	POWER_CUT_VID_WRITE = 0x02,
+};
+
 /**
  * struct ubi_wl_entry - wear-leveling entry.
  * @u.rb: link in the corresponding (free/used) RB-tree
@@ -356,30 +367,48 @@ struct ubi_wl_entry;
  *
  * @chk_gen: if UBI general extra checks are enabled
  * @chk_io: if UBI I/O extra checks are enabled
+ * @chk_fastmap: if UBI fastmap extra checks are enabled
  * @disable_bgt: disable the background task for testing purposes
  * @emulate_bitflips: emulate bit-flips for testing purposes
  * @emulate_io_failures: emulate write/erase failures for testing purposes
+ * @emulate_power_cut: emulate power cut for testing purposes
+ * @power_cut_counter: count down for writes left until emulated power cut
+ * @power_cut_min: minimum number of writes before emulating a power cut
+ * @power_cut_max: maximum number of writes until emulating a power cut
  * @dfs_dir_name: name of debugfs directory containing files of this UBI device
  * @dfs_dir: direntry object of the UBI device debugfs directory
  * @dfs_chk_gen: debugfs knob to enable UBI general extra checks
  * @dfs_chk_io: debugfs knob to enable UBI I/O extra checks
+ * @dfs_chk_fastmap: debugfs knob to enable UBI fastmap extra checks
  * @dfs_disable_bgt: debugfs knob to disable the background task
  * @dfs_emulate_bitflips: debugfs knob to emulate bit-flips
  * @dfs_emulate_io_failures: debugfs knob to emulate write/erase failures
+ * @dfs_emulate_power_cut: debugfs knob to emulate power cuts
+ * @dfs_power_cut_min: debugfs knob for minimum writes before power cut
+ * @dfs_power_cut_max: debugfs knob for maximum writes until power cut
  */
 struct ubi_debug_info {
 	unsigned int chk_gen:1;
 	unsigned int chk_io:1;
+	unsigned int chk_fastmap:1;
 	unsigned int disable_bgt:1;
 	unsigned int emulate_bitflips:1;
 	unsigned int emulate_io_failures:1;
+	unsigned int emulate_power_cut:2;
+	unsigned int power_cut_counter;
+	unsigned int power_cut_min;
+	unsigned int power_cut_max;
 	char dfs_dir_name[UBI_DFS_DIR_LEN + 1];
 	struct dentry *dfs_dir;
 	struct dentry *dfs_chk_gen;
 	struct dentry *dfs_chk_io;
+	struct dentry *dfs_chk_fastmap;
 	struct dentry *dfs_disable_bgt;
 	struct dentry *dfs_emulate_bitflips;
 	struct dentry *dfs_emulate_io_failures;
+	struct dentry *dfs_emulate_power_cut;
+	struct dentry *dfs_power_cut_min;
+	struct dentry *dfs_power_cut_max;
 };
 
 /**
@@ -426,11 +455,13 @@ struct ubi_debug_info {
  * @fm_pool: in-memory data structure of the fastmap pool
  * @fm_wl_pool: in-memory data structure of the fastmap pool used by the WL
  *		sub-system
- * @fm_mutex: serializes ubi_update_fastmap() and protects @fm_buf
+ * @fm_protect: serializes ubi_update_fastmap(), protects @fm_buf and makes sure
+ * that critical sections cannot be interrupted by ubi_update_fastmap()
  * @fm_buf: vmalloc()'d buffer which holds the raw fastmap
  * @fm_size: fastmap size in bytes
- * @fm_sem: allows ubi_update_fastmap() to block EBA table changes
+ * @fm_eba_sem: allows ubi_update_fastmap() to block EBA table changes
  * @fm_work: fastmap work queue
+ * @fm_work_scheduled: non-zero if fastmap work was scheduled
  *
  * @used: RB-tree of used physical eraseblocks
  * @erroneous: RB-tree of erroneous used physical eraseblocks
@@ -442,7 +473,8 @@ struct ubi_debug_info {
  * @pq_head: protection queue head
  * @wl_lock: protects the @used, @free, @pq, @pq_head, @lookuptbl, @move_from,
  *	     @move_to, @move_to_put @erase_pending, @wl_scheduled, @works,
- *	     @erroneous, and @erroneous_peb_count fields
+ *	     @erroneous, @erroneous_peb_count, @fm_work_scheduled, @fm_pool,
+ *	     and @fm_wl_pool fields
  * @move_mutex: serializes eraseblock moves
  * @work_sem: used to wait for all the scheduled works to finish and prevent
  * new works from being submitted
@@ -479,7 +511,7 @@ struct ubi_debug_info {
  * @vid_hdr_offset: starting offset of the volume identifier header (might be
  *                  unaligned)
  * @vid_hdr_aloffset: starting offset of the VID header aligned to
- * @hdrs_min_io_size
+ *                    @hdrs_min_io_size
  * @vid_hdr_shift: contains @vid_hdr_offset - @vid_hdr_aloffset
  * @bad_allowed: whether the MTD device admits of bad physical eraseblocks or
  *               not
@@ -532,11 +564,12 @@ struct ubi_device {
 	struct ubi_fastmap_layout *fm;
 	struct ubi_fm_pool fm_pool;
 	struct ubi_fm_pool fm_wl_pool;
-	struct rw_semaphore fm_sem;
-	struct mutex fm_mutex;
+	struct rw_semaphore fm_eba_sem;
+	struct rw_semaphore fm_protect;
 	void *fm_buf;
 	size_t fm_size;
 	struct work_struct fm_work;
+	int fm_work_scheduled;
 
 	/* Wear-leveling sub-system's stuff */
 	struct rb_root used;
@@ -868,10 +901,14 @@ int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 		      int pnum, const struct ubi_vid_hdr *vid_hdr);
 
 /* fastmap.c */
+#ifdef CONFIG_MTD_UBI_FASTMAP
 size_t ubi_calc_fm_size(struct ubi_device *ubi);
 int ubi_update_fastmap(struct ubi_device *ubi);
 int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		     int fm_anchor);
+#else
+static inline int ubi_update_fastmap(struct ubi_device *ubi) { return 0; }
+#endif
 
 /* block.c */
 #ifdef CONFIG_MTD_UBI_BLOCK
@@ -892,6 +929,42 @@ static inline int ubiblock_remove(struct ubi_volume_info *vi)
 }
 #endif
 
+/*
+ * ubi_for_each_free_peb - walk the UBI free RB tree.
+ * @ubi: UBI device description object
+ * @e: a pointer to a ubi_wl_entry to use as cursor
+ * @pos: a pointer to RB-tree entry type to use as a loop counter
+ */
+#define ubi_for_each_free_peb(ubi, e, tmp_rb)	\
+	ubi_rb_for_each_entry((tmp_rb), (e), &(ubi)->free, u.rb)
+
+/*
+ * ubi_for_each_used_peb - walk the UBI used RB tree.
+ * @ubi: UBI device description object
+ * @e: a pointer to a ubi_wl_entry to use as cursor
+ * @pos: a pointer to RB-tree entry type to use as a loop counter
+ */
+#define ubi_for_each_used_peb(ubi, e, tmp_rb)	\
+	ubi_rb_for_each_entry((tmp_rb), (e), &(ubi)->used, u.rb)
+
+/*
+ * ubi_for_each_scub_peb - walk the UBI scub RB tree.
+ * @ubi: UBI device description object
+ * @e: a pointer to a ubi_wl_entry to use as cursor
+ * @pos: a pointer to RB-tree entry type to use as a loop counter
+ */
+#define ubi_for_each_scrub_peb(ubi, e, tmp_rb)	\
+	ubi_rb_for_each_entry((tmp_rb), (e), &(ubi)->scrub, u.rb)
+
+/*
+ * ubi_for_each_protected_peb - walk the UBI protection queue.
+ * @ubi: UBI device description object
+ * @i: a integer used as counter
+ * @e: a pointer to a ubi_wl_entry to use as cursor
+ */
+#define ubi_for_each_protected_peb(ubi, i, e)	\
+	for ((i) = 0; (i) < UBI_PROT_QUEUE_LEN; (i)++)	\
+		list_for_each_entry((e), &(ubi->pq[(i)]), u.list)
 
 /*
  * ubi_rb_for_each_entry - walk an RB-tree.
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 8f7bde6a85d6..16214d3d57a4 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -103,6 +103,7 @@
 #include <linux/freezer.h>
 #include <linux/kthread.h>
 #include "ubi.h"
+#include "wl.h"
 
 /* Number of physical eraseblocks reserved for wear-leveling purposes */
 #define WL_RESERVED_PEBS 1
@@ -140,42 +141,6 @@ static int self_check_in_wl_tree(const struct ubi_device *ubi,
 static int self_check_in_pq(const struct ubi_device *ubi,
 			    struct ubi_wl_entry *e);
 
-#ifdef CONFIG_MTD_UBI_FASTMAP
-/**
- * update_fastmap_work_fn - calls ubi_update_fastmap from a work queue
- * @wrk: the work description object
- */
-static void update_fastmap_work_fn(struct work_struct *wrk)
-{
-	struct ubi_device *ubi = container_of(wrk, struct ubi_device, fm_work);
-	ubi_update_fastmap(ubi);
-}
-
-/**
- *  ubi_ubi_is_fm_block - returns 1 if a PEB is currently used in a fastmap.
- *  @ubi: UBI device description object
- *  @pnum: the to be checked PEB
- */
-static int ubi_is_fm_block(struct ubi_device *ubi, int pnum)
-{
-	int i;
-
-	if (!ubi->fm)
-		return 0;
-
-	for (i = 0; i < ubi->fm->used_blocks; i++)
-		if (ubi->fm->e[i]->pnum == pnum)
-			return 1;
-
-	return 0;
-}
-#else
-static int ubi_is_fm_block(struct ubi_device *ubi, int pnum)
-{
-	return 0;
-}
-#endif
-
 /**
  * wl_tree_add - add a wear-leveling entry to a WL RB-tree.
  * @e: the wear-leveling entry to add
@@ -213,6 +178,20 @@ static void wl_tree_add(struct ubi_wl_entry *e, struct rb_root *root)
 }
 
 /**
+ * wl_tree_destroy - destroy a wear-leveling entry.
+ * @ubi: UBI device description object
+ * @e: the wear-leveling entry to add
+ *
+ * This function destroys a wear leveling entry and removes
+ * the reference from the lookup table.
+ */
+static void wl_entry_destroy(struct ubi_device *ubi, struct ubi_wl_entry *e)
+{
+	ubi->lookuptbl[e->pnum] = NULL;
+	kmem_cache_free(ubi_wl_entry_slab, e);
+}
+
+/**
  * do_work - do one pending work.
  * @ubi: UBI device description object
  *
@@ -260,33 +239,6 @@ static int do_work(struct ubi_device *ubi)
 }
 
 /**
- * produce_free_peb - produce a free physical eraseblock.
- * @ubi: UBI device description object
- *
- * This function tries to make a free PEB by means of synchronous execution of
- * pending works. This may be needed if, for example the background thread is
- * disabled. Returns zero in case of success and a negative error code in case
- * of failure.
- */
-static int produce_free_peb(struct ubi_device *ubi)
-{
-	int err;
-
-	while (!ubi->free.rb_node && ubi->works_count) {
-		spin_unlock(&ubi->wl_lock);
-
-		dbg_wl("do one work synchronously");
-		err = do_work(ubi);
-
-		spin_lock(&ubi->wl_lock);
-		if (err)
-			return err;
-	}
-
-	return 0;
-}
-
-/**
  * in_wl_tree - check if wear-leveling entry is present in a WL RB-tree.
  * @e: the wear-leveling entry to check
  * @root: the root of the tree
@@ -409,119 +361,32 @@ static struct ubi_wl_entry *find_mean_wl_entry(struct ubi_device *ubi,
 	if (last->ec - first->ec < WL_FREE_MAX_DIFF) {
 		e = rb_entry(root->rb_node, struct ubi_wl_entry, u.rb);
 
-#ifdef CONFIG_MTD_UBI_FASTMAP
 		/* If no fastmap has been written and this WL entry can be used
 		 * as anchor PEB, hold it back and return the second best
 		 * WL entry such that fastmap can use the anchor PEB later. */
-		if (e && !ubi->fm_disabled && !ubi->fm &&
-		    e->pnum < UBI_FM_MAX_START)
-			e = rb_entry(rb_next(root->rb_node),
-				     struct ubi_wl_entry, u.rb);
-#endif
+		e = may_reserve_for_fm(ubi, e, root);
 	} else
 		e = find_wl_entry(ubi, root, WL_FREE_MAX_DIFF/2);
 
 	return e;
 }
 
-#ifdef CONFIG_MTD_UBI_FASTMAP
-/**
- * find_anchor_wl_entry - find wear-leveling entry to used as anchor PEB.
- * @root: the RB-tree where to look for
- */
-static struct ubi_wl_entry *find_anchor_wl_entry(struct rb_root *root)
-{
-	struct rb_node *p;
-	struct ubi_wl_entry *e, *victim = NULL;
-	int max_ec = UBI_MAX_ERASECOUNTER;
-
-	ubi_rb_for_each_entry(p, e, root, u.rb) {
-		if (e->pnum < UBI_FM_MAX_START && e->ec < max_ec) {
-			victim = e;
-			max_ec = e->ec;
-		}
-	}
-
-	return victim;
-}
-
-static int anchor_pebs_avalible(struct rb_root *root)
-{
-	struct rb_node *p;
-	struct ubi_wl_entry *e;
-
-	ubi_rb_for_each_entry(p, e, root, u.rb)
-		if (e->pnum < UBI_FM_MAX_START)
-			return 1;
-
-	return 0;
-}
-
 /**
- * ubi_wl_get_fm_peb - find a physical erase block with a given maximal number.
+ * wl_get_wle - get a mean wl entry to be used by ubi_wl_get_peb() or
+ * refill_wl_user_pool().
  * @ubi: UBI device description object
- * @anchor: This PEB will be used as anchor PEB by fastmap
  *
- * The function returns a physical erase block with a given maximal number
- * and removes it from the wl subsystem.
- * Must be called with wl_lock held!
+ * This function returns a a wear leveling entry in case of success and
+ * NULL in case of failure.
  */
-struct ubi_wl_entry *ubi_wl_get_fm_peb(struct ubi_device *ubi, int anchor)
+static struct ubi_wl_entry *wl_get_wle(struct ubi_device *ubi)
 {
-	struct ubi_wl_entry *e = NULL;
-
-	if (!ubi->free.rb_node || (ubi->free_count - ubi->beb_rsvd_pebs < 1))
-		goto out;
-
-	if (anchor)
-		e = find_anchor_wl_entry(&ubi->free);
-	else
-		e = find_mean_wl_entry(ubi, &ubi->free);
-
-	if (!e)
-		goto out;
-
-	self_check_in_wl_tree(ubi, e, &ubi->free);
-
-	/* remove it from the free list,
-	 * the wl subsystem does no longer know this erase block */
-	rb_erase(&e->u.rb, &ubi->free);
-	ubi->free_count--;
-out:
-	return e;
-}
-#endif
-
-/**
- * __wl_get_peb - get a physical eraseblock.
- * @ubi: UBI device description object
- *
- * This function returns a physical eraseblock in case of success and a
- * negative error code in case of failure.
- */
-static int __wl_get_peb(struct ubi_device *ubi)
-{
-	int err;
 	struct ubi_wl_entry *e;
 
-retry:
-	if (!ubi->free.rb_node) {
-		if (ubi->works_count == 0) {
-			ubi_err(ubi, "no free eraseblocks");
-			ubi_assert(list_empty(&ubi->works));
-			return -ENOSPC;
-		}
-
-		err = produce_free_peb(ubi);
-		if (err < 0)
-			return err;
-		goto retry;
-	}
-
 	e = find_mean_wl_entry(ubi, &ubi->free);
 	if (!e) {
 		ubi_err(ubi, "no free eraseblocks");
-		return -ENOSPC;
+		return NULL;
 	}
 
 	self_check_in_wl_tree(ubi, e, &ubi->free);
@@ -533,174 +398,10 @@ retry:
 	rb_erase(&e->u.rb, &ubi->free);
 	ubi->free_count--;
 	dbg_wl("PEB %d EC %d", e->pnum, e->ec);
-#ifndef CONFIG_MTD_UBI_FASTMAP
-	/* We have to enqueue e only if fastmap is disabled,
-	 * is fastmap enabled prot_queue_add() will be called by
-	 * ubi_wl_get_peb() after removing e from the pool. */
-	prot_queue_add(ubi, e);
-#endif
-	return e->pnum;
-}
-
-#ifdef CONFIG_MTD_UBI_FASTMAP
-/**
- * return_unused_pool_pebs - returns unused PEB to the free tree.
- * @ubi: UBI device description object
- * @pool: fastmap pool description object
- */
-static void return_unused_pool_pebs(struct ubi_device *ubi,
-				    struct ubi_fm_pool *pool)
-{
-	int i;
-	struct ubi_wl_entry *e;
-
-	for (i = pool->used; i < pool->size; i++) {
-		e = ubi->lookuptbl[pool->pebs[i]];
-		wl_tree_add(e, &ubi->free);
-		ubi->free_count++;
-	}
-}
-
-/**
- * refill_wl_pool - refills all the fastmap pool used by the
- * WL sub-system.
- * @ubi: UBI device description object
- */
-static void refill_wl_pool(struct ubi_device *ubi)
-{
-	struct ubi_wl_entry *e;
-	struct ubi_fm_pool *pool = &ubi->fm_wl_pool;
-
-	return_unused_pool_pebs(ubi, pool);
-
-	for (pool->size = 0; pool->size < pool->max_size; pool->size++) {
-		if (!ubi->free.rb_node ||
-		   (ubi->free_count - ubi->beb_rsvd_pebs < 5))
-			break;
-
-		e = find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
-		self_check_in_wl_tree(ubi, e, &ubi->free);
-		rb_erase(&e->u.rb, &ubi->free);
-		ubi->free_count--;
-
-		pool->pebs[pool->size] = e->pnum;
-	}
-	pool->used = 0;
-}
-
-/**
- * refill_wl_user_pool - refills all the fastmap pool used by ubi_wl_get_peb.
- * @ubi: UBI device description object
- */
-static void refill_wl_user_pool(struct ubi_device *ubi)
-{
-	struct ubi_fm_pool *pool = &ubi->fm_pool;
-
-	return_unused_pool_pebs(ubi, pool);
-
-	for (pool->size = 0; pool->size < pool->max_size; pool->size++) {
-		pool->pebs[pool->size] = __wl_get_peb(ubi);
-		if (pool->pebs[pool->size] < 0)
-			break;
-	}
-	pool->used = 0;
-}
-
-/**
- * ubi_refill_pools - refills all fastmap PEB pools.
- * @ubi: UBI device description object
- */
-void ubi_refill_pools(struct ubi_device *ubi)
-{
-	spin_lock(&ubi->wl_lock);
-	refill_wl_pool(ubi);
-	refill_wl_user_pool(ubi);
-	spin_unlock(&ubi->wl_lock);
-}
-
-/* ubi_wl_get_peb - works exaclty like __wl_get_peb but keeps track of
- * the fastmap pool.
- */
-int ubi_wl_get_peb(struct ubi_device *ubi)
-{
-	int ret;
-	struct ubi_fm_pool *pool = &ubi->fm_pool;
-	struct ubi_fm_pool *wl_pool = &ubi->fm_wl_pool;
-
-	if (!pool->size || !wl_pool->size || pool->used == pool->size ||
-	    wl_pool->used == wl_pool->size)
-		ubi_update_fastmap(ubi);
-
-	/* we got not a single free PEB */
-	if (!pool->size)
-		ret = -ENOSPC;
-	else {
-		spin_lock(&ubi->wl_lock);
-		ret = pool->pebs[pool->used++];
-		prot_queue_add(ubi, ubi->lookuptbl[ret]);
-		spin_unlock(&ubi->wl_lock);
-	}
-
-	return ret;
-}
-
-/* get_peb_for_wl - returns a PEB to be used internally by the WL sub-system.
- *
- * @ubi: UBI device description object
- */
-static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
-{
-	struct ubi_fm_pool *pool = &ubi->fm_wl_pool;
-	int pnum;
-
-	if (pool->used == pool->size || !pool->size) {
-		/* We cannot update the fastmap here because this
-		 * function is called in atomic context.
-		 * Let's fail here and refill/update it as soon as possible. */
-		schedule_work(&ubi->fm_work);
-		return NULL;
-	} else {
-		pnum = pool->pebs[pool->used++];
-		return ubi->lookuptbl[pnum];
-	}
-}
-#else
-static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
-{
-	struct ubi_wl_entry *e;
-
-	e = find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
-	self_check_in_wl_tree(ubi, e, &ubi->free);
-	ubi->free_count--;
-	ubi_assert(ubi->free_count >= 0);
-	rb_erase(&e->u.rb, &ubi->free);
 
 	return e;
 }
 
-int ubi_wl_get_peb(struct ubi_device *ubi)
-{
-	int peb, err;
-
-	spin_lock(&ubi->wl_lock);
-	peb = __wl_get_peb(ubi);
-	spin_unlock(&ubi->wl_lock);
-
-	if (peb < 0)
-		return peb;
-
-	err = ubi_self_check_all_ff(ubi, peb, ubi->vid_hdr_aloffset,
-				    ubi->peb_size - ubi->vid_hdr_aloffset);
-	if (err) {
-		ubi_err(ubi, "new PEB %d does not contain all 0xFF bytes",
-			peb);
-		return err;
-	}
-
-	return peb;
-}
-#endif
-
 /**
  * prot_queue_del - remove a physical eraseblock from the protection queue.
  * @ubi: UBI device description object
@@ -867,17 +568,6 @@ static void schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
 static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 			int shutdown);
 
-#ifdef CONFIG_MTD_UBI_FASTMAP
-/**
- * ubi_is_erase_work - checks whether a work is erase work.
- * @wrk: The work object to be checked
- */
-int ubi_is_erase_work(struct ubi_work *wrk)
-{
-	return wrk->func == erase_worker;
-}
-#endif
-
 /**
  * schedule_erase - schedule an erase work.
  * @ubi: UBI device description object
@@ -895,7 +585,6 @@ static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 	struct ubi_work *wl_wrk;
 
 	ubi_assert(e);
-	ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
 
 	dbg_wl("schedule erasure of PEB %d, EC %d, torture %d",
 	       e->pnum, e->ec, torture);
@@ -942,51 +631,6 @@ static int do_sync_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 	return erase_worker(ubi, wl_wrk, 0);
 }
 
-#ifdef CONFIG_MTD_UBI_FASTMAP
-/**
- * ubi_wl_put_fm_peb - returns a PEB used in a fastmap to the wear-leveling
- * sub-system.
- * see: ubi_wl_put_peb()
- *
- * @ubi: UBI device description object
- * @fm_e: physical eraseblock to return
- * @lnum: the last used logical eraseblock number for the PEB
- * @torture: if this physical eraseblock has to be tortured
- */
-int ubi_wl_put_fm_peb(struct ubi_device *ubi, struct ubi_wl_entry *fm_e,
-		      int lnum, int torture)
-{
-	struct ubi_wl_entry *e;
-	int vol_id, pnum = fm_e->pnum;
-
-	dbg_wl("PEB %d", pnum);
-
-	ubi_assert(pnum >= 0);
-	ubi_assert(pnum < ubi->peb_count);
-
-	spin_lock(&ubi->wl_lock);
-	e = ubi->lookuptbl[pnum];
-
-	/* This can happen if we recovered from a fastmap the very
-	 * first time and writing now a new one. In this case the wl system
-	 * has never seen any PEB used by the original fastmap.
-	 */
-	if (!e) {
-		e = fm_e;
-		ubi_assert(e->ec >= 0);
-		ubi->lookuptbl[pnum] = e;
-	} else {
-		e->ec = fm_e->ec;
-		kfree(fm_e);
-	}
-
-	spin_unlock(&ubi->wl_lock);
-
-	vol_id = lnum ? UBI_FM_DATA_VOLUME_ID : UBI_FM_SB_VOLUME_ID;
-	return schedule_erase(ubi, e, vol_id, lnum, torture);
-}
-#endif
-
 /**
  * wear_leveling_worker - wear-leveling worker function.
  * @ubi: UBI device description object
@@ -1002,7 +646,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 				int shutdown)
 {
 	int err, scrubbing = 0, torture = 0, protect = 0, erroneous = 0;
-	int vol_id = -1, uninitialized_var(lnum);
+	int vol_id = -1, lnum = -1;
 #ifdef CONFIG_MTD_UBI_FASTMAP
 	int anchor = wrk->anchor;
 #endif
@@ -1214,7 +858,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	err = do_sync_erase(ubi, e1, vol_id, lnum, 0);
 	if (err) {
 		if (e2)
-			kmem_cache_free(ubi_wl_entry_slab, e2);
+			wl_entry_destroy(ubi, e2);
 		goto out_ro;
 	}
 
@@ -1282,8 +926,8 @@ out_error:
 	spin_unlock(&ubi->wl_lock);
 
 	ubi_free_vid_hdr(ubi, vid_hdr);
-	kmem_cache_free(ubi_wl_entry_slab, e1);
-	kmem_cache_free(ubi_wl_entry_slab, e2);
+	wl_entry_destroy(ubi, e1);
+	wl_entry_destroy(ubi, e2);
 
 out_ro:
 	ubi_ro_mode(ubi);
@@ -1369,38 +1013,6 @@ out_unlock:
 	return err;
 }
 
-#ifdef CONFIG_MTD_UBI_FASTMAP
-/**
- * ubi_ensure_anchor_pebs - schedule wear-leveling to produce an anchor PEB.
- * @ubi: UBI device description object
- */
-int ubi_ensure_anchor_pebs(struct ubi_device *ubi)
-{
-	struct ubi_work *wrk;
-
-	spin_lock(&ubi->wl_lock);
-	if (ubi->wl_scheduled) {
-		spin_unlock(&ubi->wl_lock);
-		return 0;
-	}
-	ubi->wl_scheduled = 1;
-	spin_unlock(&ubi->wl_lock);
-
-	wrk = kmalloc(sizeof(struct ubi_work), GFP_NOFS);
-	if (!wrk) {
-		spin_lock(&ubi->wl_lock);
-		ubi->wl_scheduled = 0;
-		spin_unlock(&ubi->wl_lock);
-		return -ENOMEM;
-	}
-
-	wrk->anchor = 1;
-	wrk->func = &wear_leveling_worker;
-	schedule_ubi_work(ubi, wrk);
-	return 0;
-}
-#endif
-
 /**
  * erase_worker - physical eraseblock erase worker function.
  * @ubi: UBI device description object
@@ -1425,15 +1037,13 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 	if (shutdown) {
 		dbg_wl("cancel erasure of PEB %d EC %d", pnum, e->ec);
 		kfree(wl_wrk);
-		kmem_cache_free(ubi_wl_entry_slab, e);
+		wl_entry_destroy(ubi, e);
 		return 0;
 	}
 
 	dbg_wl("erase PEB %d EC %d LEB %d:%d",
 	       pnum, e->ec, wl_wrk->vol_id, wl_wrk->lnum);
 
-	ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
-
 	err = sync_erase(ubi, e, wl_wrk->torture);
 	if (!err) {
 		/* Fine, we've erased it successfully */
@@ -1471,7 +1081,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 		return err;
 	}
 
-	kmem_cache_free(ubi_wl_entry_slab, e);
+	wl_entry_destroy(ubi, e);
 	if (err != -EIO)
 		/*
 		 * If this is not %-EIO, we have no idea what to do. Scheduling
@@ -1563,6 +1173,8 @@ int ubi_wl_put_peb(struct ubi_device *ubi, int vol_id, int lnum,
 	ubi_assert(pnum >= 0);
 	ubi_assert(pnum < ubi->peb_count);
 
+	down_read(&ubi->fm_protect);
+
 retry:
 	spin_lock(&ubi->wl_lock);
 	e = ubi->lookuptbl[pnum];
@@ -1593,6 +1205,7 @@ retry:
 		ubi_assert(!ubi->move_to_put);
 		ubi->move_to_put = 1;
 		spin_unlock(&ubi->wl_lock);
+		up_read(&ubi->fm_protect);
 		return 0;
 	} else {
 		if (in_wl_tree(e, &ubi->used)) {
@@ -1614,6 +1227,7 @@ retry:
 				ubi_err(ubi, "PEB %d not found", pnum);
 				ubi_ro_mode(ubi);
 				spin_unlock(&ubi->wl_lock);
+				up_read(&ubi->fm_protect);
 				return err;
 			}
 		}
@@ -1627,6 +1241,7 @@ retry:
 		spin_unlock(&ubi->wl_lock);
 	}
 
+	up_read(&ubi->fm_protect);
 	return err;
 }
 
@@ -1758,9 +1373,10 @@ int ubi_wl_flush(struct ubi_device *ubi, int vol_id, int lnum)
 
 /**
  * tree_destroy - destroy an RB-tree.
+ * @ubi: UBI device description object
  * @root: the root of the tree to destroy
  */
-static void tree_destroy(struct rb_root *root)
+static void tree_destroy(struct ubi_device *ubi, struct rb_root *root)
 {
 	struct rb_node *rb;
 	struct ubi_wl_entry *e;
@@ -1782,7 +1398,7 @@ static void tree_destroy(struct rb_root *root)
 					rb->rb_right = NULL;
 			}
 
-			kmem_cache_free(ubi_wl_entry_slab, e);
+			wl_entry_destroy(ubi, e);
 		}
 	}
 }
@@ -1850,6 +1466,9 @@ int ubi_thread(void *u)
  */
 static void shutdown_work(struct ubi_device *ubi)
 {
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	flush_work(&ubi->fm_work);
+#endif
 	while (!list_empty(&ubi->works)) {
 		struct ubi_work *wrk;
 
@@ -1883,9 +1502,6 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	init_rwsem(&ubi->work_sem);
 	ubi->max_ec = ai->max_ec;
 	INIT_LIST_HEAD(&ubi->works);
-#ifdef CONFIG_MTD_UBI_FASTMAP
-	INIT_WORK(&ubi->fm_work, update_fastmap_work_fn);
-#endif
 
 	sprintf(ubi->bgt_name, UBI_BGT_NAME_PATTERN, ubi->ubi_num);
 
@@ -1907,10 +1523,9 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 
 		e->pnum = aeb->pnum;
 		e->ec = aeb->ec;
-		ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
 		ubi->lookuptbl[e->pnum] = e;
 		if (schedule_erase(ubi, e, aeb->vol_id, aeb->lnum, 0)) {
-			kmem_cache_free(ubi_wl_entry_slab, e);
+			wl_entry_destroy(ubi, e);
 			goto out_free;
 		}
 
@@ -1928,7 +1543,6 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		e->pnum = aeb->pnum;
 		e->ec = aeb->ec;
 		ubi_assert(e->ec >= 0);
-		ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
 
 		wl_tree_add(e, &ubi->free);
 		ubi->free_count++;
@@ -1966,17 +1580,20 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 
 	dbg_wl("found %i PEBs", found_pebs);
 
-	if (ubi->fm)
+	if (ubi->fm) {
 		ubi_assert(ubi->good_peb_count == \
 			   found_pebs + ubi->fm->used_blocks);
+
+		for (i = 0; i < ubi->fm->used_blocks; i++) {
+			e = ubi->fm->e[i];
+			ubi->lookuptbl[e->pnum] = e;
+		}
+	}
 	else
 		ubi_assert(ubi->good_peb_count == found_pebs);
 
 	reserved_pebs = WL_RESERVED_PEBS;
-#ifdef CONFIG_MTD_UBI_FASTMAP
-	/* Reserve enough LEBs to store two fastmaps. */
-	reserved_pebs += (ubi->fm_size / ubi->leb_size) * 2;
-#endif
+	ubi_fastmap_init(ubi, &reserved_pebs);
 
 	if (ubi->avail_pebs < reserved_pebs) {
 		ubi_err(ubi, "no enough physical eraseblocks (%d, need %d)",
@@ -1998,9 +1615,9 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 
 out_free:
 	shutdown_work(ubi);
-	tree_destroy(&ubi->used);
-	tree_destroy(&ubi->free);
-	tree_destroy(&ubi->scrub);
+	tree_destroy(ubi, &ubi->used);
+	tree_destroy(ubi, &ubi->free);
+	tree_destroy(ubi, &ubi->scrub);
 	kfree(ubi->lookuptbl);
 	return err;
 }
@@ -2017,7 +1634,7 @@ static void protection_queue_destroy(struct ubi_device *ubi)
 	for (i = 0; i < UBI_PROT_QUEUE_LEN; ++i) {
 		list_for_each_entry_safe(e, tmp, &ubi->pq[i], u.list) {
 			list_del(&e->u.list);
-			kmem_cache_free(ubi_wl_entry_slab, e);
+			wl_entry_destroy(ubi, e);
 		}
 	}
 }
@@ -2029,12 +1646,13 @@ static void protection_queue_destroy(struct ubi_device *ubi)
 void ubi_wl_close(struct ubi_device *ubi)
 {
 	dbg_wl("close the WL sub-system");
+	ubi_fastmap_close(ubi);
 	shutdown_work(ubi);
 	protection_queue_destroy(ubi);
-	tree_destroy(&ubi->used);
-	tree_destroy(&ubi->erroneous);
-	tree_destroy(&ubi->free);
-	tree_destroy(&ubi->scrub);
+	tree_destroy(ubi, &ubi->used);
+	tree_destroy(ubi, &ubi->erroneous);
+	tree_destroy(ubi, &ubi->free);
+	tree_destroy(ubi, &ubi->scrub);
 	kfree(ubi->lookuptbl);
 }
 
@@ -2133,3 +1751,94 @@ static int self_check_in_pq(const struct ubi_device *ubi,
 	dump_stack();
 	return -EINVAL;
 }
+#ifndef CONFIG_MTD_UBI_FASTMAP
+static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
+{
+	struct ubi_wl_entry *e;
+
+	e = find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
+	self_check_in_wl_tree(ubi, e, &ubi->free);
+	ubi->free_count--;
+	ubi_assert(ubi->free_count >= 0);
+	rb_erase(&e->u.rb, &ubi->free);
+
+	return e;
+}
+
+/**
+ * produce_free_peb - produce a free physical eraseblock.
+ * @ubi: UBI device description object
+ *
+ * This function tries to make a free PEB by means of synchronous execution of
+ * pending works. This may be needed if, for example the background thread is
+ * disabled. Returns zero in case of success and a negative error code in case
+ * of failure.
+ */
+static int produce_free_peb(struct ubi_device *ubi)
+{
+	int err;
+
+	while (!ubi->free.rb_node && ubi->works_count) {
+		spin_unlock(&ubi->wl_lock);
+
+		dbg_wl("do one work synchronously");
+		err = do_work(ubi);
+
+		spin_lock(&ubi->wl_lock);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+/**
+ * ubi_wl_get_peb - get a physical eraseblock.
+ * @ubi: UBI device description object
+ *
+ * This function returns a physical eraseblock in case of success and a
+ * negative error code in case of failure.
+ * Returns with ubi->fm_eba_sem held in read mode!
+ */
+int ubi_wl_get_peb(struct ubi_device *ubi)
+{
+	int err;
+	struct ubi_wl_entry *e;
+
+retry:
+	down_read(&ubi->fm_eba_sem);
+	spin_lock(&ubi->wl_lock);
+	if (!ubi->free.rb_node) {
+		if (ubi->works_count == 0) {
+			ubi_err(ubi, "no free eraseblocks");
+			ubi_assert(list_empty(&ubi->works));
+			spin_unlock(&ubi->wl_lock);
+			return -ENOSPC;
+		}
+
+		err = produce_free_peb(ubi);
+		if (err < 0) {
+			spin_unlock(&ubi->wl_lock);
+			return err;
+		}
+		spin_unlock(&ubi->wl_lock);
+		up_read(&ubi->fm_eba_sem);
+		goto retry;
+
+	}
+	e = wl_get_wle(ubi);
+	prot_queue_add(ubi, e);
+	spin_unlock(&ubi->wl_lock);
+
+	err = ubi_self_check_all_ff(ubi, e->pnum, ubi->vid_hdr_aloffset,
+				    ubi->peb_size - ubi->vid_hdr_aloffset);
+	if (err) {
+		ubi_err(ubi, "new PEB %d does not contain all 0xFF bytes", e->pnum);
+		return err;
+	}
+
+	return e->pnum;
+}
+#else
+#include "fastmap-wl.c"
+#endif
diff --git a/drivers/mtd/ubi/wl.h b/drivers/mtd/ubi/wl.h
new file mode 100644
index 000000000000..bd1f07e5ce9a
--- /dev/null
+++ b/drivers/mtd/ubi/wl.h
@@ -0,0 +1,28 @@
+#ifndef UBI_WL_H
+#define UBI_WL_H
+#ifdef CONFIG_MTD_UBI_FASTMAP
+static int anchor_pebs_avalible(struct rb_root *root);
+static void update_fastmap_work_fn(struct work_struct *wrk);
+static struct ubi_wl_entry *find_anchor_wl_entry(struct rb_root *root);
+static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi);
+static void ubi_fastmap_close(struct ubi_device *ubi);
+static inline void ubi_fastmap_init(struct ubi_device *ubi, int *count)
+{
+	/* Reserve enough LEBs to store two fastmaps. */
+	*count += (ubi->fm_size / ubi->leb_size) * 2;
+	INIT_WORK(&ubi->fm_work, update_fastmap_work_fn);
+}
+static struct ubi_wl_entry *may_reserve_for_fm(struct ubi_device *ubi,
+					       struct ubi_wl_entry *e,
+					       struct rb_root *root);
+#else /* !CONFIG_MTD_UBI_FASTMAP */
+static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi);
+static inline void ubi_fastmap_close(struct ubi_device *ubi) { }
+static inline void ubi_fastmap_init(struct ubi_device *ubi, int *count) { }
+static struct ubi_wl_entry *may_reserve_for_fm(struct ubi_device *ubi,
+					       struct ubi_wl_entry *e,
+					       struct rb_root *root) {
+	return e;
+}
+#endif /* CONFIG_MTD_UBI_FASTMAP */
+#endif /* UBI_WL_H */
diff --git a/drivers/net/arcnet/arcnet.c b/drivers/net/arcnet/arcnet.c
index 09de683c167e..10f71c732b59 100644
--- a/drivers/net/arcnet/arcnet.c
+++ b/drivers/net/arcnet/arcnet.c
@@ -104,7 +104,6 @@ EXPORT_SYMBOL(arcnet_timeout);
 static int arcnet_header(struct sk_buff *skb, struct net_device *dev,
 			 unsigned short type, const void *daddr,
 			 const void *saddr, unsigned len);
-static int arcnet_rebuild_header(struct sk_buff *skb);
 static int go_tx(struct net_device *dev);
 
 static int debug = ARCNET_DEBUG;
@@ -312,7 +311,6 @@ static int choose_mtu(void)
 
 static const struct header_ops arcnet_header_ops = {
 	.create = arcnet_header,
-	.rebuild = arcnet_rebuild_header,
 };
 
 static const struct net_device_ops arcnet_netdev_ops = {
@@ -538,59 +536,6 @@ static int arcnet_header(struct sk_buff *skb, struct net_device *dev,
 	return proto->build_header(skb, dev, type, _daddr);
 }
 
-
-/* 
- * Rebuild the ARCnet hard header. This is called after an ARP (or in the
- * future other address resolution) has completed on this sk_buff. We now
- * let ARP fill in the destination field.
- */
-static int arcnet_rebuild_header(struct sk_buff *skb)
-{
-	struct net_device *dev = skb->dev;
-	struct arcnet_local *lp = netdev_priv(dev);
-	int status = 0;		/* default is failure */
-	unsigned short type;
-	uint8_t daddr=0;
-	struct ArcProto *proto;
-	/*
-	 * XXX: Why not use skb->mac_len?
-	 */
-	if (skb->network_header - skb->mac_header != 2) {
-		BUGMSG(D_NORMAL,
-		       "rebuild_header: shouldn't be here! (hdrsize=%d)\n",
-		       (int)(skb->network_header - skb->mac_header));
-		return 0;
-	}
-	type = *(uint16_t *) skb_pull(skb, 2);
-	BUGMSG(D_DURING, "rebuild header for protocol %Xh\n", type);
-
-	if (type == ETH_P_IP) {
-#ifdef CONFIG_INET
-		BUGMSG(D_DURING, "rebuild header for ethernet protocol %Xh\n", type);
-		status = arp_find(&daddr, skb) ? 1 : 0;
-		BUGMSG(D_DURING, " rebuilt: dest is %d; protocol %Xh\n",
-		       daddr, type);
-#endif
-	} else {
-		BUGMSG(D_NORMAL,
-		       "I don't understand ethernet protocol %Xh addresses!\n", type);
-		dev->stats.tx_errors++;
-		dev->stats.tx_aborted_errors++;
-	}
-
-	/* if we couldn't resolve the address... give up. */
-	if (!status)
-		return 0;
-
-	/* add the _real_ header this time! */
-	proto = arc_proto_map[lp->default_proto[daddr]];
-	proto->build_header(skb, dev, type, daddr);
-
-	return 1;		/* success */
-}
-
-
-
 /* Called by the kernel in order to transmit a packet. */
 netdev_tx_t arcnet_send_packet(struct sk_buff *skb,
 				     struct net_device *dev)
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index cfc4a9c1000a..fbd54f0e32e8 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -70,6 +70,7 @@
 #define AD_PORT_STANDBY         0x80
 #define AD_PORT_SELECTED        0x100
 #define AD_PORT_MOVED           0x200
+#define AD_PORT_CHURNED         (AD_PORT_ACTOR_CHURN | AD_PORT_PARTNER_CHURN)
 
 /* Port Key definitions
  * key is determined according to the link speed, duplex and
@@ -1013,16 +1014,19 @@ static void ad_rx_machine(struct lacpdu *lacpdu, struct port *port)
 	/* check if state machine should change state */
 
 	/* first, check if port was reinitialized */
-	if (port->sm_vars & AD_PORT_BEGIN)
+	if (port->sm_vars & AD_PORT_BEGIN) {
 		port->sm_rx_state = AD_RX_INITIALIZE;
+		port->sm_vars |= AD_PORT_CHURNED;
 	/* check if port is not enabled */
-	else if (!(port->sm_vars & AD_PORT_BEGIN)
+	} else if (!(port->sm_vars & AD_PORT_BEGIN)
 		 && !port->is_enabled && !(port->sm_vars & AD_PORT_MOVED))
 		port->sm_rx_state = AD_RX_PORT_DISABLED;
 	/* check if new lacpdu arrived */
 	else if (lacpdu && ((port->sm_rx_state == AD_RX_EXPIRED) ||
 		 (port->sm_rx_state == AD_RX_DEFAULTED) ||
 		 (port->sm_rx_state == AD_RX_CURRENT))) {
+		if (port->sm_rx_state != AD_RX_CURRENT)
+			port->sm_vars |= AD_PORT_CHURNED;
 		port->sm_rx_timer_counter = 0;
 		port->sm_rx_state = AD_RX_CURRENT;
 	} else {
@@ -1100,9 +1104,11 @@ static void ad_rx_machine(struct lacpdu *lacpdu, struct port *port)
 			 */
 			port->partner_oper.port_state &= ~AD_STATE_SYNCHRONIZATION;
 			port->sm_vars &= ~AD_PORT_MATCHED;
+			port->partner_oper.port_state |= AD_STATE_LACP_TIMEOUT;
 			port->partner_oper.port_state |= AD_STATE_LACP_ACTIVITY;
 			port->sm_rx_timer_counter = __ad_timer_to_ticks(AD_CURRENT_WHILE_TIMER, (u16)(AD_SHORT_TIMEOUT));
 			port->actor_oper_port_state |= AD_STATE_EXPIRED;
+			port->sm_vars |= AD_PORT_CHURNED;
 			break;
 		case AD_RX_DEFAULTED:
 			__update_default_selected(port);
@@ -1132,6 +1138,45 @@ static void ad_rx_machine(struct lacpdu *lacpdu, struct port *port)
 }
 
 /**
+ * ad_churn_machine - handle port churn's state machine
+ * @port: the port we're looking at
+ *
+ */
+static void ad_churn_machine(struct port *port)
+{
+	if (port->sm_vars & AD_PORT_CHURNED) {
+		port->sm_vars &= ~AD_PORT_CHURNED;
+		port->sm_churn_actor_state = AD_CHURN_MONITOR;
+		port->sm_churn_partner_state = AD_CHURN_MONITOR;
+		port->sm_churn_actor_timer_counter =
+			__ad_timer_to_ticks(AD_ACTOR_CHURN_TIMER, 0);
+		 port->sm_churn_partner_timer_counter =
+			 __ad_timer_to_ticks(AD_PARTNER_CHURN_TIMER, 0);
+		return;
+	}
+	if (port->sm_churn_actor_timer_counter &&
+	    !(--port->sm_churn_actor_timer_counter) &&
+	    port->sm_churn_actor_state == AD_CHURN_MONITOR) {
+		if (port->actor_oper_port_state & AD_STATE_SYNCHRONIZATION) {
+			port->sm_churn_actor_state = AD_NO_CHURN;
+		} else {
+			port->churn_actor_count++;
+			port->sm_churn_actor_state = AD_CHURN;
+		}
+	}
+	if (port->sm_churn_partner_timer_counter &&
+	    !(--port->sm_churn_partner_timer_counter) &&
+	    port->sm_churn_partner_state == AD_CHURN_MONITOR) {
+		if (port->partner_oper.port_state & AD_STATE_SYNCHRONIZATION) {
+			port->sm_churn_partner_state = AD_NO_CHURN;
+		} else {
+			port->churn_partner_count++;
+			port->sm_churn_partner_state = AD_CHURN;
+		}
+	}
+}
+
+/**
  * ad_tx_machine - handle a port's tx state machine
  * @port: the port we're looking at
  */
@@ -1383,8 +1428,10 @@ static void ad_port_selection_logic(struct port *port, bool *update_slave_arr)
 			else
 				port->aggregator->is_individual = true;
 
-			port->aggregator->actor_admin_aggregator_key = port->actor_admin_port_key;
-			port->aggregator->actor_oper_aggregator_key = port->actor_oper_port_key;
+			port->aggregator->actor_admin_aggregator_key =
+				port->actor_admin_port_key;
+			port->aggregator->actor_oper_aggregator_key =
+				port->actor_oper_port_key;
 			port->aggregator->partner_system =
 				port->partner_oper.system;
 			port->aggregator->partner_system_priority =
@@ -1710,14 +1757,9 @@ static void ad_initialize_port(struct port *port, int lacp_fast)
 	};
 
 	if (port) {
-		port->actor_port_number = 1;
 		port->actor_port_priority = 0xff;
-		port->actor_system = null_mac_addr;
-		port->actor_system_priority = 0xffff;
 		port->actor_port_aggregator_identifier = 0;
 		port->ntt = false;
-		port->actor_admin_port_key = 1;
-		port->actor_oper_port_key  = 1;
 		port->actor_admin_port_state = AD_STATE_AGGREGATION |
 					       AD_STATE_LACP_ACTIVITY;
 		port->actor_oper_port_state  = AD_STATE_AGGREGATION |
@@ -1731,7 +1773,7 @@ static void ad_initialize_port(struct port *port, int lacp_fast)
 
 		port->is_enabled = true;
 		/* private parameters */
-		port->sm_vars = 0x3;
+		port->sm_vars = AD_PORT_BEGIN | AD_PORT_LACP_ENABLED;
 		port->sm_rx_state = 0;
 		port->sm_rx_timer_counter = 0;
 		port->sm_periodic_state = 0;
@@ -1739,12 +1781,17 @@ static void ad_initialize_port(struct port *port, int lacp_fast)
 		port->sm_mux_state = 0;
 		port->sm_mux_timer_counter = 0;
 		port->sm_tx_state = 0;
-		port->sm_tx_timer_counter = 0;
-		port->slave = NULL;
 		port->aggregator = NULL;
 		port->next_port_in_aggregator = NULL;
 		port->transaction_id = 0;
 
+		port->sm_churn_actor_timer_counter = 0;
+		port->sm_churn_actor_state = 0;
+		port->churn_actor_count = 0;
+		port->sm_churn_partner_timer_counter = 0;
+		port->sm_churn_partner_state = 0;
+		port->churn_partner_count = 0;
+
 		memcpy(&port->lacpdu, &lacpdu, sizeof(lacpdu));
 	}
 }
@@ -1916,8 +1963,6 @@ void bond_3ad_bind_slave(struct slave *slave)
 		 * lacpdu's are sent in one second)
 		 */
 		port->sm_tx_timer_counter = ad_ticks_per_sec/AD_MAX_TX_IN_SECOND;
-		port->aggregator = NULL;
-		port->next_port_in_aggregator = NULL;
 
 		__disable_port(port);
 
@@ -2164,6 +2209,7 @@ void bond_3ad_state_machine_handler(struct work_struct *work)
 		ad_port_selection_logic(port, &update_slave_arr);
 		ad_mux_machine(port, &update_slave_arr);
 		ad_tx_machine(port);
+		ad_churn_machine(port);
 
 		/* turn off the BEGIN bit, since we already handled it */
 		if (port->sm_vars & AD_PORT_BEGIN)
@@ -2279,8 +2325,8 @@ void bond_3ad_adapter_speed_changed(struct slave *slave)
 	spin_lock_bh(&slave->bond->mode_lock);
 
 	port->actor_admin_port_key &= ~AD_SPEED_KEY_MASKS;
-	port->actor_oper_port_key = port->actor_admin_port_key |=
-		(__get_link_speed(port) << 1);
+	port->actor_admin_port_key |= __get_link_speed(port) << 1;
+	port->actor_oper_port_key = port->actor_admin_port_key;
 	netdev_dbg(slave->bond->dev, "Port %d changed speed\n", port->actor_port_number);
 	/* there is no need to reselect a new aggregator, just signal the
 	 * state machines to reinitialize
@@ -2312,8 +2358,8 @@ void bond_3ad_adapter_duplex_changed(struct slave *slave)
 	spin_lock_bh(&slave->bond->mode_lock);
 
 	port->actor_admin_port_key &= ~AD_DUPLEX_KEY_MASKS;
-	port->actor_oper_port_key = port->actor_admin_port_key |=
-		__get_duplex(port);
+	port->actor_admin_port_key |= __get_duplex(port);
+	port->actor_oper_port_key = port->actor_admin_port_key;
 	netdev_dbg(slave->bond->dev, "Port %d slave %s changed duplex\n",
 		   port->actor_port_number, slave->dev->name);
 	if (port->actor_oper_port_key & AD_DUPLEX_KEY_MASKS)
@@ -2354,21 +2400,19 @@ void bond_3ad_handle_link_change(struct slave *slave, char link)
 	 * on link up we are forcing recheck on the duplex and speed since
 	 * some of he adaptors(ce1000.lan) report.
 	 */
+	port->actor_admin_port_key &= ~(AD_DUPLEX_KEY_MASKS|AD_SPEED_KEY_MASKS);
 	if (link == BOND_LINK_UP) {
 		port->is_enabled = true;
-		port->actor_admin_port_key &= ~AD_DUPLEX_KEY_MASKS;
-		port->actor_oper_port_key = port->actor_admin_port_key |=
-			__get_duplex(port);
-		port->actor_admin_port_key &= ~AD_SPEED_KEY_MASKS;
-		port->actor_oper_port_key = port->actor_admin_port_key |=
-			(__get_link_speed(port) << 1);
+		port->actor_admin_port_key |=
+			(__get_link_speed(port) << 1) | __get_duplex(port);
+		if (port->actor_admin_port_key & AD_DUPLEX_KEY_MASKS)
+			port->sm_vars |= AD_PORT_LACP_ENABLED;
 	} else {
 		/* link has failed */
 		port->is_enabled = false;
-		port->actor_admin_port_key &= ~AD_DUPLEX_KEY_MASKS;
-		port->actor_oper_port_key = (port->actor_admin_port_key &=
-					     ~AD_SPEED_KEY_MASKS);
+		port->sm_vars &= ~AD_PORT_LACP_ENABLED;
 	}
+	port->actor_oper_port_key = port->actor_admin_port_key;
 	netdev_dbg(slave->bond->dev, "Port %d changed link status to %s\n",
 		   port->actor_port_number,
 		   link == BOND_LINK_UP ? "UP" : "DOWN");
@@ -2485,6 +2529,9 @@ int bond_3ad_lacpdu_recv(const struct sk_buff *skb, struct bonding *bond,
 	if (skb->protocol != PKT_TYPE_LACPDU)
 		return RX_HANDLER_ANOTHER;
 
+	if (!MAC_ADDRESS_EQUAL(eth_hdr(skb)->h_dest, lacpdu_mcast_addr))
+		return RX_HANDLER_ANOTHER;
+
 	lacpdu = skb_header_pointer(skb, 0, sizeof(_lacpdu), &_lacpdu);
 	if (!lacpdu)
 		return RX_HANDLER_ANOTHER;
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index b979c265fc51..78dde56ae6e6 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -928,6 +928,39 @@ static inline void slave_disable_netpoll(struct slave *slave)
 
 static void bond_poll_controller(struct net_device *bond_dev)
 {
+	struct bonding *bond = netdev_priv(bond_dev);
+	struct slave *slave = NULL;
+	struct list_head *iter;
+	struct ad_info ad_info;
+	struct netpoll_info *ni;
+	const struct net_device_ops *ops;
+
+	if (BOND_MODE(bond) == BOND_MODE_8023AD)
+		if (bond_3ad_get_active_agg_info(bond, &ad_info))
+			return;
+
+	rcu_read_lock_bh();
+	bond_for_each_slave_rcu(bond, slave, iter) {
+		ops = slave->dev->netdev_ops;
+		if (!bond_slave_is_up(slave) || !ops->ndo_poll_controller)
+			continue;
+
+		if (BOND_MODE(bond) == BOND_MODE_8023AD) {
+			struct aggregator *agg =
+			    SLAVE_AD_INFO(slave)->port.aggregator;
+
+			if (agg &&
+			    agg->aggregator_identifier != ad_info.aggregator_id)
+				continue;
+		}
+
+		ni = rcu_dereference_bh(slave->dev->npinfo);
+		if (down_trylock(&ni->dev_lock))
+			continue;
+		ops->ndo_poll_controller(slave->dev);
+		up(&ni->dev_lock);
+	}
+	rcu_read_unlock_bh();
 }
 
 static void bond_netpoll_cleanup(struct net_device *bond_dev)
@@ -2900,6 +2933,8 @@ static int bond_slave_netdev_event(unsigned long event,
 			if (old_duplex != slave->duplex)
 				bond_3ad_adapter_duplex_changed(slave);
 		}
+		/* Fallthrough */
+	case NETDEV_DOWN:
 		/* Refresh slave-array if applicable!
 		 * If the setup does not use miimon or arpmon (mode-specific!),
 		 * then these events will not cause the slave-array to be
@@ -2911,10 +2946,6 @@ static int bond_slave_netdev_event(unsigned long event,
 		if (bond_mode_uses_xmit_hash(bond))
 			bond_update_slave_arr(bond, NULL);
 		break;
-	case NETDEV_DOWN:
-		if (bond_mode_uses_xmit_hash(bond))
-			bond_update_slave_arr(bond, NULL);
-		break;
 	case NETDEV_CHANGEMTU:
 		/* TODO: Should slaves be allowed to
 		 * independently alter their MTU?  For
@@ -3850,7 +3881,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;
 			}
@@ -4007,6 +4039,7 @@ static const struct net_device_ops bond_netdev_ops = {
 	.ndo_fix_features	= bond_fix_features,
 	.ndo_bridge_setlink	= ndo_dflt_netdev_switch_port_bridge_setlink,
 	.ndo_bridge_dellink	= ndo_dflt_netdev_switch_port_bridge_dellink,
+	.ndo_features_check	= passthru_features_check,
 };
 
 static const struct device_type bond_type = {
diff --git a/drivers/net/bonding/bond_procfs.c b/drivers/net/bonding/bond_procfs.c
index 976f5ad2a0f2..62694cfc05b6 100644
--- a/drivers/net/bonding/bond_procfs.c
+++ b/drivers/net/bonding/bond_procfs.c
@@ -176,18 +176,51 @@ static void bond_info_show_slave(struct seq_file *seq,
 		   slave->link_failure_count);
 
 	seq_printf(seq, "Permanent HW addr: %pM\n", slave->perm_hwaddr);
+	seq_printf(seq, "Slave queue ID: %d\n", slave->queue_id);
 
 	if (BOND_MODE(bond) == BOND_MODE_8023AD) {
-		const struct aggregator *agg
-			= SLAVE_AD_INFO(slave)->port.aggregator;
+		const struct port *port = &SLAVE_AD_INFO(slave)->port;
+		const struct aggregator *agg = port->aggregator;
 
-		if (agg)
+		if (agg) {
 			seq_printf(seq, "Aggregator ID: %d\n",
 				   agg->aggregator_identifier);
-		else
+			seq_printf(seq, "Actor Churn State: %s\n",
+				   bond_3ad_churn_desc(port->sm_churn_actor_state));
+			seq_printf(seq, "Partner Churn State: %s\n",
+				   bond_3ad_churn_desc(port->sm_churn_partner_state));
+			seq_printf(seq, "Actor Churned Count: %d\n",
+				   port->churn_actor_count);
+			seq_printf(seq, "Partner Churned Count: %d\n",
+				   port->churn_partner_count);
+
+			seq_puts(seq, "details actor lacp pdu:\n");
+			seq_printf(seq, "    system priority: %d\n",
+				   port->actor_system_priority);
+			seq_printf(seq, "    port key: %d\n",
+				   port->actor_oper_port_key);
+			seq_printf(seq, "    port priority: %d\n",
+				   port->actor_port_priority);
+			seq_printf(seq, "    port number: %d\n",
+				   port->actor_port_number);
+			seq_printf(seq, "    port state: %d\n",
+				   port->actor_oper_port_state);
+
+			seq_puts(seq, "details partner lacp pdu:\n");
+			seq_printf(seq, "    system priority: %d\n",
+				   port->partner_oper.system_priority);
+			seq_printf(seq, "    oper key: %d\n",
+				   port->partner_oper.key);
+			seq_printf(seq, "    port priority: %d\n",
+				   port->partner_oper.port_priority);
+			seq_printf(seq, "    port number: %d\n",
+				   port->partner_oper.port_number);
+			seq_printf(seq, "    port state: %d\n",
+				   port->partner_oper.port_state);
+		} else {
 			seq_puts(seq, "Aggregator ID: N/A\n");
+		}
 	}
-	seq_printf(seq, "Slave queue ID: %d\n", slave->queue_id);
 }
 
 static int bond_info_seq_show(struct seq_file *seq, void *v)
diff --git a/drivers/net/caif/caif_serial.c b/drivers/net/caif/caif_serial.c
index 27bbc56de15f..9da06537237f 100644
--- a/drivers/net/caif/caif_serial.c
+++ b/drivers/net/caif/caif_serial.c
@@ -70,7 +70,6 @@ struct ser_device {
 	struct tty_struct *tty;
 	bool tx_started;
 	unsigned long state;
-	char *tty_name;
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *debugfs_tty_dir;
 	struct debugfs_blob_wrapper tx_blob;
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/at91_can.c b/drivers/net/can/at91_can.c
index eeb4b8b6b335..f4e40aa4d2a2 100644
--- a/drivers/net/can/at91_can.c
+++ b/drivers/net/can/at91_can.c
@@ -291,13 +291,13 @@ static inline unsigned int get_tx_echo_mb(const struct at91_priv *priv)
 
 static inline u32 at91_read(const struct at91_priv *priv, enum at91_reg reg)
 {
-	return __raw_readl(priv->reg_base + reg);
+	return readl_relaxed(priv->reg_base + reg);
 }
 
 static inline void at91_write(const struct at91_priv *priv, enum at91_reg reg,
 		u32 value)
 {
-	__raw_writel(value, priv->reg_base + reg);
+	writel_relaxed(value, priv->reg_base + reg);
 }
 
 static inline void set_mb_mode_prio(const struct at91_priv *priv,
diff --git a/drivers/net/can/bfin_can.c b/drivers/net/can/bfin_can.c
index e7a6363e736b..27ad312e7abf 100644
--- a/drivers/net/can/bfin_can.c
+++ b/drivers/net/can/bfin_can.c
@@ -20,13 +20,121 @@
 #include <linux/can/dev.h>
 #include <linux/can/error.h>
 
-#include <asm/bfin_can.h>
 #include <asm/portmux.h>
 
 #define DRV_NAME "bfin_can"
 #define BFIN_CAN_TIMEOUT 100
 #define TX_ECHO_SKB_MAX  1
 
+/* transmit and receive channels */
+#define TRANSMIT_CHL 24
+#define RECEIVE_STD_CHL 0
+#define RECEIVE_EXT_CHL 4
+#define RECEIVE_RTR_CHL 8
+#define RECEIVE_EXT_RTR_CHL 12
+#define MAX_CHL_NUMBER 32
+
+/* All Blackfin system MMRs are padded to 32bits even if the register
+ * itself is only 16bits.  So use a helper macro to streamline this
+ */
+#define __BFP(m) u16 m; u16 __pad_##m
+
+/* bfin can registers layout */
+struct bfin_can_mask_regs {
+	__BFP(aml);
+	__BFP(amh);
+};
+
+struct bfin_can_channel_regs {
+	/* data[0,2,4,6] -> data{0,1,2,3} while data[1,3,5,7] is padding */
+	u16 data[8];
+	__BFP(dlc);
+	__BFP(tsv);
+	__BFP(id0);
+	__BFP(id1);
+};
+
+struct bfin_can_regs {
+	/* global control and status registers */
+	__BFP(mc1);		/* offset 0x00 */
+	__BFP(md1);		/* offset 0x04 */
+	__BFP(trs1);		/* offset 0x08 */
+	__BFP(trr1);		/* offset 0x0c */
+	__BFP(ta1);		/* offset 0x10 */
+	__BFP(aa1);		/* offset 0x14 */
+	__BFP(rmp1);		/* offset 0x18 */
+	__BFP(rml1);		/* offset 0x1c */
+	__BFP(mbtif1);		/* offset 0x20 */
+	__BFP(mbrif1);		/* offset 0x24 */
+	__BFP(mbim1);		/* offset 0x28 */
+	__BFP(rfh1);		/* offset 0x2c */
+	__BFP(opss1);		/* offset 0x30 */
+	u32 __pad1[3];
+	__BFP(mc2);		/* offset 0x40 */
+	__BFP(md2);		/* offset 0x44 */
+	__BFP(trs2);		/* offset 0x48 */
+	__BFP(trr2);		/* offset 0x4c */
+	__BFP(ta2);		/* offset 0x50 */
+	__BFP(aa2);		/* offset 0x54 */
+	__BFP(rmp2);		/* offset 0x58 */
+	__BFP(rml2);		/* offset 0x5c */
+	__BFP(mbtif2);		/* offset 0x60 */
+	__BFP(mbrif2);		/* offset 0x64 */
+	__BFP(mbim2);		/* offset 0x68 */
+	__BFP(rfh2);		/* offset 0x6c */
+	__BFP(opss2);		/* offset 0x70 */
+	u32 __pad2[3];
+	__BFP(clock);		/* offset 0x80 */
+	__BFP(timing);		/* offset 0x84 */
+	__BFP(debug);		/* offset 0x88 */
+	__BFP(status);		/* offset 0x8c */
+	__BFP(cec);		/* offset 0x90 */
+	__BFP(gis);		/* offset 0x94 */
+	__BFP(gim);		/* offset 0x98 */
+	__BFP(gif);		/* offset 0x9c */
+	__BFP(control);		/* offset 0xa0 */
+	__BFP(intr);		/* offset 0xa4 */
+	__BFP(version);		/* offset 0xa8 */
+	__BFP(mbtd);		/* offset 0xac */
+	__BFP(ewr);		/* offset 0xb0 */
+	__BFP(esr);		/* offset 0xb4 */
+	u32 __pad3[2];
+	__BFP(ucreg);		/* offset 0xc0 */
+	__BFP(uccnt);		/* offset 0xc4 */
+	__BFP(ucrc);		/* offset 0xc8 */
+	__BFP(uccnf);		/* offset 0xcc */
+	u32 __pad4[1];
+	__BFP(version2);	/* offset 0xd4 */
+	u32 __pad5[10];
+
+	/* channel(mailbox) mask and message registers */
+	struct bfin_can_mask_regs msk[MAX_CHL_NUMBER];		/* offset 0x100 */
+	struct bfin_can_channel_regs chl[MAX_CHL_NUMBER];	/* offset 0x200 */
+};
+
+#undef __BFP
+
+#define SRS 0x0001		/* Software Reset */
+#define SER 0x0008		/* Stuff Error */
+#define BOIM 0x0008		/* Enable Bus Off Interrupt */
+#define CCR 0x0080		/* CAN Configuration Mode Request */
+#define CCA 0x0080		/* Configuration Mode Acknowledge */
+#define SAM 0x0080		/* Sampling */
+#define AME 0x8000		/* Acceptance Mask Enable */
+#define RMLIM 0x0080		/* Enable RX Message Lost Interrupt */
+#define RMLIS 0x0080		/* RX Message Lost IRQ Status */
+#define RTR 0x4000		/* Remote Frame Transmission Request */
+#define BOIS 0x0008		/* Bus Off IRQ Status */
+#define IDE 0x2000		/* Identifier Extension */
+#define EPIS 0x0004		/* Error-Passive Mode IRQ Status */
+#define EPIM 0x0004		/* Enable Error-Passive Mode Interrupt */
+#define EWTIS 0x0001		/* TX Error Count IRQ Status */
+#define EWRIS 0x0002		/* RX Error Count IRQ Status */
+#define BEF 0x0040		/* Bit Error Flag */
+#define FER 0x0080		/* Form Error Flag */
+#define SMR 0x0020		/* Sleep Mode Request */
+#define SMACK 0x0008		/* Sleep Mode Acknowledge */
+
 /*
  * bfin can private data
  */
@@ -78,8 +186,8 @@ static int bfin_can_set_bittiming(struct net_device *dev)
 	if (priv->can.ctrlmode & CAN_CTRLMODE_3_SAMPLES)
 		timing |= SAM;
 
-	bfin_write(&reg->clock, clk);
-	bfin_write(&reg->timing, timing);
+	writew(clk, &reg->clock);
+	writew(timing, &reg->timing);
 
 	netdev_info(dev, "setting CLOCK=0x%04x TIMING=0x%04x\n", clk, timing);
 
@@ -94,16 +202,14 @@ static void bfin_can_set_reset_mode(struct net_device *dev)
 	int i;
 
 	/* disable interrupts */
-	bfin_write(&reg->mbim1, 0);
-	bfin_write(&reg->mbim2, 0);
-	bfin_write(&reg->gim, 0);
+	writew(0, &reg->mbim1);
+	writew(0, &reg->mbim2);
+	writew(0, &reg->gim);
 
 	/* reset can and enter configuration mode */
-	bfin_write(&reg->control, SRS | CCR);
-	SSYNC();
-	bfin_write(&reg->control, CCR);
-	SSYNC();
-	while (!(bfin_read(&reg->control) & CCA)) {
+	writew(SRS | CCR, &reg->control);
+	writew(CCR, &reg->control);
+	while (!(readw(&reg->control) & CCA)) {
 		udelay(10);
 		if (--timeout == 0) {
 			netdev_err(dev, "fail to enter configuration mode\n");
@@ -116,34 +222,33 @@ static void bfin_can_set_reset_mode(struct net_device *dev)
 	 * by writing to CAN Mailbox Configuration Registers 1 and 2
 	 * For all bits: 0 - Mailbox disabled, 1 - Mailbox enabled
 	 */
-	bfin_write(&reg->mc1, 0);
-	bfin_write(&reg->mc2, 0);
+	writew(0, &reg->mc1);
+	writew(0, &reg->mc2);
 
 	/* Set Mailbox Direction */
-	bfin_write(&reg->md1, 0xFFFF);   /* mailbox 1-16 are RX */
-	bfin_write(&reg->md2, 0);   /* mailbox 17-32 are TX */
+	writew(0xFFFF, &reg->md1);   /* mailbox 1-16 are RX */
+	writew(0, &reg->md2);   /* mailbox 17-32 are TX */
 
 	/* RECEIVE_STD_CHL */
 	for (i = 0; i < 2; i++) {
-		bfin_write(&reg->chl[RECEIVE_STD_CHL + i].id0, 0);
-		bfin_write(&reg->chl[RECEIVE_STD_CHL + i].id1, AME);
-		bfin_write(&reg->chl[RECEIVE_STD_CHL + i].dlc, 0);
-		bfin_write(&reg->msk[RECEIVE_STD_CHL + i].amh, 0x1FFF);
-		bfin_write(&reg->msk[RECEIVE_STD_CHL + i].aml, 0xFFFF);
+		writew(0, &reg->chl[RECEIVE_STD_CHL + i].id0);
+		writew(AME, &reg->chl[RECEIVE_STD_CHL + i].id1);
+		writew(0, &reg->chl[RECEIVE_STD_CHL + i].dlc);
+		writew(0x1FFF, &reg->msk[RECEIVE_STD_CHL + i].amh);
+		writew(0xFFFF, &reg->msk[RECEIVE_STD_CHL + i].aml);
 	}
 
 	/* RECEIVE_EXT_CHL */
 	for (i = 0; i < 2; i++) {
-		bfin_write(&reg->chl[RECEIVE_EXT_CHL + i].id0, 0);
-		bfin_write(&reg->chl[RECEIVE_EXT_CHL + i].id1, AME | IDE);
-		bfin_write(&reg->chl[RECEIVE_EXT_CHL + i].dlc, 0);
-		bfin_write(&reg->msk[RECEIVE_EXT_CHL + i].amh, 0x1FFF);
-		bfin_write(&reg->msk[RECEIVE_EXT_CHL + i].aml, 0xFFFF);
+		writew(0, &reg->chl[RECEIVE_EXT_CHL + i].id0);
+		writew(AME | IDE, &reg->chl[RECEIVE_EXT_CHL + i].id1);
+		writew(0, &reg->chl[RECEIVE_EXT_CHL + i].dlc);
+		writew(0x1FFF, &reg->msk[RECEIVE_EXT_CHL + i].amh);
+		writew(0xFFFF, &reg->msk[RECEIVE_EXT_CHL + i].aml);
 	}
 
-	bfin_write(&reg->mc2, BIT(TRANSMIT_CHL - 16));
-	bfin_write(&reg->mc1, BIT(RECEIVE_STD_CHL) + BIT(RECEIVE_EXT_CHL));
-	SSYNC();
+	writew(BIT(TRANSMIT_CHL - 16), &reg->mc2);
+	writew(BIT(RECEIVE_STD_CHL) + BIT(RECEIVE_EXT_CHL), &reg->mc1);
 
 	priv->can.state = CAN_STATE_STOPPED;
 }
@@ -157,9 +262,9 @@ static void bfin_can_set_normal_mode(struct net_device *dev)
 	/*
 	 * leave configuration mode
 	 */
-	bfin_write(&reg->control, bfin_read(&reg->control) & ~CCR);
+	writew(readw(&reg->control) & ~CCR, &reg->control);
 
-	while (bfin_read(&reg->status) & CCA) {
+	while (readw(&reg->status) & CCA) {
 		udelay(10);
 		if (--timeout == 0) {
 			netdev_err(dev, "fail to leave configuration mode\n");
@@ -170,26 +275,25 @@ static void bfin_can_set_normal_mode(struct net_device *dev)
 	/*
 	 * clear _All_  tx and rx interrupts
 	 */
-	bfin_write(&reg->mbtif1, 0xFFFF);
-	bfin_write(&reg->mbtif2, 0xFFFF);
-	bfin_write(&reg->mbrif1, 0xFFFF);
-	bfin_write(&reg->mbrif2, 0xFFFF);
+	writew(0xFFFF, &reg->mbtif1);
+	writew(0xFFFF, &reg->mbtif2);
+	writew(0xFFFF, &reg->mbrif1);
+	writew(0xFFFF, &reg->mbrif2);
 
 	/*
 	 * clear global interrupt status register
 	 */
-	bfin_write(&reg->gis, 0x7FF); /* overwrites with '1' */
+	writew(0x7FF, &reg->gis); /* overwrites with '1' */
 
 	/*
 	 * Initialize Interrupts
 	 * - set bits in the mailbox interrupt mask register
 	 * - global interrupt mask
 	 */
-	bfin_write(&reg->mbim1, BIT(RECEIVE_STD_CHL) + BIT(RECEIVE_EXT_CHL));
-	bfin_write(&reg->mbim2, BIT(TRANSMIT_CHL - 16));
+	writew(BIT(RECEIVE_STD_CHL) + BIT(RECEIVE_EXT_CHL), &reg->mbim1);
+	writew(BIT(TRANSMIT_CHL - 16), &reg->mbim2);
 
-	bfin_write(&reg->gim, EPIM | BOIM | RMLIM);
-	SSYNC();
+	writew(EPIM | BOIM | RMLIM, &reg->gim);
 }
 
 static void bfin_can_start(struct net_device *dev)
@@ -226,7 +330,7 @@ static int bfin_can_get_berr_counter(const struct net_device *dev,
 	struct bfin_can_priv *priv = netdev_priv(dev);
 	struct bfin_can_regs __iomem *reg = priv->membase;
 
-	u16 cec = bfin_read(&reg->cec);
+	u16 cec = readw(&reg->cec);
 
 	bec->txerr = cec >> 8;
 	bec->rxerr = cec;
@@ -252,28 +356,28 @@ static int bfin_can_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* fill id */
 	if (id & CAN_EFF_FLAG) {
-		bfin_write(&reg->chl[TRANSMIT_CHL].id0, id);
+		writew(id, &reg->chl[TRANSMIT_CHL].id0);
 		val = ((id & 0x1FFF0000) >> 16) | IDE;
 	} else
 		val = (id << 2);
 	if (id & CAN_RTR_FLAG)
 		val |= RTR;
-	bfin_write(&reg->chl[TRANSMIT_CHL].id1, val | AME);
+	writew(val | AME, &reg->chl[TRANSMIT_CHL].id1);
 
 	/* fill payload */
 	for (i = 0; i < 8; i += 2) {
 		val = ((7 - i) < dlc ? (data[7 - i]) : 0) +
 			((6 - i) < dlc ? (data[6 - i] << 8) : 0);
-		bfin_write(&reg->chl[TRANSMIT_CHL].data[i], val);
+		writew(val, &reg->chl[TRANSMIT_CHL].data[i]);
 	}
 
 	/* fill data length code */
-	bfin_write(&reg->chl[TRANSMIT_CHL].dlc, dlc);
+	writew(dlc, &reg->chl[TRANSMIT_CHL].dlc);
 
 	can_put_echo_skb(skb, dev, 0);
 
 	/* set transmit request */
-	bfin_write(&reg->trs2, BIT(TRANSMIT_CHL - 16));
+	writew(BIT(TRANSMIT_CHL - 16), &reg->trs2);
 
 	return 0;
 }
@@ -296,26 +400,26 @@ static void bfin_can_rx(struct net_device *dev, u16 isrc)
 	/* get id */
 	if (isrc & BIT(RECEIVE_EXT_CHL)) {
 		/* extended frame format (EFF) */
-		cf->can_id = ((bfin_read(&reg->chl[RECEIVE_EXT_CHL].id1)
+		cf->can_id = ((readw(&reg->chl[RECEIVE_EXT_CHL].id1)
 			     & 0x1FFF) << 16)
-			     + bfin_read(&reg->chl[RECEIVE_EXT_CHL].id0);
+			     + readw(&reg->chl[RECEIVE_EXT_CHL].id0);
 		cf->can_id |= CAN_EFF_FLAG;
 		obj = RECEIVE_EXT_CHL;
 	} else {
 		/* standard frame format (SFF) */
-		cf->can_id = (bfin_read(&reg->chl[RECEIVE_STD_CHL].id1)
+		cf->can_id = (readw(&reg->chl[RECEIVE_STD_CHL].id1)
 			     & 0x1ffc) >> 2;
 		obj = RECEIVE_STD_CHL;
 	}
-	if (bfin_read(&reg->chl[obj].id1) & RTR)
+	if (readw(&reg->chl[obj].id1) & RTR)
 		cf->can_id |= CAN_RTR_FLAG;
 
 	/* get data length code */
-	cf->can_dlc = get_can_dlc(bfin_read(&reg->chl[obj].dlc) & 0xF);
+	cf->can_dlc = get_can_dlc(readw(&reg->chl[obj].dlc) & 0xF);
 
 	/* get payload */
 	for (i = 0; i < 8; i += 2) {
-		val = bfin_read(&reg->chl[obj].data[i]);
+		val = readw(&reg->chl[obj].data[i]);
 		cf->data[7 - i] = (7 - i) < cf->can_dlc ? val : 0;
 		cf->data[6 - i] = (6 - i) < cf->can_dlc ? (val >> 8) : 0;
 	}
@@ -369,7 +473,7 @@ static int bfin_can_err(struct net_device *dev, u16 isrc, u16 status)
 
 	if (state != priv->can.state && (state == CAN_STATE_ERROR_WARNING ||
 				state == CAN_STATE_ERROR_PASSIVE)) {
-		u16 cec = bfin_read(&reg->cec);
+		u16 cec = readw(&reg->cec);
 		u8 rxerr = cec;
 		u8 txerr = cec >> 8;
 
@@ -420,23 +524,23 @@ static irqreturn_t bfin_can_interrupt(int irq, void *dev_id)
 	struct net_device_stats *stats = &dev->stats;
 	u16 status, isrc;
 
-	if ((irq == priv->tx_irq) && bfin_read(&reg->mbtif2)) {
+	if ((irq == priv->tx_irq) && readw(&reg->mbtif2)) {
 		/* transmission complete interrupt */
-		bfin_write(&reg->mbtif2, 0xFFFF);
+		writew(0xFFFF, &reg->mbtif2);
 		stats->tx_packets++;
-		stats->tx_bytes += bfin_read(&reg->chl[TRANSMIT_CHL].dlc);
+		stats->tx_bytes += readw(&reg->chl[TRANSMIT_CHL].dlc);
 		can_get_echo_skb(dev, 0);
 		netif_wake_queue(dev);
-	} else if ((irq == priv->rx_irq) && bfin_read(&reg->mbrif1)) {
+	} else if ((irq == priv->rx_irq) && readw(&reg->mbrif1)) {
 		/* receive interrupt */
-		isrc = bfin_read(&reg->mbrif1);
-		bfin_write(&reg->mbrif1, 0xFFFF);
+		isrc = readw(&reg->mbrif1);
+		writew(0xFFFF, &reg->mbrif1);
 		bfin_can_rx(dev, isrc);
-	} else if ((irq == priv->err_irq) && bfin_read(&reg->gis)) {
+	} else if ((irq == priv->err_irq) && readw(&reg->gis)) {
 		/* error interrupt */
-		isrc = bfin_read(&reg->gis);
-		status = bfin_read(&reg->esr);
-		bfin_write(&reg->gis, 0x7FF);
+		isrc = readw(&reg->gis);
+		status = readw(&reg->esr);
+		writew(0x7FF, &reg->gis);
 		bfin_can_err(dev, isrc, status);
 	} else {
 		return IRQ_NONE;
@@ -556,16 +660,10 @@ static int bfin_can_probe(struct platform_device *pdev)
 		goto exit;
 	}
 
-	if (!request_mem_region(res_mem->start, resource_size(res_mem),
-				dev_name(&pdev->dev))) {
-		err = -EBUSY;
-		goto exit;
-	}
-
 	/* request peripheral pins */
 	err = peripheral_request_list(pdata, dev_name(&pdev->dev));
 	if (err)
-		goto exit_mem_release;
+		goto exit;
 
 	dev = alloc_bfin_candev();
 	if (!dev) {
@@ -574,7 +672,13 @@ static int bfin_can_probe(struct platform_device *pdev)
 	}
 
 	priv = netdev_priv(dev);
-	priv->membase = (void __iomem *)res_mem->start;
+
+	priv->membase = devm_ioremap_resource(&pdev->dev, res_mem);
+	if (IS_ERR(priv->membase)) {
+		err = PTR_ERR(priv->membase);
+		goto exit_peri_pin_free;
+	}
+
 	priv->rx_irq = rx_irq->start;
 	priv->tx_irq = tx_irq->start;
 	priv->err_irq = err_irq->start;
@@ -606,8 +710,6 @@ exit_candev_free:
 	free_candev(dev);
 exit_peri_pin_free:
 	peripheral_free_list(pdata);
-exit_mem_release:
-	release_mem_region(res_mem->start, resource_size(res_mem));
 exit:
 	return err;
 }
@@ -616,15 +718,11 @@ static int bfin_can_remove(struct platform_device *pdev)
 {
 	struct net_device *dev = platform_get_drvdata(pdev);
 	struct bfin_can_priv *priv = netdev_priv(dev);
-	struct resource *res;
 
 	bfin_can_set_reset_mode(dev);
 
 	unregister_candev(dev);
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
-
 	peripheral_free_list(priv->pin_list);
 
 	free_candev(dev);
@@ -641,9 +739,8 @@ static int bfin_can_suspend(struct platform_device *pdev, pm_message_t mesg)
 
 	if (netif_running(dev)) {
 		/* enter sleep mode */
-		bfin_write(&reg->control, bfin_read(&reg->control) | SMR);
-		SSYNC();
-		while (!(bfin_read(&reg->intr) & SMACK)) {
+		writew(readw(&reg->control) | SMR, &reg->control);
+		while (!(readw(&reg->intr) & SMACK)) {
 			udelay(10);
 			if (--timeout == 0) {
 				netdev_err(dev, "fail to enter sleep mode\n");
@@ -663,8 +760,7 @@ static int bfin_can_resume(struct platform_device *pdev)
 
 	if (netif_running(dev)) {
 		/* leave sleep mode */
-		bfin_write(&reg->intr, 0);
-		SSYNC();
+		writew(0, &reg->intr);
 	}
 
 	return 0;
diff --git a/drivers/net/can/cc770/cc770_platform.c b/drivers/net/can/cc770/cc770_platform.c
index b1e8851d3cc4..866e5e12fdd2 100644
--- a/drivers/net/can/cc770/cc770_platform.c
+++ b/drivers/net/can/cc770/cc770_platform.c
@@ -254,7 +254,7 @@ static int cc770_platform_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id cc770_platform_table[] = {
+static const struct of_device_id cc770_platform_table[] = {
 	{.compatible = "bosch,cc770"}, /* CC770 from Bosch */
 	{.compatible = "intc,82527"},  /* AN82527 from Intel CP */
 	{},
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/grcan.c b/drivers/net/can/grcan.c
index fed1bbd0b0d2..e3d7e22a4fa0 100644
--- a/drivers/net/can/grcan.c
+++ b/drivers/net/can/grcan.c
@@ -1725,7 +1725,7 @@ static int grcan_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id grcan_match[] = {
+static const struct of_device_id grcan_match[] = {
 	{.name = "GAISLER_GRCAN"},
 	{.name = "01_03d"},
 	{.name = "GAISLER_GRHCAN"},
diff --git a/drivers/net/can/led.c b/drivers/net/can/led.c
index ab7f1b01be49..c1b667675fa1 100644
--- a/drivers/net/can/led.c
+++ b/drivers/net/can/led.c
@@ -30,20 +30,28 @@ void can_led_event(struct net_device *netdev, enum can_led_event event)
 	case CAN_LED_EVENT_OPEN:
 		led_trigger_event(priv->tx_led_trig, LED_FULL);
 		led_trigger_event(priv->rx_led_trig, LED_FULL);
+		led_trigger_event(priv->rxtx_led_trig, LED_FULL);
 		break;
 	case CAN_LED_EVENT_STOP:
 		led_trigger_event(priv->tx_led_trig, LED_OFF);
 		led_trigger_event(priv->rx_led_trig, LED_OFF);
+		led_trigger_event(priv->rxtx_led_trig, LED_OFF);
 		break;
 	case CAN_LED_EVENT_TX:
-		if (led_delay)
+		if (led_delay) {
 			led_trigger_blink_oneshot(priv->tx_led_trig,
 						  &led_delay, &led_delay, 1);
+			led_trigger_blink_oneshot(priv->rxtx_led_trig,
+						  &led_delay, &led_delay, 1);
+		}
 		break;
 	case CAN_LED_EVENT_RX:
-		if (led_delay)
+		if (led_delay) {
 			led_trigger_blink_oneshot(priv->rx_led_trig,
 						  &led_delay, &led_delay, 1);
+			led_trigger_blink_oneshot(priv->rxtx_led_trig,
+						  &led_delay, &led_delay, 1);
+		}
 		break;
 	}
 }
@@ -55,6 +63,7 @@ static void can_led_release(struct device *gendev, void *res)
 
 	led_trigger_unregister_simple(priv->tx_led_trig);
 	led_trigger_unregister_simple(priv->rx_led_trig);
+	led_trigger_unregister_simple(priv->rxtx_led_trig);
 }
 
 /* Register CAN LED triggers for a CAN device
@@ -76,11 +85,15 @@ void devm_can_led_init(struct net_device *netdev)
 		 "%s-tx", netdev->name);
 	snprintf(priv->rx_led_trig_name, sizeof(priv->rx_led_trig_name),
 		 "%s-rx", netdev->name);
+	snprintf(priv->rxtx_led_trig_name, sizeof(priv->rxtx_led_trig_name),
+		 "%s-rxtx", netdev->name);
 
 	led_trigger_register_simple(priv->tx_led_trig_name,
 				    &priv->tx_led_trig);
 	led_trigger_register_simple(priv->rx_led_trig_name,
 				    &priv->rx_led_trig);
+	led_trigger_register_simple(priv->rxtx_led_trig_name,
+				    &priv->rxtx_led_trig);
 
 	devres_add(&netdev->dev, res);
 }
@@ -97,7 +110,7 @@ static int can_led_notifier(struct notifier_block *nb, unsigned long msg,
 	if (!priv)
 		return NOTIFY_DONE;
 
-	if (!priv->tx_led_trig || !priv->rx_led_trig)
+	if (!priv->tx_led_trig || !priv->rx_led_trig || !priv->rxtx_led_trig)
 		return NOTIFY_DONE;
 
 	if (msg == NETDEV_CHANGENAME) {
@@ -106,6 +119,9 @@ static int can_led_notifier(struct notifier_block *nb, unsigned long msg,
 
 		snprintf(name, sizeof(name), "%s-rx", netdev->name);
 		led_trigger_rename_static(name, priv->rx_led_trig);
+
+		snprintf(name, sizeof(name), "%s-rxtx", netdev->name);
+		led_trigger_rename_static(name, priv->rxtx_led_trig);
 	}
 
 	return NOTIFY_DONE;
diff --git a/drivers/net/can/m_can/m_can.c b/drivers/net/can/m_can/m_can.c
index 2e04b3aeeb37..ef655177bb5e 100644
--- a/drivers/net/can/m_can/m_can.c
+++ b/drivers/net/can/m_can/m_can.c
@@ -312,8 +312,8 @@ static inline u32 m_can_fifo_read(const struct m_can_priv *priv,
 static inline void m_can_fifo_write(const struct m_can_priv *priv,
 				    u32 fpi, unsigned int offset, u32 val)
 {
-	return writel(val, priv->mram_base + priv->mcfg[MRAM_TXB].off +
-		      fpi * TXB_ELEMENT_SIZE + offset);
+	writel(val, priv->mram_base + priv->mcfg[MRAM_TXB].off +
+	       fpi * TXB_ELEMENT_SIZE + offset);
 }
 
 static inline void m_can_config_endisable(const struct m_can_priv *priv,
diff --git a/drivers/net/can/mscan/mpc5xxx_can.c b/drivers/net/can/mscan/mpc5xxx_can.c
index ad024e60ba8c..c7427bdd3a4b 100644
--- a/drivers/net/can/mscan/mpc5xxx_can.c
+++ b/drivers/net/can/mscan/mpc5xxx_can.c
@@ -43,7 +43,7 @@ struct mpc5xxx_can_data {
 };
 
 #ifdef CONFIG_PPC_MPC52xx
-static struct of_device_id mpc52xx_cdm_ids[] = {
+static const struct of_device_id mpc52xx_cdm_ids[] = {
 	{ .compatible = "fsl,mpc5200-cdm", },
 	{}
 };
diff --git a/drivers/net/can/sja1000/sja1000_platform.c b/drivers/net/can/sja1000/sja1000_platform.c
index 93115250eaf5..0552ed46a206 100644
--- a/drivers/net/can/sja1000/sja1000_platform.c
+++ b/drivers/net/can/sja1000/sja1000_platform.c
@@ -242,7 +242,7 @@ static int sp_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id sp_of_table[] = {
+static const struct of_device_id sp_of_table[] = {
 	{.compatible = "nxp,sja1000"},
 	{},
 };
diff --git a/drivers/net/can/usb/ems_usb.c b/drivers/net/can/usb/ems_usb.c
index 9376f5e5b94e..866bac0ae7e9 100644
--- a/drivers/net/can/usb/ems_usb.c
+++ b/drivers/net/can/usb/ems_usb.c
@@ -123,7 +123,7 @@ MODULE_LICENSE("GPL v2");
  * CPC_MSG_TYPE_EXT_CAN_FRAME or CPC_MSG_TYPE_EXT_RTR_FRAME.
  */
 struct cpc_can_msg {
-	u32 id;
+	__le32 id;
 	u8 length;
 	u8 msg[8];
 };
@@ -200,8 +200,8 @@ struct __packed ems_cpc_msg {
 	u8 type;	/* type of message */
 	u8 length;	/* length of data within union 'msg' */
 	u8 msgid;	/* confirmation handle */
-	u32 ts_sec;	/* timestamp in seconds */
-	u32 ts_nsec;	/* timestamp in nano seconds */
+	__le32 ts_sec;	/* timestamp in seconds */
+	__le32 ts_nsec;	/* timestamp in nano seconds */
 
 	union {
 		u8 generic[64];
@@ -765,7 +765,7 @@ static netdev_tx_t ems_usb_start_xmit(struct sk_buff *skb, struct net_device *ne
 
 	msg = (struct ems_cpc_msg *)&buf[CPC_HEADER_SIZE];
 
-	msg->msg.can_msg.id = cf->can_id & CAN_ERR_MASK;
+	msg->msg.can_msg.id = cpu_to_le32(cf->can_id & CAN_ERR_MASK);
 	msg->msg.can_msg.length = cf->can_dlc;
 
 	if (cf->can_id & CAN_RTR_FLAG) {
@@ -783,9 +783,6 @@ static netdev_tx_t ems_usb_start_xmit(struct sk_buff *skb, struct net_device *ne
 		msg->length = CPC_CAN_MSG_MIN_SIZE + cf->can_dlc;
 	}
 
-	/* Respect byte order */
-	msg->msg.can_msg.id = cpu_to_le32(msg->msg.can_msg.id);
-
 	for (i = 0; i < MAX_TX_URBS; i++) {
 		if (dev->tx_contexts[i].echo_index == MAX_TX_URBS) {
 			context = &dev->tx_contexts[i];
diff --git a/drivers/net/can/usb/esd_usb2.c b/drivers/net/can/usb/esd_usb2.c
index bacca0bd89c1..411c1af92c62 100644
--- a/drivers/net/can/usb/esd_usb2.c
+++ b/drivers/net/can/usb/esd_usb2.c
@@ -139,7 +139,7 @@ struct tx_msg {
 	u8 cmd;
 	u8 net;
 	u8 dlc;
-	__le32 hnd;
+	u32 hnd;	/* opaque handle, not used by device */
 	__le32 id; /* upper 3 bits contain flags */
 	u8 data[8];
 };
@@ -149,7 +149,7 @@ struct tx_done_msg {
 	u8 cmd;
 	u8 net;
 	u8 status;
-	__le32 hnd;
+	u32 hnd;	/* opaque handle, not used by device */
 	__le32 ts;
 };
 
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 a316fa4b91ab..4643914859b2 100644
--- a/drivers/net/can/usb/kvaser_usb.c
+++ b/drivers/net/can/usb/kvaser_usb.c
@@ -14,6 +14,7 @@
  * Copyright (C) 2015 Valeo S.A.
  */
 
+#include <linux/spinlock.h>
 #include <linux/kernel.h>
 #include <linux/completion.h>
 #include <linux/module.h>
@@ -24,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 */
@@ -442,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;
@@ -455,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;
@@ -466,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[] = {
@@ -590,8 +596,8 @@ static int kvaser_usb_wait_msg(const struct kvaser_usb *dev, u8 id,
 			 * for further details.
 			 */
 			if (tmp->len == 0) {
-				pos = round_up(pos,
-					       dev->bulk_in->wMaxPacketSize);
+				pos = round_up(pos, le16_to_cpu(dev->bulk_in->
+								wMaxPacketSize));
 				continue;
 			}
 
@@ -655,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;
 	}
 
@@ -694,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;
@@ -712,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 &&
@@ -737,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)
@@ -803,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)
@@ -1334,7 +1337,8 @@ static void kvaser_usb_read_bulk_callback(struct urb *urb)
 		 * number of events in case of a heavy rx load on the bus.
 		 */
 		if (msg->len == 0) {
-			pos = round_up(pos, dev->bulk_in->wMaxPacketSize);
+			pos = round_up(pos, le16_to_cpu(dev->bulk_in->
+							wMaxPacketSize));
 			continue;
 		}
 
@@ -1515,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;
@@ -1634,6 +1658,7 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 	struct kvaser_msg *msg;
 	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;
@@ -1687,12 +1712,21 @@ 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) {
@@ -1704,7 +1738,6 @@ static netdev_tx_t kvaser_usb_start_xmit(struct sk_buff *skb,
 	}
 
 	context->priv = priv;
-	context->echo_index = i;
 	context->dlc = cf->can_dlc;
 
 	msg->u.tx_can.tid = context->echo_index;
@@ -1716,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++;
@@ -1835,7 +1867,7 @@ static void kvaser_usb_remove_interfaces(struct kvaser_usb *dev)
 		if (!dev->nets[i])
 			continue;
 
-		unregister_netdev(dev->nets[i]->netdev);
+		unregister_candev(dev->nets[i]->netdev);
 	}
 
 	kvaser_usb_unlink_all_urbs(dev);
@@ -1854,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;
@@ -1868,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;
@@ -1990,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,
@@ -1997,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 0bac0f14edc3..09d14e70abd7 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;
 };
@@ -182,7 +182,7 @@ static inline void *pcan_usb_fd_cmd_buffer(struct peak_usb_device *dev)
 static int pcan_usb_fd_send_cmd(struct peak_usb_device *dev, void *cmd_tail)
 {
 	void *cmd_head = pcan_usb_fd_cmd_buffer(dev);
-	int err;
+	int err = 0;
 	u8 *packet_ptr;
 	int i, n = 1, packet_len;
 	ptrdiff_t cmd_len;
@@ -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);
@@ -937,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/can/xilinx_can.c b/drivers/net/can/xilinx_can.c
index 6c6764312285..6bddfe062b51 100644
--- a/drivers/net/can/xilinx_can.c
+++ b/drivers/net/can/xilinx_can.c
@@ -1185,7 +1185,7 @@ static int xcan_remove(struct platform_device *pdev)
 }
 
 /* Match table for OF platform binding */
-static struct of_device_id xcan_of_match[] = {
+static const struct of_device_id xcan_of_match[] = {
 	{ .compatible = "xlnx,zynq-can-1.0", },
 	{ .compatible = "xlnx,axi-can-1.00.a", },
 	{ /* end of list */ },
diff --git a/drivers/net/dsa/Kconfig b/drivers/net/dsa/Kconfig
index 48e62a34f7f2..18550c7ebe6f 100644
--- a/drivers/net/dsa/Kconfig
+++ b/drivers/net/dsa/Kconfig
@@ -7,7 +7,7 @@ config NET_DSA_MV88E6XXX
 
 config NET_DSA_MV88E6060
 	tristate "Marvell 88E6060 ethernet switch chip support"
-	select NET_DSA
+	depends on NET_DSA
 	select NET_DSA_TAG_TRAILER
 	---help---
 	  This enables support for the Marvell 88E6060 ethernet switch
@@ -19,7 +19,7 @@ config NET_DSA_MV88E6XXX_NEED_PPU
 
 config NET_DSA_MV88E6131
 	tristate "Marvell 88E6085/6095/6095F/6131 ethernet switch chip support"
-	select NET_DSA
+	depends on NET_DSA
 	select NET_DSA_MV88E6XXX
 	select NET_DSA_MV88E6XXX_NEED_PPU
 	select NET_DSA_TAG_DSA
@@ -29,7 +29,7 @@ config NET_DSA_MV88E6131
 
 config NET_DSA_MV88E6123_61_65
 	tristate "Marvell 88E6123/6161/6165 ethernet switch chip support"
-	select NET_DSA
+	depends on NET_DSA
 	select NET_DSA_MV88E6XXX
 	select NET_DSA_TAG_EDSA
 	---help---
@@ -38,7 +38,7 @@ config NET_DSA_MV88E6123_61_65
 
 config NET_DSA_MV88E6171
 	tristate "Marvell 88E6171/6172 ethernet switch chip support"
-	select NET_DSA
+	depends on NET_DSA
 	select NET_DSA_MV88E6XXX
 	select NET_DSA_TAG_EDSA
 	---help---
@@ -47,7 +47,7 @@ config NET_DSA_MV88E6171
 
 config NET_DSA_MV88E6352
 	tristate "Marvell 88E6176/88E6352 ethernet switch chip support"
-	select NET_DSA
+	depends on NET_DSA
 	select NET_DSA_MV88E6XXX
 	select NET_DSA_TAG_EDSA
 	---help---
@@ -56,8 +56,7 @@ config NET_DSA_MV88E6352
 
 config NET_DSA_BCM_SF2
 	tristate "Broadcom Starfighter 2 Ethernet switch support"
-	depends on HAS_IOMEM
-	select NET_DSA
+	depends on HAS_IOMEM && NET_DSA
 	select NET_DSA_TAG_BRCM
 	select FIXED_PHY
 	select BCM7XXX_PHY
diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c
index 4daffb284931..cedb572bf25a 100644
--- a/drivers/net/dsa/bcm_sf2.c
+++ b/drivers/net/dsa/bcm_sf2.c
@@ -23,6 +23,7 @@
 #include <linux/of_address.h>
 #include <net/dsa.h>
 #include <linux/ethtool.h>
+#include <linux/if_bridge.h>
 
 #include "bcm_sf2.h"
 #include "bcm_sf2_regs.h"
@@ -299,10 +300,14 @@ static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
 	if (port == 7)
 		intrl2_1_mask_clear(priv, P_IRQ_MASK(P7_IRQ_OFF));
 
-	/* Set this port, and only this one to be in the default VLAN */
+	/* Set this port, and only this one to be in the default VLAN,
+	 * if member of a bridge, restore its membership prior to
+	 * bringing down this port.
+	 */
 	reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(port));
 	reg &= ~PORT_VLAN_CTRL_MASK;
 	reg |= (1 << port);
+	reg |= priv->port_sts[port].vlan_ctl_mask;
 	core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(port));
 
 	bcm_sf2_imp_vlan_setup(ds, cpu_port);
@@ -400,6 +405,151 @@ static int bcm_sf2_sw_set_eee(struct dsa_switch *ds, int port,
 	return 0;
 }
 
+/* Fast-ageing of ARL entries for a given port, equivalent to an ARL
+ * flush for that port.
+ */
+static int bcm_sf2_sw_fast_age_port(struct dsa_switch  *ds, int port)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	unsigned int timeout = 1000;
+	u32 reg;
+
+	core_writel(priv, port, CORE_FAST_AGE_PORT);
+
+	reg = core_readl(priv, CORE_FAST_AGE_CTRL);
+	reg |= EN_AGE_PORT | FAST_AGE_STR_DONE;
+	core_writel(priv, reg, CORE_FAST_AGE_CTRL);
+
+	do {
+		reg = core_readl(priv, CORE_FAST_AGE_CTRL);
+		if (!(reg & FAST_AGE_STR_DONE))
+			break;
+
+		cpu_relax();
+	} while (timeout--);
+
+	if (!timeout)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int bcm_sf2_sw_br_join(struct dsa_switch *ds, int port,
+			      u32 br_port_mask)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	unsigned int i;
+	u32 reg, p_ctl;
+
+	p_ctl = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(port));
+
+	for (i = 0; i < priv->hw_params.num_ports; i++) {
+		if (!((1 << i) & br_port_mask))
+			continue;
+
+		/* Add this local port to the remote port VLAN control
+		 * membership and update the remote port bitmask
+		 */
+		reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(i));
+		reg |= 1 << port;
+		core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(i));
+		priv->port_sts[i].vlan_ctl_mask = reg;
+
+		p_ctl |= 1 << i;
+	}
+
+	/* Configure the local port VLAN control membership to include
+	 * remote ports and update the local port bitmask
+	 */
+	core_writel(priv, p_ctl, CORE_PORT_VLAN_CTL_PORT(port));
+	priv->port_sts[port].vlan_ctl_mask = p_ctl;
+
+	return 0;
+}
+
+static int bcm_sf2_sw_br_leave(struct dsa_switch *ds, int port,
+			       u32 br_port_mask)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	unsigned int i;
+	u32 reg, p_ctl;
+
+	p_ctl = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(port));
+
+	for (i = 0; i < priv->hw_params.num_ports; i++) {
+		/* Don't touch the remaining ports */
+		if (!((1 << i) & br_port_mask))
+			continue;
+
+		reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(i));
+		reg &= ~(1 << port);
+		core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(i));
+		priv->port_sts[port].vlan_ctl_mask = reg;
+
+		/* Prevent self removal to preserve isolation */
+		if (port != i)
+			p_ctl &= ~(1 << i);
+	}
+
+	core_writel(priv, p_ctl, CORE_PORT_VLAN_CTL_PORT(port));
+	priv->port_sts[port].vlan_ctl_mask = p_ctl;
+
+	return 0;
+}
+
+static int bcm_sf2_sw_br_set_stp_state(struct dsa_switch *ds, int port,
+				       u8 state)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	u8 hw_state, cur_hw_state;
+	int ret = 0;
+	u32 reg;
+
+	reg = core_readl(priv, CORE_G_PCTL_PORT(port));
+	cur_hw_state = reg >> G_MISTP_STATE_SHIFT;
+
+	switch (state) {
+	case BR_STATE_DISABLED:
+		hw_state = G_MISTP_DIS_STATE;
+		break;
+	case BR_STATE_LISTENING:
+		hw_state = G_MISTP_LISTEN_STATE;
+		break;
+	case BR_STATE_LEARNING:
+		hw_state = G_MISTP_LEARN_STATE;
+		break;
+	case BR_STATE_FORWARDING:
+		hw_state = G_MISTP_FWD_STATE;
+		break;
+	case BR_STATE_BLOCKING:
+		hw_state = G_MISTP_BLOCK_STATE;
+		break;
+	default:
+		pr_err("%s: invalid STP state: %d\n", __func__, state);
+		return -EINVAL;
+	}
+
+	/* Fast-age ARL entries if we are moving a port from Learning or
+	 * Forwarding state to Disabled, Blocking or Listening state
+	 */
+	if (cur_hw_state != hw_state) {
+		if (cur_hw_state & 4 && !(hw_state & 4)) {
+			ret = bcm_sf2_sw_fast_age_port(ds, port);
+			if (ret) {
+				pr_err("%s: fast-ageing failed\n", __func__);
+				return ret;
+			}
+		}
+	}
+
+	reg = core_readl(priv, CORE_G_PCTL_PORT(port));
+	reg &= ~(G_MISTP_STATE_MASK << G_MISTP_STATE_SHIFT);
+	reg |= hw_state;
+	core_writel(priv, reg, CORE_G_PCTL_PORT(port));
+
+	return 0;
+}
+
 static irqreturn_t bcm_sf2_switch_0_isr(int irq, void *dev_id)
 {
 	struct bcm_sf2_priv *priv = dev_id;
@@ -916,6 +1066,9 @@ static struct dsa_switch_driver bcm_sf2_switch_driver = {
 	.port_disable		= bcm_sf2_port_disable,
 	.get_eee		= bcm_sf2_sw_get_eee,
 	.set_eee		= bcm_sf2_sw_set_eee,
+	.port_join_bridge	= bcm_sf2_sw_br_join,
+	.port_leave_bridge	= bcm_sf2_sw_br_leave,
+	.port_stp_update	= bcm_sf2_sw_br_set_stp_state,
 };
 
 static int __init bcm_sf2_init(void)
diff --git a/drivers/net/dsa/bcm_sf2.h b/drivers/net/dsa/bcm_sf2.h
index 7b7053d3c5fa..22e2ebf31333 100644
--- a/drivers/net/dsa/bcm_sf2.h
+++ b/drivers/net/dsa/bcm_sf2.h
@@ -46,6 +46,8 @@ struct bcm_sf2_port_status {
 	unsigned int link;
 
 	struct ethtool_eee eee;
+
+	u32 vlan_ctl_mask;
 };
 
 struct bcm_sf2_priv {
diff --git a/drivers/net/dsa/bcm_sf2_regs.h b/drivers/net/dsa/bcm_sf2_regs.h
index cabdfa5e217a..fa4e6e78c9ea 100644
--- a/drivers/net/dsa/bcm_sf2_regs.h
+++ b/drivers/net/dsa/bcm_sf2_regs.h
@@ -163,6 +163,21 @@
 #define  EN_CHIP_RST			(1 << 6)
 #define  EN_SW_RESET			(1 << 4)
 
+#define CORE_FAST_AGE_CTRL		0x00220
+#define  EN_FAST_AGE_STATIC		(1 << 0)
+#define  EN_AGE_DYNAMIC			(1 << 1)
+#define  EN_AGE_PORT			(1 << 2)
+#define  EN_AGE_VLAN			(1 << 3)
+#define  EN_AGE_SPT			(1 << 4)
+#define  EN_AGE_MCAST			(1 << 5)
+#define  FAST_AGE_STR_DONE		(1 << 7)
+
+#define CORE_FAST_AGE_PORT		0x00224
+#define  AGE_PORT_MASK			0xf
+
+#define CORE_FAST_AGE_VID		0x00228
+#define  AGE_VID_MASK			0x3fff
+
 #define CORE_LNKSTS			0x00400
 #define  LNK_STS_MASK			0x1ff
 
diff --git a/drivers/net/dsa/mv88e6123_61_65.c b/drivers/net/dsa/mv88e6123_61_65.c
index e9c736e1cef3..b4af6d5aff7c 100644
--- a/drivers/net/dsa/mv88e6123_61_65.c
+++ b/drivers/net/dsa/mv88e6123_61_65.c
@@ -25,66 +25,33 @@ static char *mv88e6123_61_65_probe(struct device *host_dev, int sw_addr)
 	if (bus == NULL)
 		return NULL;
 
-	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
+	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), PORT_SWITCH_ID);
 	if (ret >= 0) {
-		if (ret == 0x1212)
+		if (ret == PORT_SWITCH_ID_6123_A1)
 			return "Marvell 88E6123 (A1)";
-		if (ret == 0x1213)
+		if (ret == PORT_SWITCH_ID_6123_A2)
 			return "Marvell 88E6123 (A2)";
-		if ((ret & 0xfff0) == 0x1210)
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6123)
 			return "Marvell 88E6123";
 
-		if (ret == 0x1612)
+		if (ret == PORT_SWITCH_ID_6161_A1)
 			return "Marvell 88E6161 (A1)";
-		if (ret == 0x1613)
+		if (ret == PORT_SWITCH_ID_6161_A2)
 			return "Marvell 88E6161 (A2)";
-		if ((ret & 0xfff0) == 0x1610)
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6161)
 			return "Marvell 88E6161";
 
-		if (ret == 0x1652)
+		if (ret == PORT_SWITCH_ID_6165_A1)
 			return "Marvell 88E6165 (A1)";
-		if (ret == 0x1653)
+		if (ret == PORT_SWITCH_ID_6165_A2)
 			return "Marvell 88e6165 (A2)";
-		if ((ret & 0xfff0) == 0x1650)
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6165)
 			return "Marvell 88E6165";
 	}
 
 	return NULL;
 }
 
-static int mv88e6123_61_65_switch_reset(struct dsa_switch *ds)
-{
-	int i;
-	int ret;
-	unsigned long timeout;
-
-	/* Set all ports to the disabled state. */
-	for (i = 0; i < 8; i++) {
-		ret = REG_READ(REG_PORT(i), 0x04);
-		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
-	}
-
-	/* Wait for transmit queues to drain. */
-	usleep_range(2000, 4000);
-
-	/* Reset the switch. */
-	REG_WRITE(REG_GLOBAL, 0x04, 0xc400);
-
-	/* Wait up to one second for reset to complete. */
-	timeout = jiffies + 1 * HZ;
-	while (time_before(jiffies, timeout)) {
-		ret = REG_READ(REG_GLOBAL, 0x00);
-		if ((ret & 0xc800) == 0xc800)
-			break;
-
-		usleep_range(1000, 2000);
-	}
-	if (time_after(jiffies, timeout))
-		return -ETIMEDOUT;
-
-	return 0;
-}
-
 static int mv88e6123_61_65_setup_global(struct dsa_switch *ds)
 {
 	int ret;
@@ -222,28 +189,6 @@ static int mv88e6123_61_65_setup_port(struct dsa_switch *ds, int p)
 		val |= 0x000c;
 	REG_WRITE(addr, 0x04, val);
 
-	/* Port Control 1: disable trunking.  Also, if this is the
-	 * CPU port, enable learn messages to be sent to this port.
-	 */
-	REG_WRITE(addr, 0x05, dsa_is_cpu_port(ds, p) ? 0x8000 : 0x0000);
-
-	/* Port based VLAN map: give each port its own address
-	 * database, allow the CPU port to talk to each of the 'real'
-	 * ports, and allow each of the 'real' ports to only talk to
-	 * the upstream port.
-	 */
-	val = (p & 0xf) << 12;
-	if (dsa_is_cpu_port(ds, p))
-		val |= ds->phys_port_mask;
-	else
-		val |= 1 << dsa_upstream_port(ds);
-	REG_WRITE(addr, 0x06, val);
-
-	/* Default VLAN ID and priority: don't set a default VLAN
-	 * ID, and set the default packet priority to zero.
-	 */
-	REG_WRITE(addr, 0x07, 0x0000);
-
 	/* Port Control 2: don't force a good FCS, set the maximum
 	 * frame size to 10240 bytes, don't let the switch add or
 	 * strip 802.1q tags, don't discard tagged or untagged frames
@@ -288,7 +233,7 @@ static int mv88e6123_61_65_setup_port(struct dsa_switch *ds, int p)
 	 */
 	REG_WRITE(addr, 0x19, 0x7654);
 
-	return 0;
+	return mv88e6xxx_setup_port_common(ds, p);
 }
 
 static int mv88e6123_61_65_setup(struct dsa_switch *ds)
@@ -297,11 +242,23 @@ static int mv88e6123_61_65_setup(struct dsa_switch *ds)
 	int i;
 	int ret;
 
-	mutex_init(&ps->smi_mutex);
-	mutex_init(&ps->stats_mutex);
-	mutex_init(&ps->phy_mutex);
+	ret = mv88e6xxx_setup_common(ds);
+	if (ret < 0)
+		return ret;
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6123:
+		ps->num_ports = 3;
+		break;
+	case PORT_SWITCH_ID_6161:
+	case PORT_SWITCH_ID_6165:
+		ps->num_ports = 6;
+		break;
+	default:
+		return -ENODEV;
+	}
 
-	ret = mv88e6123_61_65_switch_reset(ds);
+	ret = mv88e6xxx_switch_reset(ds, false);
 	if (ret < 0)
 		return ret;
 
@@ -311,7 +268,7 @@ static int mv88e6123_61_65_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < 6; i++) {
+	for (i = 0; i < ps->num_ports; i++) {
 		ret = mv88e6123_61_65_setup_port(ds, i);
 		if (ret < 0)
 			return ret;
@@ -320,108 +277,18 @@ static int mv88e6123_61_65_setup(struct dsa_switch *ds)
 	return 0;
 }
 
-static int mv88e6123_61_65_port_to_phy_addr(int port)
-{
-	if (port >= 0 && port <= 4)
-		return port;
-	return -1;
-}
-
-static int
-mv88e6123_61_65_phy_read(struct dsa_switch *ds, int port, int regnum)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int addr = mv88e6123_61_65_port_to_phy_addr(port);
-	int ret;
-
-	mutex_lock(&ps->phy_mutex);
-	ret = mv88e6xxx_phy_read(ds, addr, regnum);
-	mutex_unlock(&ps->phy_mutex);
-	return ret;
-}
-
-static int
-mv88e6123_61_65_phy_write(struct dsa_switch *ds,
-			      int port, int regnum, u16 val)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int addr = mv88e6123_61_65_port_to_phy_addr(port);
-	int ret;
-
-	mutex_lock(&ps->phy_mutex);
-	ret = mv88e6xxx_phy_write(ds, addr, regnum, val);
-	mutex_unlock(&ps->phy_mutex);
-	return ret;
-}
-
-static struct mv88e6xxx_hw_stat mv88e6123_61_65_hw_stats[] = {
-	{ "in_good_octets", 8, 0x00, },
-	{ "in_bad_octets", 4, 0x02, },
-	{ "in_unicast", 4, 0x04, },
-	{ "in_broadcasts", 4, 0x06, },
-	{ "in_multicasts", 4, 0x07, },
-	{ "in_pause", 4, 0x16, },
-	{ "in_undersize", 4, 0x18, },
-	{ "in_fragments", 4, 0x19, },
-	{ "in_oversize", 4, 0x1a, },
-	{ "in_jabber", 4, 0x1b, },
-	{ "in_rx_error", 4, 0x1c, },
-	{ "in_fcs_error", 4, 0x1d, },
-	{ "out_octets", 8, 0x0e, },
-	{ "out_unicast", 4, 0x10, },
-	{ "out_broadcasts", 4, 0x13, },
-	{ "out_multicasts", 4, 0x12, },
-	{ "out_pause", 4, 0x15, },
-	{ "excessive", 4, 0x11, },
-	{ "collisions", 4, 0x1e, },
-	{ "deferred", 4, 0x05, },
-	{ "single", 4, 0x14, },
-	{ "multiple", 4, 0x17, },
-	{ "out_fcs_error", 4, 0x03, },
-	{ "late", 4, 0x1f, },
-	{ "hist_64bytes", 4, 0x08, },
-	{ "hist_65_127bytes", 4, 0x09, },
-	{ "hist_128_255bytes", 4, 0x0a, },
-	{ "hist_256_511bytes", 4, 0x0b, },
-	{ "hist_512_1023bytes", 4, 0x0c, },
-	{ "hist_1024_max_bytes", 4, 0x0d, },
-	{ "sw_in_discards", 4, 0x110, },
-	{ "sw_in_filtered", 2, 0x112, },
-	{ "sw_out_filtered", 2, 0x113, },
-};
-
-static void
-mv88e6123_61_65_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
-{
-	mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6123_61_65_hw_stats),
-			      mv88e6123_61_65_hw_stats, port, data);
-}
-
-static void
-mv88e6123_61_65_get_ethtool_stats(struct dsa_switch *ds,
-				  int port, uint64_t *data)
-{
-	mv88e6xxx_get_ethtool_stats(ds, ARRAY_SIZE(mv88e6123_61_65_hw_stats),
-				    mv88e6123_61_65_hw_stats, port, data);
-}
-
-static int mv88e6123_61_65_get_sset_count(struct dsa_switch *ds)
-{
-	return ARRAY_SIZE(mv88e6123_61_65_hw_stats);
-}
-
 struct dsa_switch_driver mv88e6123_61_65_switch_driver = {
 	.tag_protocol		= DSA_TAG_PROTO_EDSA,
 	.priv_size		= sizeof(struct mv88e6xxx_priv_state),
 	.probe			= mv88e6123_61_65_probe,
 	.setup			= mv88e6123_61_65_setup,
 	.set_addr		= mv88e6xxx_set_addr_indirect,
-	.phy_read		= mv88e6123_61_65_phy_read,
-	.phy_write		= mv88e6123_61_65_phy_write,
+	.phy_read		= mv88e6xxx_phy_read,
+	.phy_write		= mv88e6xxx_phy_write,
 	.poll_link		= mv88e6xxx_poll_link,
-	.get_strings		= mv88e6123_61_65_get_strings,
-	.get_ethtool_stats	= mv88e6123_61_65_get_ethtool_stats,
-	.get_sset_count		= mv88e6123_61_65_get_sset_count,
+	.get_strings		= mv88e6xxx_get_strings,
+	.get_ethtool_stats	= mv88e6xxx_get_ethtool_stats,
+	.get_sset_count		= mv88e6xxx_get_sset_count,
 #ifdef CONFIG_NET_DSA_HWMON
 	.get_temp		= mv88e6xxx_get_temp,
 #endif
diff --git a/drivers/net/dsa/mv88e6131.c b/drivers/net/dsa/mv88e6131.c
index 2540ef0142af..e54824fa0d95 100644
--- a/drivers/net/dsa/mv88e6131.c
+++ b/drivers/net/dsa/mv88e6131.c
@@ -17,12 +17,6 @@
 #include <net/dsa.h>
 #include "mv88e6xxx.h"
 
-/* Switch product IDs */
-#define ID_6085		0x04a0
-#define ID_6095		0x0950
-#define ID_6131		0x1060
-#define ID_6131_B2	0x1066
-
 static char *mv88e6131_probe(struct device *host_dev, int sw_addr)
 {
 	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
@@ -31,56 +25,23 @@ static char *mv88e6131_probe(struct device *host_dev, int sw_addr)
 	if (bus == NULL)
 		return NULL;
 
-	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
+	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), PORT_SWITCH_ID);
 	if (ret >= 0) {
 		int ret_masked = ret & 0xfff0;
 
-		if (ret_masked == ID_6085)
+		if (ret_masked == PORT_SWITCH_ID_6085)
 			return "Marvell 88E6085";
-		if (ret_masked == ID_6095)
+		if (ret_masked == PORT_SWITCH_ID_6095)
 			return "Marvell 88E6095/88E6095F";
-		if (ret == ID_6131_B2)
+		if (ret == PORT_SWITCH_ID_6131_B2)
 			return "Marvell 88E6131 (B2)";
-		if (ret_masked == ID_6131)
+		if (ret_masked == PORT_SWITCH_ID_6131)
 			return "Marvell 88E6131";
 	}
 
 	return NULL;
 }
 
-static int mv88e6131_switch_reset(struct dsa_switch *ds)
-{
-	int i;
-	int ret;
-	unsigned long timeout;
-
-	/* Set all ports to the disabled state. */
-	for (i = 0; i < 11; i++) {
-		ret = REG_READ(REG_PORT(i), 0x04);
-		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
-	}
-
-	/* Wait for transmit queues to drain. */
-	usleep_range(2000, 4000);
-
-	/* Reset the switch. */
-	REG_WRITE(REG_GLOBAL, 0x04, 0xc400);
-
-	/* Wait up to one second for reset to complete. */
-	timeout = jiffies + 1 * HZ;
-	while (time_before(jiffies, timeout)) {
-		ret = REG_READ(REG_GLOBAL, 0x00);
-		if ((ret & 0xc800) == 0xc800)
-			break;
-
-		usleep_range(1000, 2000);
-	}
-	if (time_after(jiffies, timeout))
-		return -ETIMEDOUT;
-
-	return 0;
-}
-
 static int mv88e6131_setup_global(struct dsa_switch *ds)
 {
 	int ret;
@@ -174,7 +135,7 @@ static int mv88e6131_setup_port(struct dsa_switch *ds, int p)
 	 * (100 Mb/s on 6085) full duplex.
 	 */
 	if (dsa_is_cpu_port(ds, p) || ds->dsa_port_mask & (1 << p))
-		if (ps->id == ID_6085)
+		if (ps->id == PORT_SWITCH_ID_6085)
 			REG_WRITE(addr, 0x01, 0x003d); /* 100 Mb/s */
 		else
 			REG_WRITE(addr, 0x01, 0x003e); /* 1000 Mb/s */
@@ -201,35 +162,13 @@ static int mv88e6131_setup_port(struct dsa_switch *ds, int p)
 		/* On 6085, unknown multicast forward is controlled
 		 * here rather than in Port Control 2 register.
 		 */
-		if (ps->id == ID_6085)
+		if (ps->id == PORT_SWITCH_ID_6085)
 			val |= 0x0008;
 	}
 	if (ds->dsa_port_mask & (1 << p))
 		val |= 0x0100;
 	REG_WRITE(addr, 0x04, val);
 
-	/* Port Control 1: disable trunking.  Also, if this is the
-	 * CPU port, enable learn messages to be sent to this port.
-	 */
-	REG_WRITE(addr, 0x05, dsa_is_cpu_port(ds, p) ? 0x8000 : 0x0000);
-
-	/* Port based VLAN map: give each port its own address
-	 * database, allow the CPU port to talk to each of the 'real'
-	 * ports, and allow each of the 'real' ports to only talk to
-	 * the upstream port.
-	 */
-	val = (p & 0xf) << 12;
-	if (dsa_is_cpu_port(ds, p))
-		val |= ds->phys_port_mask;
-	else
-		val |= 1 << dsa_upstream_port(ds);
-	REG_WRITE(addr, 0x06, val);
-
-	/* Default VLAN ID and priority: don't set a default VLAN
-	 * ID, and set the default packet priority to zero.
-	 */
-	REG_WRITE(addr, 0x07, 0x0000);
-
 	/* Port Control 2: don't force a good FCS, don't use
 	 * VLAN-based, source address-based or destination
 	 * address-based priority overrides, don't let the switch
@@ -242,7 +181,7 @@ static int mv88e6131_setup_port(struct dsa_switch *ds, int p)
 	 * If this is the upstream port for this switch, enable
 	 * forwarding of unknown multicast addresses.
 	 */
-	if (ps->id == ID_6085)
+	if (ps->id == PORT_SWITCH_ID_6085)
 		/* on 6085, bits 3:0 are reserved, bit 6 control ARP
 		 * mirroring, and multicast forward is handled in
 		 * Port Control register.
@@ -278,7 +217,7 @@ static int mv88e6131_setup_port(struct dsa_switch *ds, int p)
 	 */
 	REG_WRITE(addr, 0x19, 0x7654);
 
-	return 0;
+	return mv88e6xxx_setup_port_common(ds, p);
 }
 
 static int mv88e6131_setup(struct dsa_switch *ds)
@@ -287,13 +226,28 @@ static int mv88e6131_setup(struct dsa_switch *ds)
 	int i;
 	int ret;
 
-	mutex_init(&ps->smi_mutex);
+	ret = mv88e6xxx_setup_common(ds);
+	if (ret < 0)
+		return ret;
+
 	mv88e6xxx_ppu_state_init(ds);
-	mutex_init(&ps->stats_mutex);
 
-	ps->id = REG_READ(REG_PORT(0), 0x03) & 0xfff0;
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6085:
+		ps->num_ports = 10;
+		break;
+	case PORT_SWITCH_ID_6095:
+		ps->num_ports = 11;
+		break;
+	case PORT_SWITCH_ID_6131:
+	case PORT_SWITCH_ID_6131_B2:
+		ps->num_ports = 8;
+		break;
+	default:
+		return -ENODEV;
+	}
 
-	ret = mv88e6131_switch_reset(ds);
+	ret = mv88e6xxx_switch_reset(ds, false);
 	if (ret < 0)
 		return ret;
 
@@ -303,7 +257,7 @@ static int mv88e6131_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < 11; i++) {
+	for (i = 0; i < ps->num_ports; i++) {
 		ret = mv88e6131_setup_port(ds, i);
 		if (ret < 0)
 			return ret;
@@ -312,17 +266,24 @@ static int mv88e6131_setup(struct dsa_switch *ds)
 	return 0;
 }
 
-static int mv88e6131_port_to_phy_addr(int port)
+static int mv88e6131_port_to_phy_addr(struct dsa_switch *ds, int port)
 {
-	if (port >= 0 && port <= 11)
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	if (port >= 0 && port < ps->num_ports)
 		return port;
-	return -1;
+
+	return -EINVAL;
 }
 
 static int
 mv88e6131_phy_read(struct dsa_switch *ds, int port, int regnum)
 {
-	int addr = mv88e6131_port_to_phy_addr(port);
+	int addr = mv88e6131_port_to_phy_addr(ds, port);
+
+	if (addr < 0)
+		return addr;
+
 	return mv88e6xxx_phy_read_ppu(ds, addr, regnum);
 }
 
@@ -330,61 +291,12 @@ static int
 mv88e6131_phy_write(struct dsa_switch *ds,
 			      int port, int regnum, u16 val)
 {
-	int addr = mv88e6131_port_to_phy_addr(port);
-	return mv88e6xxx_phy_write_ppu(ds, addr, regnum, val);
-}
-
-static struct mv88e6xxx_hw_stat mv88e6131_hw_stats[] = {
-	{ "in_good_octets", 8, 0x00, },
-	{ "in_bad_octets", 4, 0x02, },
-	{ "in_unicast", 4, 0x04, },
-	{ "in_broadcasts", 4, 0x06, },
-	{ "in_multicasts", 4, 0x07, },
-	{ "in_pause", 4, 0x16, },
-	{ "in_undersize", 4, 0x18, },
-	{ "in_fragments", 4, 0x19, },
-	{ "in_oversize", 4, 0x1a, },
-	{ "in_jabber", 4, 0x1b, },
-	{ "in_rx_error", 4, 0x1c, },
-	{ "in_fcs_error", 4, 0x1d, },
-	{ "out_octets", 8, 0x0e, },
-	{ "out_unicast", 4, 0x10, },
-	{ "out_broadcasts", 4, 0x13, },
-	{ "out_multicasts", 4, 0x12, },
-	{ "out_pause", 4, 0x15, },
-	{ "excessive", 4, 0x11, },
-	{ "collisions", 4, 0x1e, },
-	{ "deferred", 4, 0x05, },
-	{ "single", 4, 0x14, },
-	{ "multiple", 4, 0x17, },
-	{ "out_fcs_error", 4, 0x03, },
-	{ "late", 4, 0x1f, },
-	{ "hist_64bytes", 4, 0x08, },
-	{ "hist_65_127bytes", 4, 0x09, },
-	{ "hist_128_255bytes", 4, 0x0a, },
-	{ "hist_256_511bytes", 4, 0x0b, },
-	{ "hist_512_1023bytes", 4, 0x0c, },
-	{ "hist_1024_max_bytes", 4, 0x0d, },
-};
-
-static void
-mv88e6131_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
-{
-	mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6131_hw_stats),
-			      mv88e6131_hw_stats, port, data);
-}
+	int addr = mv88e6131_port_to_phy_addr(ds, port);
 
-static void
-mv88e6131_get_ethtool_stats(struct dsa_switch *ds,
-				  int port, uint64_t *data)
-{
-	mv88e6xxx_get_ethtool_stats(ds, ARRAY_SIZE(mv88e6131_hw_stats),
-				    mv88e6131_hw_stats, port, data);
-}
+	if (addr < 0)
+		return addr;
 
-static int mv88e6131_get_sset_count(struct dsa_switch *ds)
-{
-	return ARRAY_SIZE(mv88e6131_hw_stats);
+	return mv88e6xxx_phy_write_ppu(ds, addr, regnum, val);
 }
 
 struct dsa_switch_driver mv88e6131_switch_driver = {
@@ -396,9 +308,9 @@ struct dsa_switch_driver mv88e6131_switch_driver = {
 	.phy_read		= mv88e6131_phy_read,
 	.phy_write		= mv88e6131_phy_write,
 	.poll_link		= mv88e6xxx_poll_link,
-	.get_strings		= mv88e6131_get_strings,
-	.get_ethtool_stats	= mv88e6131_get_ethtool_stats,
-	.get_sset_count		= mv88e6131_get_sset_count,
+	.get_strings		= mv88e6xxx_get_strings,
+	.get_ethtool_stats	= mv88e6xxx_get_ethtool_stats,
+	.get_sset_count		= mv88e6xxx_get_sset_count,
 };
 
 MODULE_ALIAS("platform:mv88e6085");
diff --git a/drivers/net/dsa/mv88e6171.c b/drivers/net/dsa/mv88e6171.c
index aa33d16f2e22..9104efea0e3e 100644
--- a/drivers/net/dsa/mv88e6171.c
+++ b/drivers/net/dsa/mv88e6171.c
@@ -25,69 +25,27 @@ static char *mv88e6171_probe(struct device *host_dev, int sw_addr)
 	if (bus == NULL)
 		return NULL;
 
-	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
+	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), PORT_SWITCH_ID);
 	if (ret >= 0) {
-		if ((ret & 0xfff0) == 0x1710)
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6171)
 			return "Marvell 88E6171";
-		if ((ret & 0xfff0) == 0x1720)
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6172)
 			return "Marvell 88E6172";
 	}
 
 	return NULL;
 }
 
-static int mv88e6171_switch_reset(struct dsa_switch *ds)
-{
-	int i;
-	int ret;
-	unsigned long timeout;
-
-	/* Set all ports to the disabled state. */
-	for (i = 0; i < 8; i++) {
-		ret = REG_READ(REG_PORT(i), 0x04);
-		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
-	}
-
-	/* Wait for transmit queues to drain. */
-	usleep_range(2000, 4000);
-
-	/* Reset the switch. */
-	REG_WRITE(REG_GLOBAL, 0x04, 0xc400);
-
-	/* Wait up to one second for reset to complete. */
-	timeout = jiffies + 1 * HZ;
-	while (time_before(jiffies, timeout)) {
-		ret = REG_READ(REG_GLOBAL, 0x00);
-		if ((ret & 0xc800) == 0xc800)
-			break;
-
-		usleep_range(1000, 2000);
-	}
-	if (time_after(jiffies, timeout))
-		return -ETIMEDOUT;
-
-	/* Enable ports not under DSA, e.g. WAN port */
-	for (i = 0; i < 8; i++) {
-		if (dsa_is_cpu_port(ds, i) || ds->phys_port_mask & (1 << i))
-			continue;
-
-		ret = REG_READ(REG_PORT(i), 0x04);
-		REG_WRITE(REG_PORT(i), 0x04, ret | 0x03);
-	}
-
-	return 0;
-}
-
 static int mv88e6171_setup_global(struct dsa_switch *ds)
 {
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int ret;
 	int i;
 
-	/* Disable the PHY polling unit (since there won't be any
-	 * external PHYs to poll), don't discard packets with
-	 * excessive collisions, and mask all interrupt sources.
+	/* Discard packets with excessive collisions, mask all
+	 * interrupt sources, enable PPU.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x04, 0x0000);
+	REG_WRITE(REG_GLOBAL, 0x04, 0x6000);
 
 	/* Set the default address aging time to 5 minutes, and
 	 * enable address learn messages to be sent to all message
@@ -145,7 +103,7 @@ static int mv88e6171_setup_global(struct dsa_switch *ds)
 	}
 
 	/* Clear all trunk masks. */
-	for (i = 0; i < 8; i++)
+	for (i = 0; i < ps->num_ports; i++)
 		REG_WRITE(REG_GLOBAL2, 0x07, 0x8000 | (i << 12) | 0xff);
 
 	/* Clear all trunk mappings. */
@@ -219,28 +177,6 @@ static int mv88e6171_setup_port(struct dsa_switch *ds, int p)
 		val |= 0x000c;
 	REG_WRITE(addr, 0x04, val);
 
-	/* Port Control 1: disable trunking.  Also, if this is the
-	 * CPU port, enable learn messages to be sent to this port.
-	 */
-	REG_WRITE(addr, 0x05, dsa_is_cpu_port(ds, p) ? 0x8000 : 0x0000);
-
-	/* Port based VLAN map: give each port its own address
-	 * database, allow the CPU port to talk to each of the 'real'
-	 * ports, and allow each of the 'real' ports to only talk to
-	 * the upstream port.
-	 */
-	val = (p & 0xf) << 12;
-	if (dsa_is_cpu_port(ds, p))
-		val |= ds->phys_port_mask;
-	else
-		val |= 1 << dsa_upstream_port(ds);
-	REG_WRITE(addr, 0x06, val);
-
-	/* Default VLAN ID and priority: don't set a default VLAN
-	 * ID, and set the default packet priority to zero.
-	 */
-	REG_WRITE(addr, 0x07, 0x0000);
-
 	/* Port Control 2: don't force a good FCS, set the maximum
 	 * frame size to 10240 bytes, don't let the switch add or
 	 * strip 802.1q tags, don't discard tagged or untagged frames
@@ -285,19 +221,22 @@ static int mv88e6171_setup_port(struct dsa_switch *ds, int p)
 	 */
 	REG_WRITE(addr, 0x19, 0x7654);
 
-	return 0;
+	return mv88e6xxx_setup_port_common(ds, p);
 }
 
 static int mv88e6171_setup(struct dsa_switch *ds)
 {
-	struct mv88e6xxx_priv_state *ps = (void *)(ds + 1);
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int i;
 	int ret;
 
-	mutex_init(&ps->smi_mutex);
-	mutex_init(&ps->stats_mutex);
+	ret = mv88e6xxx_setup_common(ds);
+	if (ret < 0)
+		return ret;
+
+	ps->num_ports = 7;
 
-	ret = mv88e6171_switch_reset(ds);
+	ret = mv88e6xxx_switch_reset(ds, true);
 	if (ret < 0)
 		return ret;
 
@@ -307,7 +246,7 @@ static int mv88e6171_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < 8; i++) {
+	for (i = 0; i < ps->num_ports; i++) {
 		if (!(dsa_is_cpu_port(ds, i) || ds->phys_port_mask & (1 << i)))
 			continue;
 
@@ -316,96 +255,29 @@ static int mv88e6171_setup(struct dsa_switch *ds)
 			return ret;
 	}
 
-	mutex_init(&ps->phy_mutex);
-
 	return 0;
 }
 
-static int mv88e6171_port_to_phy_addr(int port)
-{
-	if (port >= 0 && port <= 4)
-		return port;
-	return -1;
-}
-
-static int
-mv88e6171_phy_read(struct dsa_switch *ds, int port, int regnum)
+static int mv88e6171_get_eee(struct dsa_switch *ds, int port,
+			     struct ethtool_eee *e)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int addr = mv88e6171_port_to_phy_addr(port);
-	int ret;
 
-	mutex_lock(&ps->phy_mutex);
-	ret = mv88e6xxx_phy_read(ds, addr, regnum);
-	mutex_unlock(&ps->phy_mutex);
-	return ret;
-}
-
-static int
-mv88e6171_phy_write(struct dsa_switch *ds,
-		    int port, int regnum, u16 val)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int addr = mv88e6171_port_to_phy_addr(port);
-	int ret;
+	if (ps->id == PORT_SWITCH_ID_6172)
+		return mv88e6xxx_get_eee(ds, port, e);
 
-	mutex_lock(&ps->phy_mutex);
-	ret = mv88e6xxx_phy_write(ds, addr, regnum, val);
-	mutex_unlock(&ps->phy_mutex);
-	return ret;
+	return -EOPNOTSUPP;
 }
 
-static struct mv88e6xxx_hw_stat mv88e6171_hw_stats[] = {
-	{ "in_good_octets", 8, 0x00, },
-	{ "in_bad_octets", 4, 0x02, },
-	{ "in_unicast", 4, 0x04, },
-	{ "in_broadcasts", 4, 0x06, },
-	{ "in_multicasts", 4, 0x07, },
-	{ "in_pause", 4, 0x16, },
-	{ "in_undersize", 4, 0x18, },
-	{ "in_fragments", 4, 0x19, },
-	{ "in_oversize", 4, 0x1a, },
-	{ "in_jabber", 4, 0x1b, },
-	{ "in_rx_error", 4, 0x1c, },
-	{ "in_fcs_error", 4, 0x1d, },
-	{ "out_octets", 8, 0x0e, },
-	{ "out_unicast", 4, 0x10, },
-	{ "out_broadcasts", 4, 0x13, },
-	{ "out_multicasts", 4, 0x12, },
-	{ "out_pause", 4, 0x15, },
-	{ "excessive", 4, 0x11, },
-	{ "collisions", 4, 0x1e, },
-	{ "deferred", 4, 0x05, },
-	{ "single", 4, 0x14, },
-	{ "multiple", 4, 0x17, },
-	{ "out_fcs_error", 4, 0x03, },
-	{ "late", 4, 0x1f, },
-	{ "hist_64bytes", 4, 0x08, },
-	{ "hist_65_127bytes", 4, 0x09, },
-	{ "hist_128_255bytes", 4, 0x0a, },
-	{ "hist_256_511bytes", 4, 0x0b, },
-	{ "hist_512_1023bytes", 4, 0x0c, },
-	{ "hist_1024_max_bytes", 4, 0x0d, },
-};
-
-static void
-mv88e6171_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
+static int mv88e6171_set_eee(struct dsa_switch *ds, int port,
+			     struct phy_device *phydev, struct ethtool_eee *e)
 {
-	mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6171_hw_stats),
-			      mv88e6171_hw_stats, port, data);
-}
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 
-static void
-mv88e6171_get_ethtool_stats(struct dsa_switch *ds,
-			    int port, uint64_t *data)
-{
-	mv88e6xxx_get_ethtool_stats(ds, ARRAY_SIZE(mv88e6171_hw_stats),
-				    mv88e6171_hw_stats, port, data);
-}
+	if (ps->id == PORT_SWITCH_ID_6172)
+		return mv88e6xxx_set_eee(ds, port, phydev, e);
 
-static int mv88e6171_get_sset_count(struct dsa_switch *ds)
-{
-	return ARRAY_SIZE(mv88e6171_hw_stats);
+	return -EOPNOTSUPP;
 }
 
 struct dsa_switch_driver mv88e6171_switch_driver = {
@@ -414,17 +286,25 @@ struct dsa_switch_driver mv88e6171_switch_driver = {
 	.probe			= mv88e6171_probe,
 	.setup			= mv88e6171_setup,
 	.set_addr		= mv88e6xxx_set_addr_indirect,
-	.phy_read		= mv88e6171_phy_read,
-	.phy_write		= mv88e6171_phy_write,
+	.phy_read		= mv88e6xxx_phy_read_indirect,
+	.phy_write		= mv88e6xxx_phy_write_indirect,
 	.poll_link		= mv88e6xxx_poll_link,
-	.get_strings		= mv88e6171_get_strings,
-	.get_ethtool_stats	= mv88e6171_get_ethtool_stats,
-	.get_sset_count		= mv88e6171_get_sset_count,
+	.get_strings		= mv88e6xxx_get_strings,
+	.get_ethtool_stats	= mv88e6xxx_get_ethtool_stats,
+	.get_sset_count		= mv88e6xxx_get_sset_count,
+	.set_eee		= mv88e6171_set_eee,
+	.get_eee		= mv88e6171_get_eee,
 #ifdef CONFIG_NET_DSA_HWMON
 	.get_temp               = mv88e6xxx_get_temp,
 #endif
 	.get_regs_len		= mv88e6xxx_get_regs_len,
 	.get_regs		= mv88e6xxx_get_regs,
+	.port_join_bridge       = mv88e6xxx_join_bridge,
+	.port_leave_bridge      = mv88e6xxx_leave_bridge,
+	.port_stp_update        = mv88e6xxx_port_stp_update,
+	.fdb_add		= mv88e6xxx_port_fdb_add,
+	.fdb_del		= mv88e6xxx_port_fdb_del,
+	.fdb_getnext		= mv88e6xxx_port_fdb_getnext,
 };
 
 MODULE_ALIAS("platform:mv88e6171");
diff --git a/drivers/net/dsa/mv88e6352.c b/drivers/net/dsa/mv88e6352.c
index e13adc7b3dda..126c11b81e75 100644
--- a/drivers/net/dsa/mv88e6352.c
+++ b/drivers/net/dsa/mv88e6352.c
@@ -22,59 +22,6 @@
 #include <net/dsa.h>
 #include "mv88e6xxx.h"
 
-static int mv88e6352_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
-{
-	unsigned long timeout = jiffies + HZ / 10;
-
-	while (time_before(jiffies, timeout)) {
-		int ret;
-
-		ret = REG_READ(reg, offset);
-		if (!(ret & mask))
-			return 0;
-
-		usleep_range(1000, 2000);
-	}
-	return -ETIMEDOUT;
-}
-
-static inline int mv88e6352_phy_wait(struct dsa_switch *ds)
-{
-	return mv88e6352_wait(ds, REG_GLOBAL2, 0x18, 0x8000);
-}
-
-static inline int mv88e6352_eeprom_load_wait(struct dsa_switch *ds)
-{
-	return mv88e6352_wait(ds, REG_GLOBAL2, 0x14, 0x0800);
-}
-
-static inline int mv88e6352_eeprom_busy_wait(struct dsa_switch *ds)
-{
-	return mv88e6352_wait(ds, REG_GLOBAL2, 0x14, 0x8000);
-}
-
-static int __mv88e6352_phy_read(struct dsa_switch *ds, int addr, int regnum)
-{
-	int ret;
-
-	REG_WRITE(REG_GLOBAL2, 0x18, 0x9800 | (addr << 5) | regnum);
-
-	ret = mv88e6352_phy_wait(ds);
-	if (ret < 0)
-		return ret;
-
-	return REG_READ(REG_GLOBAL2, 0x19);
-}
-
-static int __mv88e6352_phy_write(struct dsa_switch *ds, int addr, int regnum,
-				 u16 val)
-{
-	REG_WRITE(REG_GLOBAL2, 0x19, val);
-	REG_WRITE(REG_GLOBAL2, 0x18, 0x9400 | (addr << 5) | regnum);
-
-	return mv88e6352_phy_wait(ds);
-}
-
 static char *mv88e6352_probe(struct device *host_dev, int sw_addr)
 {
 	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
@@ -83,58 +30,24 @@ static char *mv88e6352_probe(struct device *host_dev, int sw_addr)
 	if (bus == NULL)
 		return NULL;
 
-	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
+	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), PORT_SWITCH_ID);
 	if (ret >= 0) {
-		if ((ret & 0xfff0) == 0x1760)
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6176)
 			return "Marvell 88E6176";
-		if (ret == 0x3521)
+		if (ret == PORT_SWITCH_ID_6352_A0)
 			return "Marvell 88E6352 (A0)";
-		if (ret == 0x3522)
+		if (ret == PORT_SWITCH_ID_6352_A1)
 			return "Marvell 88E6352 (A1)";
-		if ((ret & 0xfff0) == 0x3520)
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6352)
 			return "Marvell 88E6352";
 	}
 
 	return NULL;
 }
 
-static int mv88e6352_switch_reset(struct dsa_switch *ds)
-{
-	unsigned long timeout;
-	int ret;
-	int i;
-
-	/* Set all ports to the disabled state. */
-	for (i = 0; i < 7; i++) {
-		ret = REG_READ(REG_PORT(i), 0x04);
-		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
-	}
-
-	/* Wait for transmit queues to drain. */
-	usleep_range(2000, 4000);
-
-	/* Reset the switch. Keep PPU active (bit 14, undocumented).
-	 * The PPU needs to be active to support indirect phy register
-	 * accesses through global registers 0x18 and 0x19.
-	 */
-	REG_WRITE(REG_GLOBAL, 0x04, 0xc000);
-
-	/* Wait up to one second for reset to complete. */
-	timeout = jiffies + 1 * HZ;
-	while (time_before(jiffies, timeout)) {
-		ret = REG_READ(REG_GLOBAL, 0x00);
-		if ((ret & 0x8800) == 0x8800)
-			break;
-		usleep_range(1000, 2000);
-	}
-	if (time_after(jiffies, timeout))
-		return -ETIMEDOUT;
-
-	return 0;
-}
-
 static int mv88e6352_setup_global(struct dsa_switch *ds)
 {
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int ret;
 	int i;
 
@@ -205,7 +118,7 @@ static int mv88e6352_setup_global(struct dsa_switch *ds)
 	/* Disable ingress rate limiting by resetting all ingress
 	 * rate limit registers to their initial state.
 	 */
-	for (i = 0; i < 7; i++)
+	for (i = 0; i < ps->num_ports; i++)
 		REG_WRITE(REG_GLOBAL2, 0x09, 0x9000 | (i << 8));
 
 	/* Initialise cross-chip port VLAN table to reset defaults. */
@@ -268,28 +181,6 @@ static int mv88e6352_setup_port(struct dsa_switch *ds, int p)
 		val |= 0x000c;
 	REG_WRITE(addr, 0x04, val);
 
-	/* Port Control 1: disable trunking.  Also, if this is the
-	 * CPU port, enable learn messages to be sent to this port.
-	 */
-	REG_WRITE(addr, 0x05, dsa_is_cpu_port(ds, p) ? 0x8000 : 0x0000);
-
-	/* Port based VLAN map: give each port its own address
-	 * database, allow the CPU port to talk to each of the 'real'
-	 * ports, and allow each of the 'real' ports to only talk to
-	 * the upstream port.
-	 */
-	val = (p & 0xf) << 12;
-	if (dsa_is_cpu_port(ds, p))
-		val |= ds->phys_port_mask;
-	else
-		val |= 1 << dsa_upstream_port(ds);
-	REG_WRITE(addr, 0x06, val);
-
-	/* Default VLAN ID and priority: don't set a default VLAN
-	 * ID, and set the default packet priority to zero.
-	 */
-	REG_WRITE(addr, 0x07, 0x0000);
-
 	/* Port Control 2: don't force a good FCS, set the maximum
 	 * frame size to 10240 bytes, don't let the switch add or
 	 * strip 802.1q tags, don't discard tagged or untagged frames
@@ -334,53 +225,18 @@ static int mv88e6352_setup_port(struct dsa_switch *ds, int p)
 	 */
 	REG_WRITE(addr, 0x19, 0x7654);
 
-	return 0;
+	return mv88e6xxx_setup_port_common(ds, p);
 }
 
 #ifdef CONFIG_NET_DSA_HWMON
 
-static int mv88e6352_phy_page_read(struct dsa_switch *ds,
-				   int port, int page, int reg)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int ret;
-
-	mutex_lock(&ps->phy_mutex);
-	ret = __mv88e6352_phy_write(ds, port, 0x16, page);
-	if (ret < 0)
-		goto error;
-	ret = __mv88e6352_phy_read(ds, port, reg);
-error:
-	__mv88e6352_phy_write(ds, port, 0x16, 0x0);
-	mutex_unlock(&ps->phy_mutex);
-	return ret;
-}
-
-static int mv88e6352_phy_page_write(struct dsa_switch *ds,
-				    int port, int page, int reg, int val)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int ret;
-
-	mutex_lock(&ps->phy_mutex);
-	ret = __mv88e6352_phy_write(ds, port, 0x16, page);
-	if (ret < 0)
-		goto error;
-
-	ret = __mv88e6352_phy_write(ds, port, reg, val);
-error:
-	__mv88e6352_phy_write(ds, port, 0x16, 0x0);
-	mutex_unlock(&ps->phy_mutex);
-	return ret;
-}
-
 static int mv88e6352_get_temp(struct dsa_switch *ds, int *temp)
 {
 	int ret;
 
 	*temp = 0;
 
-	ret = mv88e6352_phy_page_read(ds, 0, 6, 27);
+	ret = mv88e6xxx_phy_page_read(ds, 0, 6, 27);
 	if (ret < 0)
 		return ret;
 
@@ -395,7 +251,7 @@ static int mv88e6352_get_temp_limit(struct dsa_switch *ds, int *temp)
 
 	*temp = 0;
 
-	ret = mv88e6352_phy_page_read(ds, 0, 6, 26);
+	ret = mv88e6xxx_phy_page_read(ds, 0, 6, 26);
 	if (ret < 0)
 		return ret;
 
@@ -408,11 +264,11 @@ static int mv88e6352_set_temp_limit(struct dsa_switch *ds, int temp)
 {
 	int ret;
 
-	ret = mv88e6352_phy_page_read(ds, 0, 6, 26);
+	ret = mv88e6xxx_phy_page_read(ds, 0, 6, 26);
 	if (ret < 0)
 		return ret;
 	temp = clamp_val(DIV_ROUND_CLOSEST(temp, 5) + 5, 0, 0x1f);
-	return mv88e6352_phy_page_write(ds, 0, 6, 26,
+	return mv88e6xxx_phy_page_write(ds, 0, 6, 26,
 					(ret & 0xe0ff) | (temp << 8));
 }
 
@@ -422,7 +278,7 @@ static int mv88e6352_get_temp_alarm(struct dsa_switch *ds, bool *alarm)
 
 	*alarm = false;
 
-	ret = mv88e6352_phy_page_read(ds, 0, 6, 26);
+	ret = mv88e6xxx_phy_page_read(ds, 0, 6, 26);
 	if (ret < 0)
 		return ret;
 
@@ -438,14 +294,15 @@ static int mv88e6352_setup(struct dsa_switch *ds)
 	int ret;
 	int i;
 
-	mutex_init(&ps->smi_mutex);
-	mutex_init(&ps->stats_mutex);
-	mutex_init(&ps->phy_mutex);
-	mutex_init(&ps->eeprom_mutex);
+	ret = mv88e6xxx_setup_common(ds);
+	if (ret < 0)
+		return ret;
 
-	ps->id = REG_READ(REG_PORT(0), 0x03) & 0xfff0;
+	ps->num_ports = 7;
 
-	ret = mv88e6352_switch_reset(ds);
+	mutex_init(&ps->eeprom_mutex);
+
+	ret = mv88e6xxx_switch_reset(ds, true);
 	if (ret < 0)
 		return ret;
 
@@ -455,7 +312,7 @@ static int mv88e6352_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < 7; i++) {
+	for (i = 0; i < ps->num_ports; i++) {
 		ret = mv88e6352_setup_port(ds, i);
 		if (ret < 0)
 			return ret;
@@ -464,83 +321,6 @@ static int mv88e6352_setup(struct dsa_switch *ds)
 	return 0;
 }
 
-static int mv88e6352_port_to_phy_addr(int port)
-{
-	if (port >= 0 && port <= 4)
-		return port;
-	return -EINVAL;
-}
-
-static int
-mv88e6352_phy_read(struct dsa_switch *ds, int port, int regnum)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int addr = mv88e6352_port_to_phy_addr(port);
-	int ret;
-
-	if (addr < 0)
-		return addr;
-
-	mutex_lock(&ps->phy_mutex);
-	ret = __mv88e6352_phy_read(ds, addr, regnum);
-	mutex_unlock(&ps->phy_mutex);
-
-	return ret;
-}
-
-static int
-mv88e6352_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int addr = mv88e6352_port_to_phy_addr(port);
-	int ret;
-
-	if (addr < 0)
-		return addr;
-
-	mutex_lock(&ps->phy_mutex);
-	ret = __mv88e6352_phy_write(ds, addr, regnum, val);
-	mutex_unlock(&ps->phy_mutex);
-
-	return ret;
-}
-
-static struct mv88e6xxx_hw_stat mv88e6352_hw_stats[] = {
-	{ "in_good_octets", 8, 0x00, },
-	{ "in_bad_octets", 4, 0x02, },
-	{ "in_unicast", 4, 0x04, },
-	{ "in_broadcasts", 4, 0x06, },
-	{ "in_multicasts", 4, 0x07, },
-	{ "in_pause", 4, 0x16, },
-	{ "in_undersize", 4, 0x18, },
-	{ "in_fragments", 4, 0x19, },
-	{ "in_oversize", 4, 0x1a, },
-	{ "in_jabber", 4, 0x1b, },
-	{ "in_rx_error", 4, 0x1c, },
-	{ "in_fcs_error", 4, 0x1d, },
-	{ "out_octets", 8, 0x0e, },
-	{ "out_unicast", 4, 0x10, },
-	{ "out_broadcasts", 4, 0x13, },
-	{ "out_multicasts", 4, 0x12, },
-	{ "out_pause", 4, 0x15, },
-	{ "excessive", 4, 0x11, },
-	{ "collisions", 4, 0x1e, },
-	{ "deferred", 4, 0x05, },
-	{ "single", 4, 0x14, },
-	{ "multiple", 4, 0x17, },
-	{ "out_fcs_error", 4, 0x03, },
-	{ "late", 4, 0x1f, },
-	{ "hist_64bytes", 4, 0x08, },
-	{ "hist_65_127bytes", 4, 0x09, },
-	{ "hist_128_255bytes", 4, 0x0a, },
-	{ "hist_256_511bytes", 4, 0x0b, },
-	{ "hist_512_1023bytes", 4, 0x0c, },
-	{ "hist_1024_max_bytes", 4, 0x0d, },
-	{ "sw_in_discards", 4, 0x110, },
-	{ "sw_in_filtered", 2, 0x112, },
-	{ "sw_out_filtered", 2, 0x113, },
-};
-
 static int mv88e6352_read_eeprom_word(struct dsa_switch *ds, int addr)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
@@ -553,7 +333,7 @@ static int mv88e6352_read_eeprom_word(struct dsa_switch *ds, int addr)
 	if (ret < 0)
 		goto error;
 
-	ret = mv88e6352_eeprom_busy_wait(ds);
+	ret = mv88e6xxx_eeprom_busy_wait(ds);
 	if (ret < 0)
 		goto error;
 
@@ -576,7 +356,7 @@ static int mv88e6352_get_eeprom(struct dsa_switch *ds,
 
 	eeprom->magic = 0xc3ec4951;
 
-	ret = mv88e6352_eeprom_load_wait(ds);
+	ret = mv88e6xxx_eeprom_load_wait(ds);
 	if (ret < 0)
 		return ret;
 
@@ -657,7 +437,7 @@ static int mv88e6352_write_eeprom_word(struct dsa_switch *ds, int addr,
 	if (ret < 0)
 		goto error;
 
-	ret = mv88e6352_eeprom_busy_wait(ds);
+	ret = mv88e6xxx_eeprom_busy_wait(ds);
 error:
 	mutex_unlock(&ps->eeprom_mutex);
 	return ret;
@@ -681,7 +461,7 @@ static int mv88e6352_set_eeprom(struct dsa_switch *ds,
 	len = eeprom->len;
 	eeprom->len = 0;
 
-	ret = mv88e6352_eeprom_load_wait(ds);
+	ret = mv88e6xxx_eeprom_load_wait(ds);
 	if (ret < 0)
 		return ret;
 
@@ -739,37 +519,20 @@ static int mv88e6352_set_eeprom(struct dsa_switch *ds,
 	return 0;
 }
 
-static void
-mv88e6352_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
-{
-	mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6352_hw_stats),
-			      mv88e6352_hw_stats, port, data);
-}
-
-static void
-mv88e6352_get_ethtool_stats(struct dsa_switch *ds, int port, uint64_t *data)
-{
-	mv88e6xxx_get_ethtool_stats(ds, ARRAY_SIZE(mv88e6352_hw_stats),
-				    mv88e6352_hw_stats, port, data);
-}
-
-static int mv88e6352_get_sset_count(struct dsa_switch *ds)
-{
-	return ARRAY_SIZE(mv88e6352_hw_stats);
-}
-
 struct dsa_switch_driver mv88e6352_switch_driver = {
 	.tag_protocol		= DSA_TAG_PROTO_EDSA,
 	.priv_size		= sizeof(struct mv88e6xxx_priv_state),
 	.probe			= mv88e6352_probe,
 	.setup			= mv88e6352_setup,
 	.set_addr		= mv88e6xxx_set_addr_indirect,
-	.phy_read		= mv88e6352_phy_read,
-	.phy_write		= mv88e6352_phy_write,
+	.phy_read		= mv88e6xxx_phy_read_indirect,
+	.phy_write		= mv88e6xxx_phy_write_indirect,
 	.poll_link		= mv88e6xxx_poll_link,
-	.get_strings		= mv88e6352_get_strings,
-	.get_ethtool_stats	= mv88e6352_get_ethtool_stats,
-	.get_sset_count		= mv88e6352_get_sset_count,
+	.get_strings		= mv88e6xxx_get_strings,
+	.get_ethtool_stats	= mv88e6xxx_get_ethtool_stats,
+	.get_sset_count		= mv88e6xxx_get_sset_count,
+	.set_eee		= mv88e6xxx_set_eee,
+	.get_eee		= mv88e6xxx_get_eee,
 #ifdef CONFIG_NET_DSA_HWMON
 	.get_temp		= mv88e6352_get_temp,
 	.get_temp_limit		= mv88e6352_get_temp_limit,
@@ -780,6 +543,12 @@ struct dsa_switch_driver mv88e6352_switch_driver = {
 	.set_eeprom		= mv88e6352_set_eeprom,
 	.get_regs_len		= mv88e6xxx_get_regs_len,
 	.get_regs		= mv88e6xxx_get_regs,
+	.port_join_bridge	= mv88e6xxx_join_bridge,
+	.port_leave_bridge	= mv88e6xxx_leave_bridge,
+	.port_stp_update	= mv88e6xxx_port_stp_update,
+	.fdb_add		= mv88e6xxx_port_fdb_add,
+	.fdb_del		= mv88e6xxx_port_fdb_del,
+	.fdb_getnext		= mv88e6xxx_port_fdb_getnext,
 };
 
 MODULE_ALIAS("platform:mv88e6352");
diff --git a/drivers/net/dsa/mv88e6xxx.c b/drivers/net/dsa/mv88e6xxx.c
index 3e7e31a6abb7..fc8d3b6ffe8e 100644
--- a/drivers/net/dsa/mv88e6xxx.c
+++ b/drivers/net/dsa/mv88e6xxx.c
@@ -9,6 +9,8 @@
  */
 
 #include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/if_bridge.h>
 #include <linux/jiffies.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -31,11 +33,11 @@ static int mv88e6xxx_reg_wait_ready(struct mii_bus *bus, int sw_addr)
 	int i;
 
 	for (i = 0; i < 16; i++) {
-		ret = mdiobus_read(bus, sw_addr, 0);
+		ret = mdiobus_read(bus, sw_addr, SMI_CMD);
 		if (ret < 0)
 			return ret;
 
-		if ((ret & 0x8000) == 0)
+		if ((ret & SMI_CMD_BUSY) == 0)
 			return 0;
 	}
 
@@ -55,7 +57,8 @@ int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
 		return ret;
 
 	/* Transmit the read command. */
-	ret = mdiobus_write(bus, sw_addr, 0, 0x9800 | (addr << 5) | reg);
+	ret = mdiobus_write(bus, sw_addr, SMI_CMD,
+			    SMI_CMD_OP_22_READ | (addr << 5) | reg);
 	if (ret < 0)
 		return ret;
 
@@ -65,26 +68,23 @@ int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
 		return ret;
 
 	/* Read the data. */
-	ret = mdiobus_read(bus, sw_addr, 1);
+	ret = mdiobus_read(bus, sw_addr, SMI_DATA);
 	if (ret < 0)
 		return ret;
 
 	return ret & 0xffff;
 }
 
-int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
+/* Must be called with SMI mutex held */
+static int _mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
 {
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
 	int ret;
 
 	if (bus == NULL)
 		return -EINVAL;
 
-	mutex_lock(&ps->smi_mutex);
 	ret = __mv88e6xxx_reg_read(bus, ds->pd->sw_addr, addr, reg);
-	mutex_unlock(&ps->smi_mutex);
-
 	if (ret < 0)
 		return ret;
 
@@ -94,6 +94,18 @@ int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
 	return ret;
 }
 
+int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
+	mutex_lock(&ps->smi_mutex);
+	ret = _mv88e6xxx_reg_read(ds, addr, reg);
+	mutex_unlock(&ps->smi_mutex);
+
+	return ret;
+}
+
 int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 			  int reg, u16 val)
 {
@@ -108,12 +120,13 @@ int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 		return ret;
 
 	/* Transmit the data to write. */
-	ret = mdiobus_write(bus, sw_addr, 1, val);
+	ret = mdiobus_write(bus, sw_addr, SMI_DATA, val);
 	if (ret < 0)
 		return ret;
 
 	/* Transmit the write command. */
-	ret = mdiobus_write(bus, sw_addr, 0, 0x9400 | (addr << 5) | reg);
+	ret = mdiobus_write(bus, sw_addr, SMI_CMD,
+			    SMI_CMD_OP_22_WRITE | (addr << 5) | reg);
 	if (ret < 0)
 		return ret;
 
@@ -125,11 +138,11 @@ int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 	return 0;
 }
 
-int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
+/* Must be called with SMI mutex held */
+static int _mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg,
+				u16 val)
 {
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
-	int ret;
 
 	if (bus == NULL)
 		return -EINVAL;
@@ -137,8 +150,16 @@ int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 	dev_dbg(ds->master_dev, "-> addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
 		addr, reg, val);
 
+	return __mv88e6xxx_reg_write(bus, ds->pd->sw_addr, addr, reg, val);
+}
+
+int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
 	mutex_lock(&ps->smi_mutex);
-	ret = __mv88e6xxx_reg_write(bus, ds->pd->sw_addr, addr, reg, val);
+	ret = _mv88e6xxx_reg_write(ds, addr, reg, val);
 	mutex_unlock(&ps->smi_mutex);
 
 	return ret;
@@ -147,26 +168,26 @@ int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 int mv88e6xxx_config_prio(struct dsa_switch *ds)
 {
 	/* Configure the IP ToS mapping registers. */
-	REG_WRITE(REG_GLOBAL, 0x10, 0x0000);
-	REG_WRITE(REG_GLOBAL, 0x11, 0x0000);
-	REG_WRITE(REG_GLOBAL, 0x12, 0x5555);
-	REG_WRITE(REG_GLOBAL, 0x13, 0x5555);
-	REG_WRITE(REG_GLOBAL, 0x14, 0xaaaa);
-	REG_WRITE(REG_GLOBAL, 0x15, 0xaaaa);
-	REG_WRITE(REG_GLOBAL, 0x16, 0xffff);
-	REG_WRITE(REG_GLOBAL, 0x17, 0xffff);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_0, 0x0000);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_1, 0x0000);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_2, 0x5555);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_3, 0x5555);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_4, 0xaaaa);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_5, 0xaaaa);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_6, 0xffff);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_7, 0xffff);
 
 	/* Configure the IEEE 802.1p priority mapping register. */
-	REG_WRITE(REG_GLOBAL, 0x18, 0xfa41);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IEEE_PRI, 0xfa41);
 
 	return 0;
 }
 
 int mv88e6xxx_set_addr_direct(struct dsa_switch *ds, u8 *addr)
 {
-	REG_WRITE(REG_GLOBAL, 0x01, (addr[0] << 8) | addr[1]);
-	REG_WRITE(REG_GLOBAL, 0x02, (addr[2] << 8) | addr[3]);
-	REG_WRITE(REG_GLOBAL, 0x03, (addr[4] << 8) | addr[5]);
+	REG_WRITE(REG_GLOBAL, GLOBAL_MAC_01, (addr[0] << 8) | addr[1]);
+	REG_WRITE(REG_GLOBAL, GLOBAL_MAC_23, (addr[2] << 8) | addr[3]);
+	REG_WRITE(REG_GLOBAL, GLOBAL_MAC_45, (addr[4] << 8) | addr[5]);
 
 	return 0;
 }
@@ -180,12 +201,13 @@ int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr)
 		int j;
 
 		/* Write the MAC address byte. */
-		REG_WRITE(REG_GLOBAL2, 0x0d, 0x8000 | (i << 8) | addr[i]);
+		REG_WRITE(REG_GLOBAL2, GLOBAL2_SWITCH_MAC,
+			  GLOBAL2_SWITCH_MAC_BUSY | (i << 8) | addr[i]);
 
 		/* Wait for the write to complete. */
 		for (j = 0; j < 16; j++) {
-			ret = REG_READ(REG_GLOBAL2, 0x0d);
-			if ((ret & 0x8000) == 0)
+			ret = REG_READ(REG_GLOBAL2, GLOBAL2_SWITCH_MAC);
+			if ((ret & GLOBAL2_SWITCH_MAC_BUSY) == 0)
 				break;
 		}
 		if (j == 16)
@@ -195,14 +217,17 @@ int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr)
 	return 0;
 }
 
-int mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum)
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum)
 {
 	if (addr >= 0)
 		return mv88e6xxx_reg_read(ds, addr, regnum);
 	return 0xffff;
 }
 
-int mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum, u16 val)
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum,
+				u16 val)
 {
 	if (addr >= 0)
 		return mv88e6xxx_reg_write(ds, addr, regnum, val);
@@ -215,14 +240,16 @@ static int mv88e6xxx_ppu_disable(struct dsa_switch *ds)
 	int ret;
 	unsigned long timeout;
 
-	ret = REG_READ(REG_GLOBAL, 0x04);
-	REG_WRITE(REG_GLOBAL, 0x04, ret & ~0x4000);
+	ret = REG_READ(REG_GLOBAL, GLOBAL_CONTROL);
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL,
+		  ret & ~GLOBAL_CONTROL_PPU_ENABLE);
 
 	timeout = jiffies + 1 * HZ;
 	while (time_before(jiffies, timeout)) {
-		ret = REG_READ(REG_GLOBAL, 0x00);
+		ret = REG_READ(REG_GLOBAL, GLOBAL_STATUS);
 		usleep_range(1000, 2000);
-		if ((ret & 0xc000) != 0xc000)
+		if ((ret & GLOBAL_STATUS_PPU_MASK) !=
+		    GLOBAL_STATUS_PPU_POLLING)
 			return 0;
 	}
 
@@ -234,14 +261,15 @@ static int mv88e6xxx_ppu_enable(struct dsa_switch *ds)
 	int ret;
 	unsigned long timeout;
 
-	ret = REG_READ(REG_GLOBAL, 0x04);
-	REG_WRITE(REG_GLOBAL, 0x04, ret | 0x4000);
+	ret = REG_READ(REG_GLOBAL, GLOBAL_CONTROL);
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL, ret | GLOBAL_CONTROL_PPU_ENABLE);
 
 	timeout = jiffies + 1 * HZ;
 	while (time_before(jiffies, timeout)) {
-		ret = REG_READ(REG_GLOBAL, 0x00);
+		ret = REG_READ(REG_GLOBAL, GLOBAL_STATUS);
 		usleep_range(1000, 2000);
-		if ((ret & 0xc000) == 0xc000)
+		if ((ret & GLOBAL_STATUS_PPU_MASK) ==
+		    GLOBAL_STATUS_PPU_POLLING)
 			return 0;
 	}
 
@@ -362,11 +390,12 @@ void mv88e6xxx_poll_link(struct dsa_switch *ds)
 
 		link = 0;
 		if (dev->flags & IFF_UP) {
-			port_status = mv88e6xxx_reg_read(ds, REG_PORT(i), 0x00);
+			port_status = mv88e6xxx_reg_read(ds, REG_PORT(i),
+							 PORT_STATUS);
 			if (port_status < 0)
 				continue;
 
-			link = !!(port_status & 0x0800);
+			link = !!(port_status & PORT_STATUS_LINK);
 		}
 
 		if (!link) {
@@ -377,22 +406,22 @@ void mv88e6xxx_poll_link(struct dsa_switch *ds)
 			continue;
 		}
 
-		switch (port_status & 0x0300) {
-		case 0x0000:
+		switch (port_status & PORT_STATUS_SPEED_MASK) {
+		case PORT_STATUS_SPEED_10:
 			speed = 10;
 			break;
-		case 0x0100:
+		case PORT_STATUS_SPEED_100:
 			speed = 100;
 			break;
-		case 0x0200:
+		case PORT_STATUS_SPEED_1000:
 			speed = 1000;
 			break;
 		default:
 			speed = -1;
 			break;
 		}
-		duplex = (port_status & 0x0400) ? 1 : 0;
-		fc = (port_status & 0x8000) ? 1 : 0;
+		duplex = (port_status & PORT_STATUS_DUPLEX) ? 1 : 0;
+		fc = (port_status & PORT_STATUS_PAUSE_EN) ? 1 : 0;
 
 		if (!netif_carrier_ok(dev)) {
 			netdev_info(dev,
@@ -405,14 +434,27 @@ void mv88e6xxx_poll_link(struct dsa_switch *ds)
 	}
 }
 
+static bool mv88e6xxx_6352_family(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6352:
+	case PORT_SWITCH_ID_6172:
+	case PORT_SWITCH_ID_6176:
+		return true;
+	}
+	return false;
+}
+
 static int mv88e6xxx_stats_wait(struct dsa_switch *ds)
 {
 	int ret;
 	int i;
 
 	for (i = 0; i < 10; i++) {
-		ret = REG_READ(REG_GLOBAL, 0x1d);
-		if ((ret & 0x8000) == 0)
+		ret = REG_READ(REG_GLOBAL, GLOBAL_STATS_OP);
+		if ((ret & GLOBAL_STATS_OP_BUSY) == 0)
 			return 0;
 	}
 
@@ -423,8 +465,13 @@ static int mv88e6xxx_stats_snapshot(struct dsa_switch *ds, int port)
 {
 	int ret;
 
+	if (mv88e6xxx_6352_family(ds))
+		port = (port + 1) << 5;
+
 	/* Snapshot the hardware statistics counters for this port. */
-	REG_WRITE(REG_GLOBAL, 0x1d, 0xdc00 | port);
+	REG_WRITE(REG_GLOBAL, GLOBAL_STATS_OP,
+		  GLOBAL_STATS_OP_CAPTURE_PORT |
+		  GLOBAL_STATS_OP_HIST_RX_TX | port);
 
 	/* Wait for the snapshotting to complete. */
 	ret = mv88e6xxx_stats_wait(ds);
@@ -441,7 +488,9 @@ static void mv88e6xxx_stats_read(struct dsa_switch *ds, int stat, u32 *val)
 
 	*val = 0;
 
-	ret = mv88e6xxx_reg_write(ds, REG_GLOBAL, 0x1d, 0xcc00 | stat);
+	ret = mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_STATS_OP,
+				  GLOBAL_STATS_OP_READ_CAPTURED |
+				  GLOBAL_STATS_OP_HIST_RX_TX | stat);
 	if (ret < 0)
 		return;
 
@@ -449,22 +498,77 @@ static void mv88e6xxx_stats_read(struct dsa_switch *ds, int stat, u32 *val)
 	if (ret < 0)
 		return;
 
-	ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, 0x1e);
+	ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_32);
 	if (ret < 0)
 		return;
 
 	_val = ret << 16;
 
-	ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, 0x1f);
+	ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_01);
 	if (ret < 0)
 		return;
 
 	*val = _val | ret;
 }
 
-void mv88e6xxx_get_strings(struct dsa_switch *ds,
-			   int nr_stats, struct mv88e6xxx_hw_stat *stats,
-			   int port, uint8_t *data)
+static struct mv88e6xxx_hw_stat mv88e6xxx_hw_stats[] = {
+	{ "in_good_octets", 8, 0x00, },
+	{ "in_bad_octets", 4, 0x02, },
+	{ "in_unicast", 4, 0x04, },
+	{ "in_broadcasts", 4, 0x06, },
+	{ "in_multicasts", 4, 0x07, },
+	{ "in_pause", 4, 0x16, },
+	{ "in_undersize", 4, 0x18, },
+	{ "in_fragments", 4, 0x19, },
+	{ "in_oversize", 4, 0x1a, },
+	{ "in_jabber", 4, 0x1b, },
+	{ "in_rx_error", 4, 0x1c, },
+	{ "in_fcs_error", 4, 0x1d, },
+	{ "out_octets", 8, 0x0e, },
+	{ "out_unicast", 4, 0x10, },
+	{ "out_broadcasts", 4, 0x13, },
+	{ "out_multicasts", 4, 0x12, },
+	{ "out_pause", 4, 0x15, },
+	{ "excessive", 4, 0x11, },
+	{ "collisions", 4, 0x1e, },
+	{ "deferred", 4, 0x05, },
+	{ "single", 4, 0x14, },
+	{ "multiple", 4, 0x17, },
+	{ "out_fcs_error", 4, 0x03, },
+	{ "late", 4, 0x1f, },
+	{ "hist_64bytes", 4, 0x08, },
+	{ "hist_65_127bytes", 4, 0x09, },
+	{ "hist_128_255bytes", 4, 0x0a, },
+	{ "hist_256_511bytes", 4, 0x0b, },
+	{ "hist_512_1023bytes", 4, 0x0c, },
+	{ "hist_1024_max_bytes", 4, 0x0d, },
+	/* Not all devices have the following counters */
+	{ "sw_in_discards", 4, 0x110, },
+	{ "sw_in_filtered", 2, 0x112, },
+	{ "sw_out_filtered", 2, 0x113, },
+
+};
+
+static bool have_sw_in_discards(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6095: case PORT_SWITCH_ID_6161:
+	case PORT_SWITCH_ID_6165: case PORT_SWITCH_ID_6171:
+	case PORT_SWITCH_ID_6172: case PORT_SWITCH_ID_6176:
+	case PORT_SWITCH_ID_6182: case PORT_SWITCH_ID_6185:
+	case PORT_SWITCH_ID_6352:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void _mv88e6xxx_get_strings(struct dsa_switch *ds,
+				   int nr_stats,
+				   struct mv88e6xxx_hw_stat *stats,
+				   int port, uint8_t *data)
 {
 	int i;
 
@@ -474,9 +578,10 @@ void mv88e6xxx_get_strings(struct dsa_switch *ds,
 	}
 }
 
-void mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
-				 int nr_stats, struct mv88e6xxx_hw_stat *stats,
-				 int port, uint64_t *data)
+static void _mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
+					 int nr_stats,
+					 struct mv88e6xxx_hw_stat *stats,
+					 int port, uint64_t *data)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int ret;
@@ -524,6 +629,39 @@ error:
 	mutex_unlock(&ps->stats_mutex);
 }
 
+/* All the statistics in the table */
+void
+mv88e6xxx_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
+{
+	if (have_sw_in_discards(ds))
+		_mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
+				       mv88e6xxx_hw_stats, port, data);
+	else
+		_mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
+				       mv88e6xxx_hw_stats, port, data);
+}
+
+int mv88e6xxx_get_sset_count(struct dsa_switch *ds)
+{
+	if (have_sw_in_discards(ds))
+		return ARRAY_SIZE(mv88e6xxx_hw_stats);
+	return ARRAY_SIZE(mv88e6xxx_hw_stats) - 3;
+}
+
+void
+mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
+			    int port, uint64_t *data)
+{
+	if (have_sw_in_discards(ds))
+		_mv88e6xxx_get_ethtool_stats(
+			ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
+			mv88e6xxx_hw_stats, port, data);
+	else
+		_mv88e6xxx_get_ethtool_stats(
+			ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
+			mv88e6xxx_hw_stats, port, data);
+}
+
 int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port)
 {
 	return 32 * sizeof(u16);
@@ -560,42 +698,754 @@ int  mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
 
 	mutex_lock(&ps->phy_mutex);
 
-	ret = mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x6);
+	ret = _mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x6);
 	if (ret < 0)
 		goto error;
 
 	/* Enable temperature sensor */
-	ret = mv88e6xxx_phy_read(ds, 0x0, 0x1a);
+	ret = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
 	if (ret < 0)
 		goto error;
 
-	ret = mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret | (1 << 5));
+	ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret | (1 << 5));
 	if (ret < 0)
 		goto error;
 
 	/* Wait for temperature to stabilize */
 	usleep_range(10000, 12000);
 
-	val = mv88e6xxx_phy_read(ds, 0x0, 0x1a);
+	val = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
 	if (val < 0) {
 		ret = val;
 		goto error;
 	}
 
 	/* Disable temperature sensor */
-	ret = mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret & ~(1 << 5));
+	ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret & ~(1 << 5));
 	if (ret < 0)
 		goto error;
 
 	*temp = ((val & 0x1f) - 5) * 5;
 
 error:
-	mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x0);
+	_mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x0);
 	mutex_unlock(&ps->phy_mutex);
 	return ret;
 }
 #endif /* CONFIG_NET_DSA_HWMON */
 
+static int mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
+{
+	unsigned long timeout = jiffies + HZ / 10;
+
+	while (time_before(jiffies, timeout)) {
+		int ret;
+
+		ret = REG_READ(reg, offset);
+		if (!(ret & mask))
+			return 0;
+
+		usleep_range(1000, 2000);
+	}
+	return -ETIMEDOUT;
+}
+
+int mv88e6xxx_phy_wait(struct dsa_switch *ds)
+{
+	return mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
+			      GLOBAL2_SMI_OP_BUSY);
+}
+
+int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds)
+{
+	return mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
+			      GLOBAL2_EEPROM_OP_LOAD);
+}
+
+int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds)
+{
+	return mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
+			      GLOBAL2_EEPROM_OP_BUSY);
+}
+
+/* Must be called with SMI lock held */
+static int _mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
+{
+	unsigned long timeout = jiffies + HZ / 10;
+
+	while (time_before(jiffies, timeout)) {
+		int ret;
+
+		ret = _mv88e6xxx_reg_read(ds, reg, offset);
+		if (ret < 0)
+			return ret;
+		if (!(ret & mask))
+			return 0;
+
+		usleep_range(1000, 2000);
+	}
+	return -ETIMEDOUT;
+}
+
+/* Must be called with SMI lock held */
+static int _mv88e6xxx_atu_wait(struct dsa_switch *ds)
+{
+	return _mv88e6xxx_wait(ds, REG_GLOBAL, GLOBAL_ATU_OP,
+			       GLOBAL_ATU_OP_BUSY);
+}
+
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr,
+					int regnum)
+{
+	int ret;
+
+	REG_WRITE(REG_GLOBAL2, GLOBAL2_SMI_OP,
+		  GLOBAL2_SMI_OP_22_READ | (addr << 5) | regnum);
+
+	ret = mv88e6xxx_phy_wait(ds);
+	if (ret < 0)
+		return ret;
+
+	return REG_READ(REG_GLOBAL2, GLOBAL2_SMI_DATA);
+}
+
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int addr,
+					 int regnum, u16 val)
+{
+	REG_WRITE(REG_GLOBAL2, GLOBAL2_SMI_DATA, val);
+	REG_WRITE(REG_GLOBAL2, GLOBAL2_SMI_OP,
+		  GLOBAL2_SMI_OP_22_WRITE | (addr << 5) | regnum);
+
+	return mv88e6xxx_phy_wait(ds);
+}
+
+int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int reg;
+
+	mutex_lock(&ps->phy_mutex);
+
+	reg = _mv88e6xxx_phy_read_indirect(ds, port, 16);
+	if (reg < 0)
+		goto out;
+
+	e->eee_enabled = !!(reg & 0x0200);
+	e->tx_lpi_enabled = !!(reg & 0x0100);
+
+	reg = mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_STATUS);
+	if (reg < 0)
+		goto out;
+
+	e->eee_active = !!(reg & PORT_STATUS_EEE);
+	reg = 0;
+
+out:
+	mutex_unlock(&ps->phy_mutex);
+	return reg;
+}
+
+int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
+		      struct phy_device *phydev, struct ethtool_eee *e)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int reg;
+	int ret;
+
+	mutex_lock(&ps->phy_mutex);
+
+	ret = _mv88e6xxx_phy_read_indirect(ds, port, 16);
+	if (ret < 0)
+		goto out;
+
+	reg = ret & ~0x0300;
+	if (e->eee_enabled)
+		reg |= 0x0200;
+	if (e->tx_lpi_enabled)
+		reg |= 0x0100;
+
+	ret = _mv88e6xxx_phy_write_indirect(ds, port, 16, reg);
+out:
+	mutex_unlock(&ps->phy_mutex);
+
+	return ret;
+}
+
+static int _mv88e6xxx_atu_cmd(struct dsa_switch *ds, int fid, u16 cmd)
+{
+	int ret;
+
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, 0x01, fid);
+	if (ret < 0)
+		return ret;
+
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_ATU_OP, cmd);
+	if (ret < 0)
+		return ret;
+
+	return _mv88e6xxx_atu_wait(ds);
+}
+
+static int _mv88e6xxx_flush_fid(struct dsa_switch *ds, int fid)
+{
+	int ret;
+
+	ret = _mv88e6xxx_atu_wait(ds);
+	if (ret < 0)
+		return ret;
+
+	return _mv88e6xxx_atu_cmd(ds, fid, GLOBAL_ATU_OP_FLUSH_NON_STATIC_DB);
+}
+
+static int mv88e6xxx_set_port_state(struct dsa_switch *ds, int port, u8 state)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int reg, ret;
+	u8 oldstate;
+
+	mutex_lock(&ps->smi_mutex);
+
+	reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_CONTROL);
+	if (reg < 0)
+		goto abort;
+
+	oldstate = reg & PORT_CONTROL_STATE_MASK;
+	if (oldstate != state) {
+		/* Flush forwarding database if we're moving a port
+		 * from Learning or Forwarding state to Disabled or
+		 * Blocking or Listening state.
+		 */
+		if (oldstate >= PORT_CONTROL_STATE_LEARNING &&
+		    state <= PORT_CONTROL_STATE_BLOCKING) {
+			ret = _mv88e6xxx_flush_fid(ds, ps->fid[port]);
+			if (ret)
+				goto abort;
+		}
+		reg = (reg & ~PORT_CONTROL_STATE_MASK) | state;
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_CONTROL,
+					   reg);
+	}
+
+abort:
+	mutex_unlock(&ps->smi_mutex);
+	return ret;
+}
+
+/* Must be called with smi lock held */
+static int _mv88e6xxx_update_port_config(struct dsa_switch *ds, int port)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	u8 fid = ps->fid[port];
+	u16 reg = fid << 12;
+
+	if (dsa_is_cpu_port(ds, port))
+		reg |= ds->phys_port_mask;
+	else
+		reg |= (ps->bridge_mask[fid] |
+		       (1 << dsa_upstream_port(ds))) & ~(1 << port);
+
+	return _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_BASE_VLAN, reg);
+}
+
+/* Must be called with smi lock held */
+static int _mv88e6xxx_update_bridge_config(struct dsa_switch *ds, int fid)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int port;
+	u32 mask;
+	int ret;
+
+	mask = ds->phys_port_mask;
+	while (mask) {
+		port = __ffs(mask);
+		mask &= ~(1 << port);
+		if (ps->fid[port] != fid)
+			continue;
+
+		ret = _mv88e6xxx_update_port_config(ds, port);
+		if (ret)
+			return ret;
+	}
+
+	return _mv88e6xxx_flush_fid(ds, fid);
+}
+
+/* Bridge handling functions */
+
+int mv88e6xxx_join_bridge(struct dsa_switch *ds, int port, u32 br_port_mask)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret = 0;
+	u32 nmask;
+	int fid;
+
+	/* If the bridge group is not empty, join that group.
+	 * Otherwise create a new group.
+	 */
+	fid = ps->fid[port];
+	nmask = br_port_mask & ~(1 << port);
+	if (nmask)
+		fid = ps->fid[__ffs(nmask)];
+
+	nmask = ps->bridge_mask[fid] | (1 << port);
+	if (nmask != br_port_mask) {
+		netdev_err(ds->ports[port],
+			   "join: Bridge port mask mismatch fid=%d mask=0x%x expected 0x%x\n",
+			   fid, br_port_mask, nmask);
+		return -EINVAL;
+	}
+
+	mutex_lock(&ps->smi_mutex);
+
+	ps->bridge_mask[fid] = br_port_mask;
+
+	if (fid != ps->fid[port]) {
+		ps->fid_mask |= 1 << ps->fid[port];
+		ps->fid[port] = fid;
+		ret = _mv88e6xxx_update_bridge_config(ds, fid);
+	}
+
+	mutex_unlock(&ps->smi_mutex);
+
+	return ret;
+}
+
+int mv88e6xxx_leave_bridge(struct dsa_switch *ds, int port, u32 br_port_mask)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	u8 fid, newfid;
+	int ret;
+
+	fid = ps->fid[port];
+
+	if (ps->bridge_mask[fid] != br_port_mask) {
+		netdev_err(ds->ports[port],
+			   "leave: Bridge port mask mismatch fid=%d mask=0x%x expected 0x%x\n",
+			   fid, br_port_mask, ps->bridge_mask[fid]);
+		return -EINVAL;
+	}
+
+	/* If the port was the last port of a bridge, we are done.
+	 * Otherwise assign a new fid to the port, and fix up
+	 * the bridge configuration.
+	 */
+	if (br_port_mask == (1 << port))
+		return 0;
+
+	mutex_lock(&ps->smi_mutex);
+
+	newfid = __ffs(ps->fid_mask);
+	ps->fid[port] = newfid;
+	ps->fid_mask &= (1 << newfid);
+	ps->bridge_mask[fid] &= ~(1 << port);
+	ps->bridge_mask[newfid] = 1 << port;
+
+	ret = _mv88e6xxx_update_bridge_config(ds, fid);
+	if (!ret)
+		ret = _mv88e6xxx_update_bridge_config(ds, newfid);
+
+	mutex_unlock(&ps->smi_mutex);
+
+	return ret;
+}
+
+int mv88e6xxx_port_stp_update(struct dsa_switch *ds, int port, u8 state)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int stp_state;
+
+	switch (state) {
+	case BR_STATE_DISABLED:
+		stp_state = PORT_CONTROL_STATE_DISABLED;
+		break;
+	case BR_STATE_BLOCKING:
+	case BR_STATE_LISTENING:
+		stp_state = PORT_CONTROL_STATE_BLOCKING;
+		break;
+	case BR_STATE_LEARNING:
+		stp_state = PORT_CONTROL_STATE_LEARNING;
+		break;
+	case BR_STATE_FORWARDING:
+	default:
+		stp_state = PORT_CONTROL_STATE_FORWARDING;
+		break;
+	}
+
+	netdev_dbg(ds->ports[port], "port state %d [%d]\n", state, stp_state);
+
+	/* mv88e6xxx_port_stp_update may be called with softirqs disabled,
+	 * so we can not update the port state directly but need to schedule it.
+	 */
+	ps->port_state[port] = stp_state;
+	set_bit(port, &ps->port_state_update_mask);
+	schedule_work(&ps->bridge_work);
+
+	return 0;
+}
+
+static int __mv88e6xxx_write_addr(struct dsa_switch *ds,
+				  const unsigned char *addr)
+{
+	int i, ret;
+
+	for (i = 0; i < 3; i++) {
+		ret = _mv88e6xxx_reg_write(
+			ds, REG_GLOBAL, GLOBAL_ATU_MAC_01 + i,
+			(addr[i * 2] << 8) | addr[i * 2 + 1]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int __mv88e6xxx_read_addr(struct dsa_switch *ds, unsigned char *addr)
+{
+	int i, ret;
+
+	for (i = 0; i < 3; i++) {
+		ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL,
+					  GLOBAL_ATU_MAC_01 + i);
+		if (ret < 0)
+			return ret;
+		addr[i * 2] = ret >> 8;
+		addr[i * 2 + 1] = ret & 0xff;
+	}
+
+	return 0;
+}
+
+static int __mv88e6xxx_port_fdb_cmd(struct dsa_switch *ds, int port,
+				    const unsigned char *addr, int state)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	u8 fid = ps->fid[port];
+	int ret;
+
+	ret = _mv88e6xxx_atu_wait(ds);
+	if (ret < 0)
+		return ret;
+
+	ret = __mv88e6xxx_write_addr(ds, addr);
+	if (ret < 0)
+		return ret;
+
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_ATU_DATA,
+				   (0x10 << port) | state);
+	if (ret)
+		return ret;
+
+	ret = _mv88e6xxx_atu_cmd(ds, fid, GLOBAL_ATU_OP_LOAD_DB);
+
+	return ret;
+}
+
+int mv88e6xxx_port_fdb_add(struct dsa_switch *ds, int port,
+			   const unsigned char *addr, u16 vid)
+{
+	int state = is_multicast_ether_addr(addr) ?
+		GLOBAL_ATU_DATA_STATE_MC_STATIC :
+		GLOBAL_ATU_DATA_STATE_UC_STATIC;
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
+	mutex_lock(&ps->smi_mutex);
+	ret = __mv88e6xxx_port_fdb_cmd(ds, port, addr, state);
+	mutex_unlock(&ps->smi_mutex);
+
+	return ret;
+}
+
+int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
+			   const unsigned char *addr, u16 vid)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
+	mutex_lock(&ps->smi_mutex);
+	ret = __mv88e6xxx_port_fdb_cmd(ds, port, addr,
+				       GLOBAL_ATU_DATA_STATE_UNUSED);
+	mutex_unlock(&ps->smi_mutex);
+
+	return ret;
+}
+
+static int __mv88e6xxx_port_getnext(struct dsa_switch *ds, int port,
+				    unsigned char *addr, bool *is_static)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	u8 fid = ps->fid[port];
+	int ret, state;
+
+	ret = _mv88e6xxx_atu_wait(ds);
+	if (ret < 0)
+		return ret;
+
+	ret = __mv88e6xxx_write_addr(ds, addr);
+	if (ret < 0)
+		return ret;
+
+	do {
+		ret = _mv88e6xxx_atu_cmd(ds, fid,  GLOBAL_ATU_OP_GET_NEXT_DB);
+		if (ret < 0)
+			return ret;
+
+		ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_ATU_DATA);
+		if (ret < 0)
+			return ret;
+		state = ret & GLOBAL_ATU_DATA_STATE_MASK;
+		if (state == GLOBAL_ATU_DATA_STATE_UNUSED)
+			return -ENOENT;
+	} while (!(((ret >> 4) & 0xff) & (1 << port)));
+
+	ret = __mv88e6xxx_read_addr(ds, addr);
+	if (ret < 0)
+		return ret;
+
+	*is_static = state == (is_multicast_ether_addr(addr) ?
+			       GLOBAL_ATU_DATA_STATE_MC_STATIC :
+			       GLOBAL_ATU_DATA_STATE_UC_STATIC);
+
+	return 0;
+}
+
+/* get next entry for port */
+int mv88e6xxx_port_fdb_getnext(struct dsa_switch *ds, int port,
+			       unsigned char *addr, bool *is_static)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
+	mutex_lock(&ps->smi_mutex);
+	ret = __mv88e6xxx_port_getnext(ds, port, addr, is_static);
+	mutex_unlock(&ps->smi_mutex);
+
+	return ret;
+}
+
+static void mv88e6xxx_bridge_work(struct work_struct *work)
+{
+	struct mv88e6xxx_priv_state *ps;
+	struct dsa_switch *ds;
+	int port;
+
+	ps = container_of(work, struct mv88e6xxx_priv_state, bridge_work);
+	ds = ((struct dsa_switch *)ps) - 1;
+
+	while (ps->port_state_update_mask) {
+		port = __ffs(ps->port_state_update_mask);
+		clear_bit(port, &ps->port_state_update_mask);
+		mv88e6xxx_set_port_state(ds, port, ps->port_state[port]);
+	}
+}
+
+int mv88e6xxx_setup_port_common(struct dsa_switch *ds, int port)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret, fid;
+
+	mutex_lock(&ps->smi_mutex);
+
+	/* Port Control 1: disable trunking, disable sending
+	 * learning messages to this port.
+	 */
+	ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_DEFAULT_VLAN,
+				   0x0000);
+	if (ret)
+		goto abort;
+
+	/* Port based VLAN map: give each port its own address
+	 * database, allow the CPU port to talk to each of the 'real'
+	 * ports, and allow each of the 'real' ports to only talk to
+	 * the upstream port.
+	 */
+	fid = __ffs(ps->fid_mask);
+	ps->fid[port] = fid;
+	ps->fid_mask &= ~(1 << fid);
+
+	if (!dsa_is_cpu_port(ds, port))
+		ps->bridge_mask[fid] = 1 << port;
+
+	ret = _mv88e6xxx_update_port_config(ds, port);
+	if (ret)
+		goto abort;
+
+	/* Default VLAN ID and priority: don't set a default VLAN
+	 * ID, and set the default packet priority to zero.
+	 */
+	ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), 0x07, 0x0000);
+abort:
+	mutex_unlock(&ps->smi_mutex);
+	return ret;
+}
+
+int mv88e6xxx_setup_common(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	mutex_init(&ps->smi_mutex);
+	mutex_init(&ps->stats_mutex);
+	mutex_init(&ps->phy_mutex);
+
+	ps->id = REG_READ(REG_PORT(0), PORT_SWITCH_ID) & 0xfff0;
+
+	ps->fid_mask = (1 << DSA_MAX_PORTS) - 1;
+
+	INIT_WORK(&ps->bridge_work, mv88e6xxx_bridge_work);
+
+	return 0;
+}
+
+int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	u16 is_reset = (ppu_active ? 0x8800 : 0xc800);
+	unsigned long timeout;
+	int ret;
+	int i;
+
+	/* Set all ports to the disabled state. */
+	for (i = 0; i < ps->num_ports; i++) {
+		ret = REG_READ(REG_PORT(i), PORT_CONTROL);
+		REG_WRITE(REG_PORT(i), PORT_CONTROL, ret & 0xfffc);
+	}
+
+	/* Wait for transmit queues to drain. */
+	usleep_range(2000, 4000);
+
+	/* Reset the switch. Keep the PPU active if requested. The PPU
+	 * needs to be active to support indirect phy register access
+	 * through global registers 0x18 and 0x19.
+	 */
+	if (ppu_active)
+		REG_WRITE(REG_GLOBAL, 0x04, 0xc000);
+	else
+		REG_WRITE(REG_GLOBAL, 0x04, 0xc400);
+
+	/* Wait up to one second for reset to complete. */
+	timeout = jiffies + 1 * HZ;
+	while (time_before(jiffies, timeout)) {
+		ret = REG_READ(REG_GLOBAL, 0x00);
+		if ((ret & is_reset) == is_reset)
+			break;
+		usleep_range(1000, 2000);
+	}
+	if (time_after(jiffies, timeout))
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+int mv88e6xxx_phy_page_read(struct dsa_switch *ds, int port, int page, int reg)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
+	if (ret < 0)
+		goto error;
+	ret = _mv88e6xxx_phy_read_indirect(ds, port, reg);
+error:
+	_mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
+			     int reg, int val)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
+	if (ret < 0)
+		goto error;
+
+	ret = _mv88e6xxx_phy_write_indirect(ds, port, reg, val);
+error:
+	_mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+static int mv88e6xxx_port_to_phy_addr(struct dsa_switch *ds, int port)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	if (port >= 0 && port < ps->num_ports)
+		return port;
+	return -EINVAL;
+}
+
+int
+mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_read(ds, addr, regnum);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+int
+mv88e6xxx_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_write(ds, addr, regnum, val);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+int
+mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int port, int regnum)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_read_indirect(ds, addr, regnum);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+int
+mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int port, int regnum,
+			     u16 val)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_write_indirect(ds, addr, regnum, val);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
 static int __init mv88e6xxx_init(void)
 {
 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6131)
diff --git a/drivers/net/dsa/mv88e6xxx.h b/drivers/net/dsa/mv88e6xxx.h
index 03e397efde36..e045154f3364 100644
--- a/drivers/net/dsa/mv88e6xxx.h
+++ b/drivers/net/dsa/mv88e6xxx.h
@@ -11,9 +11,199 @@
 #ifndef __MV88E6XXX_H
 #define __MV88E6XXX_H
 
+#define SMI_CMD			0x00
+#define SMI_CMD_BUSY		BIT(15)
+#define SMI_CMD_CLAUSE_22	BIT(12)
+#define SMI_CMD_OP_22_WRITE	((1 << 10) | SMI_CMD_BUSY | SMI_CMD_CLAUSE_22)
+#define SMI_CMD_OP_22_READ	((2 << 10) | SMI_CMD_BUSY | SMI_CMD_CLAUSE_22)
+#define SMI_CMD_OP_45_WRITE_ADDR	((0 << 10) | SMI_CMD_BUSY)
+#define SMI_CMD_OP_45_WRITE_DATA	((1 << 10) | SMI_CMD_BUSY)
+#define SMI_CMD_OP_45_READ_DATA		((2 << 10) | SMI_CMD_BUSY)
+#define SMI_CMD_OP_45_READ_DATA_INC	((3 << 10) | SMI_CMD_BUSY)
+#define SMI_DATA		0x01
+
 #define REG_PORT(p)		(0x10 + (p))
+#define PORT_STATUS		0x00
+#define PORT_STATUS_PAUSE_EN	BIT(15)
+#define PORT_STATUS_MY_PAUSE	BIT(14)
+#define PORT_STATUS_HD_FLOW	BIT(13)
+#define PORT_STATUS_PHY_DETECT	BIT(12)
+#define PORT_STATUS_LINK	BIT(11)
+#define PORT_STATUS_DUPLEX	BIT(10)
+#define PORT_STATUS_SPEED_MASK	0x0300
+#define PORT_STATUS_SPEED_10	0x0000
+#define PORT_STATUS_SPEED_100	0x0100
+#define PORT_STATUS_SPEED_1000	0x0200
+#define PORT_STATUS_EEE		BIT(6) /* 6352 */
+#define PORT_STATUS_AM_DIS	BIT(6) /* 6165 */
+#define PORT_STATUS_MGMII	BIT(6) /* 6185 */
+#define PORT_STATUS_TX_PAUSED	BIT(5)
+#define PORT_STATUS_FLOW_CTRL	BIT(4)
+#define PORT_PCS_CTRL		0x01
+#define PORT_SWITCH_ID		0x03
+#define PORT_SWITCH_ID_6085	0x04a0
+#define PORT_SWITCH_ID_6095	0x0950
+#define PORT_SWITCH_ID_6123	0x1210
+#define PORT_SWITCH_ID_6123_A1	0x1212
+#define PORT_SWITCH_ID_6123_A2	0x1213
+#define PORT_SWITCH_ID_6131	0x1060
+#define PORT_SWITCH_ID_6131_B2	0x1066
+#define PORT_SWITCH_ID_6152	0x1a40
+#define PORT_SWITCH_ID_6155	0x1a50
+#define PORT_SWITCH_ID_6161	0x1610
+#define PORT_SWITCH_ID_6161_A1	0x1612
+#define PORT_SWITCH_ID_6161_A2	0x1613
+#define PORT_SWITCH_ID_6165	0x1650
+#define PORT_SWITCH_ID_6165_A1	0x1652
+#define PORT_SWITCH_ID_6165_A2	0x1653
+#define PORT_SWITCH_ID_6171	0x1710
+#define PORT_SWITCH_ID_6172	0x1720
+#define PORT_SWITCH_ID_6176	0x1760
+#define PORT_SWITCH_ID_6182	0x1a60
+#define PORT_SWITCH_ID_6185	0x1a70
+#define PORT_SWITCH_ID_6352	0x3520
+#define PORT_SWITCH_ID_6352_A0	0x3521
+#define PORT_SWITCH_ID_6352_A1	0x3522
+#define PORT_CONTROL		0x04
+#define PORT_CONTROL_STATE_MASK		0x03
+#define PORT_CONTROL_STATE_DISABLED	0x00
+#define PORT_CONTROL_STATE_BLOCKING	0x01
+#define PORT_CONTROL_STATE_LEARNING	0x02
+#define PORT_CONTROL_STATE_FORWARDING	0x03
+#define PORT_CONTROL_1		0x05
+#define PORT_BASE_VLAN		0x06
+#define PORT_DEFAULT_VLAN	0x07
+#define PORT_CONTROL_2		0x08
+#define PORT_RATE_CONTROL	0x09
+#define PORT_RATE_CONTROL_2	0x0a
+#define PORT_ASSOC_VECTOR	0x0b
+#define PORT_IN_DISCARD_LO	0x10
+#define PORT_IN_DISCARD_HI	0x11
+#define PORT_IN_FILTERED	0x12
+#define PORT_OUT_FILTERED	0x13
+#define PORT_TAG_REGMAP_0123	0x19
+#define PORT_TAG_REGMAP_4567	0x1a
+
 #define REG_GLOBAL		0x1b
+#define GLOBAL_STATUS		0x00
+#define GLOBAL_STATUS_PPU_STATE BIT(15) /* 6351 and 6171 */
+/* Two bits for 6165, 6185 etc */
+#define GLOBAL_STATUS_PPU_MASK		(0x3 << 14)
+#define GLOBAL_STATUS_PPU_DISABLED_RST	(0x0 << 14)
+#define GLOBAL_STATUS_PPU_INITIALIZING	(0x1 << 14)
+#define GLOBAL_STATUS_PPU_DISABLED	(0x2 << 14)
+#define GLOBAL_STATUS_PPU_POLLING	(0x3 << 14)
+#define GLOBAL_MAC_01		0x01
+#define GLOBAL_MAC_23		0x02
+#define GLOBAL_MAC_45		0x03
+#define GLOBAL_CONTROL		0x04
+#define GLOBAL_CONTROL_SW_RESET		BIT(15)
+#define GLOBAL_CONTROL_PPU_ENABLE	BIT(14)
+#define GLOBAL_CONTROL_DISCARD_EXCESS	BIT(13) /* 6352 */
+#define GLOBAL_CONTROL_SCHED_PRIO	BIT(11) /* 6152 */
+#define GLOBAL_CONTROL_MAX_FRAME_1632	BIT(10) /* 6152 */
+#define GLOBAL_CONTROL_RELOAD_EEPROM	BIT(9)  /* 6152 */
+#define GLOBAL_CONTROL_DEVICE_EN	BIT(7)
+#define GLOBAL_CONTROL_STATS_DONE_EN	BIT(6)
+#define GLOBAL_CONTROL_VTU_PROBLEM_EN	BIT(5)
+#define GLOBAL_CONTROL_VTU_DONE_EN	BIT(4)
+#define GLOBAL_CONTROL_ATU_PROBLEM_EN	BIT(3)
+#define GLOBAL_CONTROL_ATU_DONE_EN	BIT(2)
+#define GLOBAL_CONTROL_TCAM_EN		BIT(1)
+#define GLOBAL_CONTROL_EEPROM_DONE_EN	BIT(0)
+#define GLOBAL_VTU_OP		0x05
+#define GLOBAL_VTU_VID		0x06
+#define GLOBAL_VTU_DATA_0_3	0x07
+#define GLOBAL_VTU_DATA_4_7	0x08
+#define GLOBAL_VTU_DATA_8_11	0x09
+#define GLOBAL_ATU_CONTROL	0x0a
+#define GLOBAL_ATU_OP		0x0b
+#define GLOBAL_ATU_OP_BUSY	BIT(15)
+#define GLOBAL_ATU_OP_NOP		(0 << 12)
+#define GLOBAL_ATU_OP_FLUSH_ALL		((1 << 12) | GLOBAL_ATU_OP_BUSY)
+#define GLOBAL_ATU_OP_FLUSH_NON_STATIC	((2 << 12) | GLOBAL_ATU_OP_BUSY)
+#define GLOBAL_ATU_OP_LOAD_DB		((3 << 12) | GLOBAL_ATU_OP_BUSY)
+#define GLOBAL_ATU_OP_GET_NEXT_DB	((4 << 12) | GLOBAL_ATU_OP_BUSY)
+#define GLOBAL_ATU_OP_FLUSH_DB		((5 << 12) | GLOBAL_ATU_OP_BUSY)
+#define GLOBAL_ATU_OP_FLUSH_NON_STATIC_DB ((6 << 12) | GLOBAL_ATU_OP_BUSY)
+#define GLOBAL_ATU_OP_GET_CLR_VIOLATION	  ((7 << 12) | GLOBAL_ATU_OP_BUSY)
+#define GLOBAL_ATU_DATA		0x0c
+#define GLOBAL_ATU_DATA_STATE_MASK		0x0f
+#define GLOBAL_ATU_DATA_STATE_UNUSED		0x00
+#define GLOBAL_ATU_DATA_STATE_UC_MGMT		0x0d
+#define GLOBAL_ATU_DATA_STATE_UC_STATIC		0x0e
+#define GLOBAL_ATU_DATA_STATE_UC_PRIO_OVER	0x0f
+#define GLOBAL_ATU_DATA_STATE_MC_NONE_RATE	0x05
+#define GLOBAL_ATU_DATA_STATE_MC_STATIC		0x07
+#define GLOBAL_ATU_DATA_STATE_MC_MGMT		0x0e
+#define GLOBAL_ATU_DATA_STATE_MC_PRIO_OVER	0x0f
+#define GLOBAL_ATU_MAC_01	0x0d
+#define GLOBAL_ATU_MAC_23	0x0e
+#define GLOBAL_ATU_MAC_45	0x0f
+#define GLOBAL_IP_PRI_0		0x10
+#define GLOBAL_IP_PRI_1		0x11
+#define GLOBAL_IP_PRI_2		0x12
+#define GLOBAL_IP_PRI_3		0x13
+#define GLOBAL_IP_PRI_4		0x14
+#define GLOBAL_IP_PRI_5		0x15
+#define GLOBAL_IP_PRI_6		0x16
+#define GLOBAL_IP_PRI_7		0x17
+#define GLOBAL_IEEE_PRI		0x18
+#define GLOBAL_CORE_TAG_TYPE	0x19
+#define GLOBAL_MONITOR_CONTROL	0x1a
+#define GLOBAL_CONTROL_2	0x1c
+#define GLOBAL_STATS_OP		0x1d
+#define GLOBAL_STATS_OP_BUSY	BIT(15)
+#define GLOBAL_STATS_OP_NOP		(0 << 12)
+#define GLOBAL_STATS_OP_FLUSH_ALL	((1 << 12) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_OP_FLUSH_PORT	((2 << 12) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_OP_READ_CAPTURED	((4 << 12) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_OP_CAPTURE_PORT	((5 << 12) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_OP_HIST_RX		((1 << 10) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_OP_HIST_TX		((2 << 10) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_OP_HIST_RX_TX	((3 << 10) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_COUNTER_32	0x1e
+#define GLOBAL_STATS_COUNTER_01	0x1f
+
 #define REG_GLOBAL2		0x1c
+#define GLOBAL2_INT_SOURCE	0x00
+#define GLOBAL2_INT_MASK	0x01
+#define GLOBAL2_MGMT_EN_2X	0x02
+#define GLOBAL2_MGMT_EN_0X	0x03
+#define GLOBAL2_FLOW_CONTROL	0x04
+#define GLOBAL2_SWITCH_MGMT	0x05
+#define GLOBAL2_DEVICE_MAPPING	0x06
+#define GLOBAL2_TRUNK_MASK	0x07
+#define GLOBAL2_TRUNK_MAPPING	0x08
+#define GLOBAL2_INGRESS_OP	0x09
+#define GLOBAL2_INGRESS_DATA	0x0a
+#define GLOBAL2_PVT_ADDR	0x0b
+#define GLOBAL2_PVT_DATA	0x0c
+#define GLOBAL2_SWITCH_MAC	0x0d
+#define GLOBAL2_SWITCH_MAC_BUSY BIT(15)
+#define GLOBAL2_ATU_STATS	0x0e
+#define GLOBAL2_PRIO_OVERRIDE	0x0f
+#define GLOBAL2_EEPROM_OP	0x14
+#define GLOBAL2_EEPROM_OP_BUSY	BIT(15)
+#define GLOBAL2_EEPROM_OP_LOAD	BIT(11)
+#define GLOBAL2_EEPROM_DATA	0x15
+#define GLOBAL2_PTP_AVB_OP	0x16
+#define GLOBAL2_PTP_AVB_DATA	0x17
+#define GLOBAL2_SMI_OP		0x18
+#define GLOBAL2_SMI_OP_BUSY		BIT(15)
+#define GLOBAL2_SMI_OP_CLAUSE_22	BIT(12)
+#define GLOBAL2_SMI_OP_22_WRITE		((1 << 10) | GLOBAL2_SMI_OP_BUSY | \
+					 GLOBAL2_SMI_OP_CLAUSE_22)
+#define GLOBAL2_SMI_OP_22_READ		((2 << 10) | GLOBAL2_SMI_OP_BUSY | \
+					 GLOBAL2_SMI_OP_CLAUSE_22)
+#define GLOBAL2_SMI_OP_45_WRITE_ADDR	((0 << 10) | GLOBAL2_SMI_OP_BUSY)
+#define GLOBAL2_SMI_OP_45_WRITE_DATA	((1 << 10) | GLOBAL2_SMI_OP_BUSY)
+#define GLOBAL2_SMI_OP_45_READ_DATA	((2 << 10) | GLOBAL2_SMI_OP_BUSY)
+#define GLOBAL2_SMI_DATA	0x19
+#define GLOBAL2_SCRATCH_MISC	0x1a
+#define GLOBAL2_WDOG_CONTROL	0x1b
+#define GLOBAL2_QOS_WEIGHT	0x1c
+#define GLOBAL2_MISC		0x1d
 
 struct mv88e6xxx_priv_state {
 	/* When using multi-chip addressing, this mutex protects
@@ -49,6 +239,18 @@ struct mv88e6xxx_priv_state {
 	struct mutex eeprom_mutex;
 
 	int		id; /* switch product id */
+	int		num_ports;	/* number of switch ports */
+
+	/* hw bridging */
+
+	u32 fid_mask;
+	u8 fid[DSA_MAX_PORTS];
+	u16 bridge_mask[DSA_MAX_PORTS];
+
+	unsigned long port_state_update_mask;
+	u8 port_state[DSA_MAX_PORTS];
+
+	struct work_struct bridge_work;
 };
 
 struct mv88e6xxx_hw_stat {
@@ -57,6 +259,9 @@ struct mv88e6xxx_hw_stat {
 	int reg;
 };
 
+int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active);
+int mv88e6xxx_setup_port_common(struct dsa_switch *ds, int port);
+int mv88e6xxx_setup_common(struct dsa_switch *ds);
 int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg);
 int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg);
 int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
@@ -65,24 +270,46 @@ int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val);
 int mv88e6xxx_config_prio(struct dsa_switch *ds);
 int mv88e6xxx_set_addr_direct(struct dsa_switch *ds, u8 *addr);
 int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr);
-int mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum);
-int mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum, u16 val);
+int mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum);
+int mv88e6xxx_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val);
+int mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int port, int regnum);
+int mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int port, int regnum,
+				 u16 val);
 void mv88e6xxx_ppu_state_init(struct dsa_switch *ds);
 int mv88e6xxx_phy_read_ppu(struct dsa_switch *ds, int addr, int regnum);
 int mv88e6xxx_phy_write_ppu(struct dsa_switch *ds, int addr,
 			    int regnum, u16 val);
 void mv88e6xxx_poll_link(struct dsa_switch *ds);
-void mv88e6xxx_get_strings(struct dsa_switch *ds,
-			   int nr_stats, struct mv88e6xxx_hw_stat *stats,
-			   int port, uint8_t *data);
-void mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
-				 int nr_stats, struct mv88e6xxx_hw_stat *stats,
-				 int port, uint64_t *data);
+void mv88e6xxx_get_strings(struct dsa_switch *ds, int port, uint8_t *data);
+void mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds, int port,
+				 uint64_t *data);
+int mv88e6xxx_get_sset_count(struct dsa_switch *ds);
+int mv88e6xxx_get_sset_count_basic(struct dsa_switch *ds);
 int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port);
 void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
 			struct ethtool_regs *regs, void *_p);
 int  mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp);
-
+int mv88e6xxx_phy_wait(struct dsa_switch *ds);
+int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds);
+int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds);
+int mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr, int regnum);
+int mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int addr, int regnum,
+				 u16 val);
+int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e);
+int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
+		      struct phy_device *phydev, struct ethtool_eee *e);
+int mv88e6xxx_join_bridge(struct dsa_switch *ds, int port, u32 br_port_mask);
+int mv88e6xxx_leave_bridge(struct dsa_switch *ds, int port, u32 br_port_mask);
+int mv88e6xxx_port_stp_update(struct dsa_switch *ds, int port, u8 state);
+int mv88e6xxx_port_fdb_add(struct dsa_switch *ds, int port,
+			   const unsigned char *addr, u16 vid);
+int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
+			   const unsigned char *addr, u16 vid);
+int mv88e6xxx_port_fdb_getnext(struct dsa_switch *ds, int port,
+			       unsigned char *addr, bool *is_static);
+int mv88e6xxx_phy_page_read(struct dsa_switch *ds, int port, int page, int reg);
+int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
+			     int reg, int val);
 extern struct dsa_switch_driver mv88e6131_switch_driver;
 extern struct dsa_switch_driver mv88e6123_61_65_switch_driver;
 extern struct dsa_switch_driver mv88e6352_switch_driver;
diff --git a/drivers/net/ethernet/adi/bfin_mac.c b/drivers/net/ethernet/adi/bfin_mac.c
index ec20611e9de2..096531a73124 100644
--- a/drivers/net/ethernet/adi/bfin_mac.c
+++ b/drivers/net/ethernet/adi/bfin_mac.c
@@ -983,10 +983,9 @@ static int bfin_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int bfin_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int bfin_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	u64 ns;
-	u32 remainder;
 	unsigned long flags;
 	struct bfin_mac_local *lp =
 		container_of(ptp, struct bfin_mac_local, caps);
@@ -997,21 +996,20 @@ static int bfin_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 
 	spin_unlock_irqrestore(&lp->phc_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
+
 	return 0;
 }
 
 static int bfin_ptp_settime(struct ptp_clock_info *ptp,
-			   const struct timespec *ts)
+			   const struct timespec64 *ts)
 {
 	u64 ns;
 	unsigned long flags;
 	struct bfin_mac_local *lp =
 		container_of(ptp, struct bfin_mac_local, caps);
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 
 	spin_lock_irqsave(&lp->phc_lock, flags);
 
@@ -1039,8 +1037,8 @@ static struct ptp_clock_info bfin_ptp_caps = {
 	.pps		= 0,
 	.adjfreq	= bfin_ptp_adjfreq,
 	.adjtime	= bfin_ptp_adjtime,
-	.gettime	= bfin_ptp_gettime,
-	.settime	= bfin_ptp_settime,
+	.gettime64	= bfin_ptp_gettime,
+	.settime64	= bfin_ptp_settime,
 	.enable		= bfin_ptp_enable,
 };
 
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index 2b8bfeeee9cf..ae89de7deb13 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -1588,7 +1588,7 @@ static int greth_of_remove(struct platform_device *of_dev)
 	return 0;
 }
 
-static struct of_device_id greth_of_match[] = {
+static const struct of_device_id greth_of_match[] = {
 	{
 	 .name = "GAISLER_ETHMAC",
 	 },
diff --git a/drivers/net/ethernet/allwinner/sun4i-emac.c b/drivers/net/ethernet/allwinner/sun4i-emac.c
index f3470d96837a..bab01c849165 100644
--- a/drivers/net/ethernet/allwinner/sun4i-emac.c
+++ b/drivers/net/ethernet/allwinner/sun4i-emac.c
@@ -757,7 +757,7 @@ static void emac_shutdown(struct net_device *dev)
 	/* Disable all interrupt */
 	writel(0, db->membase + EMAC_INT_CTL_REG);
 
-	/* clear interupt status */
+	/* clear interrupt status */
 	reg_val = readl(db->membase + EMAC_INT_STA_REG);
 	writel(reg_val, db->membase + EMAC_INT_STA_REG);
 
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index 6725dc00750b..79ea35869e1e 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -89,7 +89,7 @@ MODULE_PARM_DESC(dma_tx_num, "Number of descriptors in the TX list");
 
 #define TXQUEUESTOP_THRESHHOLD	2
 
-static struct of_device_id altera_tse_ids[];
+static const struct of_device_id altera_tse_ids[];
 
 static inline u32 tse_tx_avail(struct altera_tse_private *priv)
 {
@@ -105,11 +105,11 @@ static int altera_tse_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 
 	/* set MDIO address */
 	csrwr32((mii_id & 0x1f), priv->mac_dev,
-		tse_csroffs(mdio_phy0_addr));
+		tse_csroffs(mdio_phy1_addr));
 
 	/* get the data */
 	return csrrd32(priv->mac_dev,
-		       tse_csroffs(mdio_phy0) + regnum * 4) & 0xffff;
+		       tse_csroffs(mdio_phy1) + regnum * 4) & 0xffff;
 }
 
 static int altera_tse_mdio_write(struct mii_bus *bus, int mii_id, int regnum,
@@ -120,10 +120,10 @@ static int altera_tse_mdio_write(struct mii_bus *bus, int mii_id, int regnum,
 
 	/* set MDIO address */
 	csrwr32((mii_id & 0x1f), priv->mac_dev,
-		tse_csroffs(mdio_phy0_addr));
+		tse_csroffs(mdio_phy1_addr));
 
 	/* write the data */
-	csrwr32(value, priv->mac_dev, tse_csroffs(mdio_phy0) + regnum * 4);
+	csrwr32(value, priv->mac_dev, tse_csroffs(mdio_phy1) + regnum * 4);
 	return 0;
 }
 
@@ -1098,8 +1098,12 @@ static int tse_open(struct net_device *dev)
 
 	spin_lock(&priv->mac_cfg_lock);
 	ret = reset_mac(priv);
+	/* Note that reset_mac will fail if the clocks are gated by the PHY
+	 * due to the PHY being put into isolation or power down mode.
+	 * This is not an error if reset fails due to no clock.
+	 */
 	if (ret)
-		netdev_err(dev, "Cannot reset MAC core (error: %d)\n", ret);
+		netdev_dbg(dev, "Cannot reset MAC core (error: %d)\n", ret);
 
 	ret = init_mac(priv);
 	spin_unlock(&priv->mac_cfg_lock);
@@ -1203,8 +1207,12 @@ static int tse_shutdown(struct net_device *dev)
 	spin_lock(&priv->tx_lock);
 
 	ret = reset_mac(priv);
+	/* Note that reset_mac will fail if the clocks are gated by the PHY
+	 * due to the PHY being put into isolation or power down mode.
+	 * This is not an error if reset fails due to no clock.
+	 */
 	if (ret)
-		netdev_err(dev, "Cannot reset MAC core (error: %d)\n", ret);
+		netdev_dbg(dev, "Cannot reset MAC core (error: %d)\n", ret);
 	priv->dmaops->reset_dma(priv);
 	free_skbufs(dev);
 
@@ -1568,7 +1576,7 @@ static const struct altera_dmaops altera_dtype_msgdma = {
 	.start_rxdma = msgdma_start_rxdma,
 };
 
-static struct of_device_id altera_tse_ids[] = {
+static const struct of_device_id altera_tse_ids[] = {
 	{ .compatible = "altr,tse-msgdma-1.0", .data = &altera_dtype_msgdma, },
 	{ .compatible = "altr,tse-1.0", .data = &altera_dtype_sgdma, },
 	{ .compatible = "ALTR,tse-1.0", .data = &altera_dtype_sgdma, },
diff --git a/drivers/net/ethernet/amd/amd8111e.c b/drivers/net/ethernet/amd/amd8111e.c
index 4c2ae2221780..94960055fa1f 100644
--- a/drivers/net/ethernet/amd/amd8111e.c
+++ b/drivers/net/ethernet/amd/amd8111e.c
@@ -723,13 +723,13 @@ static int amd8111e_rx_poll(struct napi_struct *napi, int budget)
 			 * the last correctly noting the error.
 			 */
 			if(status & ERR_BIT) {
-				/* reseting flags */
+				/* resetting flags */
 				lp->rx_ring[rx_index].rx_flags &= RESET_RX_FLAGS;
 				goto err_next_pkt;
 			}
 			/* check for STP and ENP */
 			if(!((status & STP_BIT) && (status & ENP_BIT))){
-				/* reseting flags */
+				/* resetting flags */
 				lp->rx_ring[rx_index].rx_flags &= RESET_RX_FLAGS;
 				goto err_next_pkt;
 			}
diff --git a/drivers/net/ethernet/amd/amd8111e.h b/drivers/net/ethernet/amd/amd8111e.h
index a75092d584cc..7cdb18512407 100644
--- a/drivers/net/ethernet/amd/amd8111e.h
+++ b/drivers/net/ethernet/amd/amd8111e.h
@@ -614,7 +614,7 @@ typedef enum {
 /* Assume contoller gets data 10 times the maximum processing time */
 #define  REPEAT_CNT			10
 
-/* amd8111e decriptor flag definitions */
+/* amd8111e descriptor flag definitions */
 typedef enum {
 
 	OWN_BIT		=	(1 << 15),
diff --git a/drivers/net/ethernet/amd/pcnet32.c b/drivers/net/ethernet/amd/pcnet32.c
index 11d6e6561df1..bc8b04f42882 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;
@@ -1708,7 +1735,7 @@ pcnet32_probe1(unsigned long ioaddr, int shared, struct pci_dev *pdev)
 
 	/* if the ethernet address is not valid, force to 00:00:00:00:00:00 */
 	if (!is_valid_ether_addr(dev->dev_addr))
-		memset(dev->dev_addr, 0, ETH_ALEN);
+		eth_zero_addr(dev->dev_addr);
 
 	if (pcnet32_debug & NETIF_MSG_PROBE) {
 		pr_cont(" %pM", dev->dev_addr);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-common.h b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
index 29a09271b64a..34c28aac767f 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-common.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
@@ -365,6 +365,8 @@
 #define MAC_HWF0R_TXCOESEL_WIDTH	1
 #define MAC_HWF0R_VLHASH_INDEX		4
 #define MAC_HWF0R_VLHASH_WIDTH		1
+#define MAC_HWF1R_ADDR64_INDEX		14
+#define MAC_HWF1R_ADDR64_WIDTH		2
 #define MAC_HWF1R_ADVTHWORD_INDEX	13
 #define MAC_HWF1R_ADVTHWORD_WIDTH	1
 #define MAC_HWF1R_DBGMEMA_INDEX		19
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
index 400757b49872..21d9497518fd 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
@@ -853,6 +853,22 @@ static int xgbe_set_mac_address(struct xgbe_prv_data *pdata, u8 *addr)
 	return 0;
 }
 
+static int xgbe_config_rx_mode(struct xgbe_prv_data *pdata)
+{
+	struct net_device *netdev = pdata->netdev;
+	unsigned int pr_mode, am_mode;
+
+	pr_mode = ((netdev->flags & IFF_PROMISC) != 0);
+	am_mode = ((netdev->flags & IFF_ALLMULTI) != 0);
+
+	xgbe_set_promiscuous_mode(pdata, pr_mode);
+	xgbe_set_all_multicast_mode(pdata, am_mode);
+
+	xgbe_add_mac_addresses(pdata);
+
+	return 0;
+}
+
 static int xgbe_read_mmd_regs(struct xgbe_prv_data *pdata, int prtad,
 			      int mmd_reg)
 {
@@ -1068,7 +1084,7 @@ static void xgbe_tx_desc_reset(struct xgbe_ring_data *rdata)
 	rdesc->desc3 = 0;
 
 	/* Make sure ownership is written to the descriptor */
-	wmb();
+	dma_wmb();
 }
 
 static void xgbe_tx_desc_init(struct xgbe_channel *channel)
@@ -1101,9 +1117,24 @@ static void xgbe_tx_desc_init(struct xgbe_channel *channel)
 	DBGPR("<--tx_desc_init\n");
 }
 
-static void xgbe_rx_desc_reset(struct xgbe_ring_data *rdata)
+static void xgbe_rx_desc_reset(struct xgbe_prv_data *pdata,
+			       struct xgbe_ring_data *rdata, unsigned int index)
 {
 	struct xgbe_ring_desc *rdesc = rdata->rdesc;
+	unsigned int rx_usecs = pdata->rx_usecs;
+	unsigned int rx_frames = pdata->rx_frames;
+	unsigned int inte;
+
+	if (!rx_usecs && !rx_frames) {
+		/* No coalescing, interrupt for every descriptor */
+		inte = 1;
+	} else {
+		/* Set interrupt based on Rx frame coalescing setting */
+		if (rx_frames && !((index + 1) % rx_frames))
+			inte = 1;
+		else
+			inte = 0;
+	}
 
 	/* Reset the Rx descriptor
 	 *   Set buffer 1 (lo) address to header dma address (lo)
@@ -1117,19 +1148,18 @@ static void xgbe_rx_desc_reset(struct xgbe_ring_data *rdata)
 	rdesc->desc2 = cpu_to_le32(lower_32_bits(rdata->rx.buf.dma));
 	rdesc->desc3 = cpu_to_le32(upper_32_bits(rdata->rx.buf.dma));
 
-	XGMAC_SET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, INTE,
-			  rdata->interrupt ? 1 : 0);
+	XGMAC_SET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, INTE, inte);
 
 	/* Since the Rx DMA engine is likely running, make sure everything
 	 * is written to the descriptor(s) before setting the OWN bit
 	 * for the descriptor
 	 */
-	wmb();
+	dma_wmb();
 
 	XGMAC_SET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, OWN, 1);
 
 	/* Make sure ownership is written to the descriptor */
-	wmb();
+	dma_wmb();
 }
 
 static void xgbe_rx_desc_init(struct xgbe_channel *channel)
@@ -1138,26 +1168,16 @@ static void xgbe_rx_desc_init(struct xgbe_channel *channel)
 	struct xgbe_ring *ring = channel->rx_ring;
 	struct xgbe_ring_data *rdata;
 	unsigned int start_index = ring->cur;
-	unsigned int rx_coalesce, rx_frames;
 	unsigned int i;
 
 	DBGPR("-->rx_desc_init\n");
 
-	rx_coalesce = (pdata->rx_riwt || pdata->rx_frames) ? 1 : 0;
-	rx_frames = pdata->rx_frames;
-
 	/* Initialize all descriptors */
 	for (i = 0; i < ring->rdesc_count; i++) {
 		rdata = XGBE_GET_DESC_DATA(ring, i);
 
-		/* Set interrupt on completion bit as appropriate */
-		if (rx_coalesce && (!rx_frames || ((i + 1) % rx_frames)))
-			rdata->interrupt = 0;
-		else
-			rdata->interrupt = 1;
-
 		/* Initialize Rx descriptor */
-		xgbe_rx_desc_reset(rdata);
+		xgbe_rx_desc_reset(pdata, rdata, i);
 	}
 
 	/* Update the total number of Rx descriptors */
@@ -1358,18 +1378,20 @@ static void xgbe_tx_start_xmit(struct xgbe_channel *channel,
 	struct xgbe_prv_data *pdata = channel->pdata;
 	struct xgbe_ring_data *rdata;
 
+	/* Make sure everything is written before the register write */
+	wmb();
+
 	/* Issue a poll command to Tx DMA by writing address
 	 * of next immediate free descriptor */
 	rdata = XGBE_GET_DESC_DATA(ring, ring->cur);
 	XGMAC_DMA_IOWRITE(channel, DMA_CH_TDTR_LO,
 			  lower_32_bits(rdata->rdesc_dma));
 
-	/* Start the Tx coalescing timer */
+	/* Start the Tx timer */
 	if (pdata->tx_usecs && !channel->tx_timer_active) {
 		channel->tx_timer_active = 1;
-		hrtimer_start(&channel->tx_timer,
-			      ktime_set(0, pdata->tx_usecs * NSEC_PER_USEC),
-			      HRTIMER_MODE_REL);
+		mod_timer(&channel->tx_timer,
+			  jiffies + usecs_to_jiffies(pdata->tx_usecs));
 	}
 
 	ring->tx.xmit_more = 0;
@@ -1565,7 +1587,7 @@ static void xgbe_dev_xmit(struct xgbe_channel *channel)
 	 * is written to the descriptor(s) before setting the OWN bit
 	 * for the first descriptor
 	 */
-	wmb();
+	dma_wmb();
 
 	/* Set OWN bit for the first descriptor */
 	rdata = XGBE_GET_DESC_DATA(ring, start_index);
@@ -1577,7 +1599,7 @@ static void xgbe_dev_xmit(struct xgbe_channel *channel)
 #endif
 
 	/* Make sure ownership is written to the descriptor */
-	wmb();
+	dma_wmb();
 
 	ring->cur = cur_index + 1;
 	if (!packet->skb->xmit_more ||
@@ -1613,7 +1635,7 @@ static int xgbe_dev_read(struct xgbe_channel *channel)
 		return 1;
 
 	/* Make sure descriptor fields are read after reading the OWN bit */
-	rmb();
+	dma_rmb();
 
 #ifdef XGMAC_ENABLE_RX_DESC_DUMP
 	xgbe_dump_rx_desc(ring, rdesc, ring->cur);
@@ -2004,7 +2026,8 @@ static void xgbe_config_tx_fifo_size(struct xgbe_prv_data *pdata)
 	for (i = 0; i < pdata->tx_q_count; i++)
 		XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_Q_TQOMR, TQS, fifo_size);
 
-	netdev_notice(pdata->netdev, "%d Tx queues, %d byte fifo per queue\n",
+	netdev_notice(pdata->netdev,
+		      "%d Tx hardware queues, %d byte fifo per queue\n",
 		      pdata->tx_q_count, ((fifo_size + 1) * 256));
 }
 
@@ -2019,7 +2042,8 @@ static void xgbe_config_rx_fifo_size(struct xgbe_prv_data *pdata)
 	for (i = 0; i < pdata->rx_q_count; i++)
 		XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_Q_RQOMR, RQS, fifo_size);
 
-	netdev_notice(pdata->netdev, "%d Rx queues, %d byte fifo per queue\n",
+	netdev_notice(pdata->netdev,
+		      "%d Rx hardware queues, %d byte fifo per queue\n",
 		      pdata->rx_q_count, ((fifo_size + 1) * 256));
 }
 
@@ -2800,6 +2824,7 @@ static int xgbe_init(struct xgbe_prv_data *pdata)
 	 * Initialize MAC related features
 	 */
 	xgbe_config_mac_address(pdata);
+	xgbe_config_rx_mode(pdata);
 	xgbe_config_jumbo_enable(pdata);
 	xgbe_config_flow_control(pdata);
 	xgbe_config_mac_speed(pdata);
@@ -2819,10 +2844,8 @@ void xgbe_init_function_ptrs_dev(struct xgbe_hw_if *hw_if)
 
 	hw_if->tx_complete = xgbe_tx_complete;
 
-	hw_if->set_promiscuous_mode = xgbe_set_promiscuous_mode;
-	hw_if->set_all_multicast_mode = xgbe_set_all_multicast_mode;
-	hw_if->add_mac_addresses = xgbe_add_mac_addresses;
 	hw_if->set_mac_address = xgbe_set_mac_address;
+	hw_if->config_rx_mode = xgbe_config_rx_mode;
 
 	hw_if->enable_rx_csum = xgbe_enable_rx_csum;
 	hw_if->disable_rx_csum = xgbe_disable_rx_csum;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index 885b02b5be07..db84ddcfec84 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -129,7 +129,6 @@
 
 static int xgbe_one_poll(struct napi_struct *, int);
 static int xgbe_all_poll(struct napi_struct *, int);
-static void xgbe_set_rx_mode(struct net_device *);
 
 static int xgbe_alloc_channels(struct xgbe_prv_data *pdata)
 {
@@ -411,11 +410,9 @@ static irqreturn_t xgbe_dma_isr(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static enum hrtimer_restart xgbe_tx_timer(struct hrtimer *timer)
+static void xgbe_tx_timer(unsigned long data)
 {
-	struct xgbe_channel *channel = container_of(timer,
-						    struct xgbe_channel,
-						    tx_timer);
+	struct xgbe_channel *channel = (struct xgbe_channel *)data;
 	struct xgbe_prv_data *pdata = channel->pdata;
 	struct napi_struct *napi;
 
@@ -437,8 +434,6 @@ static enum hrtimer_restart xgbe_tx_timer(struct hrtimer *timer)
 	channel->tx_timer_active = 0;
 
 	DBGPR("<--xgbe_tx_timer\n");
-
-	return HRTIMER_NORESTART;
 }
 
 static void xgbe_init_tx_timers(struct xgbe_prv_data *pdata)
@@ -454,9 +449,8 @@ static void xgbe_init_tx_timers(struct xgbe_prv_data *pdata)
 			break;
 
 		DBGPR("  %s adding tx timer\n", channel->name);
-		hrtimer_init(&channel->tx_timer, CLOCK_MONOTONIC,
-			     HRTIMER_MODE_REL);
-		channel->tx_timer.function = xgbe_tx_timer;
+		setup_timer(&channel->tx_timer, xgbe_tx_timer,
+			    (unsigned long)channel);
 	}
 
 	DBGPR("<--xgbe_init_tx_timers\n");
@@ -475,8 +469,7 @@ static void xgbe_stop_tx_timers(struct xgbe_prv_data *pdata)
 			break;
 
 		DBGPR("  %s deleting tx timer\n", channel->name);
-		channel->tx_timer_active = 0;
-		hrtimer_cancel(&channel->tx_timer);
+		del_timer_sync(&channel->tx_timer);
 	}
 
 	DBGPR("<--xgbe_stop_tx_timers\n");
@@ -519,6 +512,7 @@ void xgbe_get_all_hw_features(struct xgbe_prv_data *pdata)
 						RXFIFOSIZE);
 	hw_feat->tx_fifo_size  = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R,
 						TXFIFOSIZE);
+	hw_feat->dma_width     = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, ADDR64);
 	hw_feat->dcb           = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, DCBEN);
 	hw_feat->sph           = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, SPHEN);
 	hw_feat->tso           = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, TSOEN);
@@ -553,6 +547,21 @@ void xgbe_get_all_hw_features(struct xgbe_prv_data *pdata)
 		break;
 	}
 
+	/* Translate the address width setting into actual number */
+	switch (hw_feat->dma_width) {
+	case 0:
+		hw_feat->dma_width = 32;
+		break;
+	case 1:
+		hw_feat->dma_width = 40;
+		break;
+	case 2:
+		hw_feat->dma_width = 48;
+		break;
+	default:
+		hw_feat->dma_width = 32;
+	}
+
 	/* The Queue, Channel and TC counts are zero based so increment them
 	 * to get the actual number
 	 */
@@ -692,6 +701,7 @@ void xgbe_init_rx_coalesce(struct xgbe_prv_data *pdata)
 	DBGPR("-->xgbe_init_rx_coalesce\n");
 
 	pdata->rx_riwt = hw_if->usec_to_riwt(pdata, XGMAC_INIT_DMA_RX_USECS);
+	pdata->rx_usecs = XGMAC_INIT_DMA_RX_USECS;
 	pdata->rx_frames = XGMAC_INIT_DMA_RX_FRAMES;
 
 	hw_if->config_rx_coalesce(pdata);
@@ -941,8 +951,6 @@ static int xgbe_start(struct xgbe_prv_data *pdata)
 
 	DBGPR("-->xgbe_start\n");
 
-	xgbe_set_rx_mode(netdev);
-
 	hw_if->init(pdata);
 
 	phy_start(pdata->phydev);
@@ -1522,17 +1530,10 @@ static void xgbe_set_rx_mode(struct net_device *netdev)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
 	struct xgbe_hw_if *hw_if = &pdata->hw_if;
-	unsigned int pr_mode, am_mode;
 
 	DBGPR("-->xgbe_set_rx_mode\n");
 
-	pr_mode = ((netdev->flags & IFF_PROMISC) != 0);
-	am_mode = ((netdev->flags & IFF_ALLMULTI) != 0);
-
-	hw_if->set_promiscuous_mode(pdata, pr_mode);
-	hw_if->set_all_multicast_mode(pdata, am_mode);
-
-	hw_if->add_mac_addresses(pdata);
+	hw_if->config_rx_mode(pdata);
 
 	DBGPR("<--xgbe_set_rx_mode\n");
 }
@@ -1599,6 +1600,14 @@ static int xgbe_change_mtu(struct net_device *netdev, int mtu)
 	return 0;
 }
 
+static void xgbe_tx_timeout(struct net_device *netdev)
+{
+	struct xgbe_prv_data *pdata = netdev_priv(netdev);
+
+	netdev_warn(netdev, "tx timeout, device restarting\n");
+	schedule_work(&pdata->restart_work);
+}
+
 static struct rtnl_link_stats64 *xgbe_get_stats64(struct net_device *netdev,
 						  struct rtnl_link_stats64 *s)
 {
@@ -1763,6 +1772,7 @@ static const struct net_device_ops xgbe_netdev_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_do_ioctl		= xgbe_ioctl,
 	.ndo_change_mtu		= xgbe_change_mtu,
+	.ndo_tx_timeout		= xgbe_tx_timeout,
 	.ndo_get_stats64	= xgbe_get_stats64,
 	.ndo_vlan_rx_add_vid	= xgbe_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= xgbe_vlan_rx_kill_vid,
@@ -1795,11 +1805,14 @@ static void xgbe_rx_refresh(struct xgbe_channel *channel)
 		if (desc_if->map_rx_buffer(pdata, ring, rdata))
 			break;
 
-		hw_if->rx_desc_reset(rdata);
+		hw_if->rx_desc_reset(pdata, rdata, ring->dirty);
 
 		ring->dirty++;
 	}
 
+	/* Make sure everything is written before the register write */
+	wmb();
+
 	/* Update the Rx Tail Pointer Register with address of
 	 * the last cleaned entry */
 	rdata = XGBE_GET_DESC_DATA(ring, ring->dirty - 1);
@@ -1807,16 +1820,15 @@ static void xgbe_rx_refresh(struct xgbe_channel *channel)
 			  lower_32_bits(rdata->rdesc_dma));
 }
 
-static struct sk_buff *xgbe_create_skb(struct xgbe_prv_data *pdata,
+static struct sk_buff *xgbe_create_skb(struct napi_struct *napi,
 				       struct xgbe_ring_data *rdata,
 				       unsigned int *len)
 {
-	struct net_device *netdev = pdata->netdev;
 	struct sk_buff *skb;
 	u8 *packet;
 	unsigned int copy_len;
 
-	skb = netdev_alloc_skb_ip_align(netdev, rdata->rx.hdr.dma_len);
+	skb = napi_alloc_skb(napi, rdata->rx.hdr.dma_len);
 	if (!skb)
 		return NULL;
 
@@ -1863,7 +1875,7 @@ static int xgbe_tx_poll(struct xgbe_channel *channel)
 
 		/* Make sure descriptor fields are read after reading the OWN
 		 * bit */
-		rmb();
+		dma_rmb();
 
 #ifdef XGMAC_ENABLE_TX_DESC_DUMP
 		xgbe_dump_tx_desc(ring, ring->dirty, 1, 0);
@@ -1986,7 +1998,7 @@ read_again:
 							rdata->rx.hdr.dma_len,
 							DMA_FROM_DEVICE);
 
-				skb = xgbe_create_skb(pdata, rdata, &put_len);
+				skb = xgbe_create_skb(napi, rdata, &put_len);
 				if (!skb) {
 					error = 1;
 					goto skip_data;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
index ebf489351555..5f149e8ee20f 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
@@ -291,7 +291,6 @@ static int xgbe_get_settings(struct net_device *netdev,
 		return -ENODEV;
 
 	ret = phy_ethtool_gset(pdata->phydev, cmd);
-	cmd->transceiver = XCVR_EXTERNAL;
 
 	DBGPR("<--xgbe_get_settings\n");
 
@@ -378,18 +377,14 @@ static int xgbe_get_coalesce(struct net_device *netdev,
 			     struct ethtool_coalesce *ec)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
-	struct xgbe_hw_if *hw_if = &pdata->hw_if;
-	unsigned int riwt;
 
 	DBGPR("-->xgbe_get_coalesce\n");
 
 	memset(ec, 0, sizeof(struct ethtool_coalesce));
 
-	riwt = pdata->rx_riwt;
-	ec->rx_coalesce_usecs = hw_if->riwt_to_usec(pdata, riwt);
+	ec->rx_coalesce_usecs = pdata->rx_usecs;
 	ec->rx_max_coalesced_frames = pdata->rx_frames;
 
-	ec->tx_coalesce_usecs = pdata->tx_usecs;
 	ec->tx_max_coalesced_frames = pdata->tx_frames;
 
 	DBGPR("<--xgbe_get_coalesce\n");
@@ -403,13 +398,14 @@ static int xgbe_set_coalesce(struct net_device *netdev,
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
 	struct xgbe_hw_if *hw_if = &pdata->hw_if;
 	unsigned int rx_frames, rx_riwt, rx_usecs;
-	unsigned int tx_frames, tx_usecs;
+	unsigned int tx_frames;
 
 	DBGPR("-->xgbe_set_coalesce\n");
 
 	/* Check for not supported parameters  */
 	if ((ec->rx_coalesce_usecs_irq) ||
 	    (ec->rx_max_coalesced_frames_irq) ||
+	    (ec->tx_coalesce_usecs) ||
 	    (ec->tx_coalesce_usecs_irq) ||
 	    (ec->tx_max_coalesced_frames_irq) ||
 	    (ec->stats_block_coalesce_usecs) ||
@@ -428,28 +424,18 @@ static int xgbe_set_coalesce(struct net_device *netdev,
 	    (ec->rate_sample_interval))
 		return -EOPNOTSUPP;
 
-	/* Can only change rx-frames when interface is down (see
-	 * rx_descriptor_init in xgbe-dev.c)
-	 */
-	rx_frames = pdata->rx_frames;
-	if (rx_frames != ec->rx_max_coalesced_frames && netif_running(netdev)) {
-		netdev_alert(netdev,
-			     "interface must be down to change rx-frames\n");
-		return -EINVAL;
-	}
-
 	rx_riwt = hw_if->usec_to_riwt(pdata, ec->rx_coalesce_usecs);
+	rx_usecs = ec->rx_coalesce_usecs;
 	rx_frames = ec->rx_max_coalesced_frames;
 
 	/* Use smallest possible value if conversion resulted in zero */
-	if (ec->rx_coalesce_usecs && !rx_riwt)
+	if (rx_usecs && !rx_riwt)
 		rx_riwt = 1;
 
 	/* Check the bounds of values for Rx */
 	if (rx_riwt > XGMAC_MAX_DMA_RIWT) {
-		rx_usecs = hw_if->riwt_to_usec(pdata, XGMAC_MAX_DMA_RIWT);
 		netdev_alert(netdev, "rx-usec is limited to %d usecs\n",
-			     rx_usecs);
+			     hw_if->riwt_to_usec(pdata, XGMAC_MAX_DMA_RIWT));
 		return -EINVAL;
 	}
 	if (rx_frames > pdata->rx_desc_count) {
@@ -458,7 +444,6 @@ static int xgbe_set_coalesce(struct net_device *netdev,
 		return -EINVAL;
 	}
 
-	tx_usecs = ec->tx_coalesce_usecs;
 	tx_frames = ec->tx_max_coalesced_frames;
 
 	/* Check the bounds of values for Tx */
@@ -469,10 +454,10 @@ static int xgbe_set_coalesce(struct net_device *netdev,
 	}
 
 	pdata->rx_riwt = rx_riwt;
+	pdata->rx_usecs = rx_usecs;
 	pdata->rx_frames = rx_frames;
 	hw_if->config_rx_coalesce(pdata);
 
-	pdata->tx_usecs = tx_usecs;
 	pdata->tx_frames = tx_frames;
 	hw_if->config_tx_coalesce(pdata);
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-main.c b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
index 32dd65137051..714905384900 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-main.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
@@ -374,15 +374,6 @@ static int xgbe_probe(struct platform_device *pdev)
 		pdata->awcache = XGBE_DMA_SYS_AWCACHE;
 	}
 
-	/* Set the DMA mask */
-	if (!dev->dma_mask)
-		dev->dma_mask = &dev->coherent_dma_mask;
-	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(40));
-	if (ret) {
-		dev_err(dev, "dma_set_mask_and_coherent failed\n");
-		goto err_io;
-	}
-
 	/* Get the device interrupt */
 	ret = platform_get_irq(pdev, 0);
 	if (ret < 0) {
@@ -409,6 +400,16 @@ static int xgbe_probe(struct platform_device *pdev)
 	/* Set default configuration data */
 	xgbe_default_config(pdata);
 
+	/* Set the DMA mask */
+	if (!dev->dma_mask)
+		dev->dma_mask = &dev->coherent_dma_mask;
+	ret = dma_set_mask_and_coherent(dev,
+					DMA_BIT_MASK(pdata->hw_feat.dma_width));
+	if (ret) {
+		dev_err(dev, "dma_set_mask_and_coherent failed\n");
+		goto err_io;
+	}
+
 	/* Calculate the number of Tx and Rx rings to be created
 	 *  -Tx (DMA) Channels map 1-to-1 to Tx Queues so set
 	 *   the number of Tx queues to the number of Tx channels
@@ -490,6 +491,9 @@ static int xgbe_probe(struct platform_device *pdev)
 
 	netdev->priv_flags |= IFF_UNICAST_FLT;
 
+	/* Use default watchdog timeout */
+	netdev->watchdog_timeo = 0;
+
 	xgbe_init_rx_coalesce(pdata);
 	xgbe_init_tx_coalesce(pdata);
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
index f326178ef376..b03e4f58d02e 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
@@ -179,7 +179,7 @@ static int xgbe_adjtime(struct ptp_clock_info *info, s64 delta)
 	return 0;
 }
 
-static int xgbe_gettime(struct ptp_clock_info *info, struct timespec *ts)
+static int xgbe_gettime(struct ptp_clock_info *info, struct timespec64 *ts)
 {
 	struct xgbe_prv_data *pdata = container_of(info,
 						   struct xgbe_prv_data,
@@ -193,12 +193,13 @@ static int xgbe_gettime(struct ptp_clock_info *info, struct timespec *ts)
 
 	spin_unlock_irqrestore(&pdata->tstamp_lock, flags);
 
-	*ts = ns_to_timespec(nsec);
+	*ts = ns_to_timespec64(nsec);
 
 	return 0;
 }
 
-static int xgbe_settime(struct ptp_clock_info *info, const struct timespec *ts)
+static int xgbe_settime(struct ptp_clock_info *info,
+			const struct timespec64 *ts)
 {
 	struct xgbe_prv_data *pdata = container_of(info,
 						   struct xgbe_prv_data,
@@ -206,7 +207,7 @@ static int xgbe_settime(struct ptp_clock_info *info, const struct timespec *ts)
 	unsigned long flags;
 	u64 nsec;
 
-	nsec = timespec_to_ns(ts);
+	nsec = timespec64_to_ns(ts);
 
 	spin_lock_irqsave(&pdata->tstamp_lock, flags);
 
@@ -236,8 +237,8 @@ void xgbe_ptp_register(struct xgbe_prv_data *pdata)
 	info->max_adj = pdata->ptpclk_rate;
 	info->adjfreq = xgbe_adjfreq;
 	info->adjtime = xgbe_adjtime;
-	info->gettime = xgbe_gettime;
-	info->settime = xgbe_settime;
+	info->gettime64 = xgbe_gettime;
+	info->settime64 = xgbe_settime;
 	info->enable = xgbe_enable;
 
 	clock = ptp_clock_register(info, pdata->dev);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe.h b/drivers/net/ethernet/amd/xgbe/xgbe.h
index 13e8f95c077c..e62dfa2deab6 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe.h
@@ -222,7 +222,7 @@
 	 ((_idx) & ((_ring)->rdesc_count - 1)))
 
 /* Default coalescing parameters */
-#define XGMAC_INIT_DMA_TX_USECS		50
+#define XGMAC_INIT_DMA_TX_USECS		1000
 #define XGMAC_INIT_DMA_TX_FRAMES	25
 
 #define XGMAC_MAX_DMA_RIWT		0xff
@@ -325,8 +325,6 @@ struct xgbe_ring_data {
 	struct xgbe_tx_ring_data tx;	/* Tx-related data */
 	struct xgbe_rx_ring_data rx;	/* Rx-related data */
 
-	unsigned int interrupt;		/* Interrupt indicator */
-
 	unsigned int mapped_as_page;
 
 	/* Incomplete receive save location.  If the budget is exhausted
@@ -410,7 +408,7 @@ struct xgbe_channel {
 	unsigned int saved_ier;
 
 	unsigned int tx_timer_active;
-	struct hrtimer tx_timer;
+	struct timer_list tx_timer;
 
 	struct xgbe_ring *tx_ring;
 	struct xgbe_ring *rx_ring;
@@ -497,10 +495,8 @@ struct xgbe_mmc_stats {
 struct xgbe_hw_if {
 	int (*tx_complete)(struct xgbe_ring_desc *);
 
-	int (*set_promiscuous_mode)(struct xgbe_prv_data *, unsigned int);
-	int (*set_all_multicast_mode)(struct xgbe_prv_data *, unsigned int);
-	int (*add_mac_addresses)(struct xgbe_prv_data *);
 	int (*set_mac_address)(struct xgbe_prv_data *, u8 *addr);
+	int (*config_rx_mode)(struct xgbe_prv_data *);
 
 	int (*enable_rx_csum)(struct xgbe_prv_data *);
 	int (*disable_rx_csum)(struct xgbe_prv_data *);
@@ -536,8 +532,9 @@ struct xgbe_hw_if {
 	int (*dev_read)(struct xgbe_channel *);
 	void (*tx_desc_init)(struct xgbe_channel *);
 	void (*rx_desc_init)(struct xgbe_channel *);
-	void (*rx_desc_reset)(struct xgbe_ring_data *);
 	void (*tx_desc_reset)(struct xgbe_ring_data *);
+	void (*rx_desc_reset)(struct xgbe_prv_data *, struct xgbe_ring_data *,
+			      unsigned int);
 	int (*is_last_desc)(struct xgbe_ring_desc *);
 	int (*is_context_desc)(struct xgbe_ring_desc *);
 	void (*tx_start_xmit)(struct xgbe_channel *, struct xgbe_ring *);
@@ -620,7 +617,7 @@ struct xgbe_hw_features {
 	unsigned int mgk;		/* PMT magic packet */
 	unsigned int mmc;		/* RMON module */
 	unsigned int aoe;		/* ARP Offload */
-	unsigned int ts;		/* IEEE 1588-2008 Adavanced Timestamp */
+	unsigned int ts;		/* IEEE 1588-2008 Advanced Timestamp */
 	unsigned int eee;		/* Energy Efficient Ethernet */
 	unsigned int tx_coe;		/* Tx Checksum Offload */
 	unsigned int rx_coe;		/* Rx Checksum Offload */
@@ -632,6 +629,7 @@ struct xgbe_hw_features {
 	unsigned int rx_fifo_size;	/* MTL Receive FIFO Size */
 	unsigned int tx_fifo_size;	/* MTL Transmit FIFO Size */
 	unsigned int adv_ts_hi;		/* Advance Timestamping High Word */
+	unsigned int dma_width;		/* DMA width */
 	unsigned int dcb;		/* DCB Feature */
 	unsigned int sph;		/* Split Header Feature */
 	unsigned int tso;		/* TCP Segmentation Offload */
@@ -715,6 +713,7 @@ struct xgbe_prv_data {
 
 	/* Rx coalescing settings */
 	unsigned int rx_riwt;
+	unsigned int rx_usecs;
 	unsigned int rx_frames;
 
 	/* Current Rx buffer size */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
index ec45f3256f0e..d9bc89d69266 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
@@ -97,6 +97,8 @@ enum xgene_enet_rm {
 #define QCOHERENT		BIT(4)
 #define RECOMBBUF		BIT(27)
 
+#define MAC_OFFSET			0x30
+
 #define BLOCK_ETH_CSR_OFFSET		0x2000
 #define BLOCK_ETH_RING_IF_OFFSET	0x9000
 #define BLOCK_ETH_DIAG_CSR_OFFSET	0xD000
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
index 635a83be7e5e..40d3530d7f30 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
@@ -428,13 +428,23 @@ static int xgene_enet_register_irq(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
 	struct device *dev = ndev_to_dev(ndev);
+	struct xgene_enet_desc_ring *ring;
 	int ret;
 
-	ret = devm_request_irq(dev, pdata->rx_ring->irq, xgene_enet_rx_irq,
-			       IRQF_SHARED, ndev->name, pdata->rx_ring);
-	if (ret) {
-		netdev_err(ndev, "rx%d interrupt request failed\n",
-			   pdata->rx_ring->irq);
+	ring = pdata->rx_ring;
+	ret = devm_request_irq(dev, ring->irq, xgene_enet_rx_irq,
+			       IRQF_SHARED, ring->irq_name, ring);
+	if (ret)
+		netdev_err(ndev, "Failed to request irq %s\n", ring->irq_name);
+
+	if (pdata->cq_cnt) {
+		ring = pdata->tx_ring->cp_ring;
+		ret = devm_request_irq(dev, ring->irq, xgene_enet_rx_irq,
+				       IRQF_SHARED, ring->irq_name, ring);
+		if (ret) {
+			netdev_err(ndev, "Failed to request irq %s\n",
+				   ring->irq_name);
+		}
 	}
 
 	return ret;
@@ -448,6 +458,37 @@ static void xgene_enet_free_irq(struct net_device *ndev)
 	pdata = netdev_priv(ndev);
 	dev = ndev_to_dev(ndev);
 	devm_free_irq(dev, pdata->rx_ring->irq, pdata->rx_ring);
+
+	if (pdata->cq_cnt) {
+		devm_free_irq(dev, pdata->tx_ring->cp_ring->irq,
+			      pdata->tx_ring->cp_ring);
+	}
+}
+
+static void xgene_enet_napi_enable(struct xgene_enet_pdata *pdata)
+{
+	struct napi_struct *napi;
+
+	napi = &pdata->rx_ring->napi;
+	napi_enable(napi);
+
+	if (pdata->cq_cnt) {
+		napi = &pdata->tx_ring->cp_ring->napi;
+		napi_enable(napi);
+	}
+}
+
+static void xgene_enet_napi_disable(struct xgene_enet_pdata *pdata)
+{
+	struct napi_struct *napi;
+
+	napi = &pdata->rx_ring->napi;
+	napi_disable(napi);
+
+	if (pdata->cq_cnt) {
+		napi = &pdata->tx_ring->cp_ring->napi;
+		napi_disable(napi);
+	}
 }
 
 static int xgene_enet_open(struct net_device *ndev)
@@ -462,7 +503,7 @@ static int xgene_enet_open(struct net_device *ndev)
 	ret = xgene_enet_register_irq(ndev);
 	if (ret)
 		return ret;
-	napi_enable(&pdata->rx_ring->napi);
+	xgene_enet_napi_enable(pdata);
 
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
 		phy_start(pdata->phy_dev);
@@ -486,7 +527,7 @@ static int xgene_enet_close(struct net_device *ndev)
 	else
 		cancel_delayed_work_sync(&pdata->link_work);
 
-	napi_disable(&pdata->rx_ring->napi);
+	xgene_enet_napi_disable(pdata);
 	xgene_enet_free_irq(ndev);
 	xgene_enet_process_ring(pdata->rx_ring, -1);
 
@@ -580,6 +621,8 @@ static void xgene_enet_free_desc_rings(struct xgene_enet_pdata *pdata)
 	if (ring) {
 		if (ring->cp_ring && ring->cp_ring->cp_skb)
 			devm_kfree(dev, ring->cp_ring->cp_skb);
+		if (ring->cp_ring && pdata->cq_cnt)
+			xgene_enet_free_desc_ring(ring->cp_ring);
 		xgene_enet_free_desc_ring(ring);
 	}
 
@@ -645,9 +688,11 @@ static int xgene_enet_create_desc_rings(struct net_device *ndev)
 	struct device *dev = ndev_to_dev(ndev);
 	struct xgene_enet_desc_ring *rx_ring, *tx_ring, *cp_ring;
 	struct xgene_enet_desc_ring *buf_pool = NULL;
-	u8 cpu_bufnum = 0, eth_bufnum = START_ETH_BUFNUM;
-	u8 bp_bufnum = START_BP_BUFNUM;
-	u16 ring_id, ring_num = START_RING_NUM;
+	u8 cpu_bufnum = pdata->cpu_bufnum;
+	u8 eth_bufnum = pdata->eth_bufnum;
+	u8 bp_bufnum = pdata->bp_bufnum;
+	u16 ring_num = pdata->ring_num;
+	u16 ring_id;
 	int ret;
 
 	/* allocate rx descriptor ring */
@@ -671,6 +716,12 @@ static int xgene_enet_create_desc_rings(struct net_device *ndev)
 	rx_ring->nbufpool = NUM_BUFPOOL;
 	rx_ring->buf_pool = buf_pool;
 	rx_ring->irq = pdata->rx_irq;
+	if (!pdata->cq_cnt) {
+		snprintf(rx_ring->irq_name, IRQ_ID_SIZE, "%s-rx-txc",
+			 ndev->name);
+	} else {
+		snprintf(rx_ring->irq_name, IRQ_ID_SIZE, "%s-rx", ndev->name);
+	}
 	buf_pool->rx_skb = devm_kcalloc(dev, buf_pool->slots,
 					sizeof(struct sk_buff *), GFP_KERNEL);
 	if (!buf_pool->rx_skb) {
@@ -692,7 +743,22 @@ static int xgene_enet_create_desc_rings(struct net_device *ndev)
 	}
 	pdata->tx_ring = tx_ring;
 
-	cp_ring = pdata->rx_ring;
+	if (!pdata->cq_cnt) {
+		cp_ring = pdata->rx_ring;
+	} else {
+		/* allocate tx completion descriptor ring */
+		ring_id = xgene_enet_get_ring_id(RING_OWNER_CPU, cpu_bufnum++);
+		cp_ring = xgene_enet_create_desc_ring(ndev, ring_num++,
+						      RING_CFGSIZE_16KB,
+						      ring_id);
+		if (!cp_ring) {
+			ret = -ENOMEM;
+			goto err;
+		}
+		cp_ring->irq = pdata->txc_irq;
+		snprintf(cp_ring->irq_name, IRQ_ID_SIZE, "%s-txc", ndev->name);
+	}
+
 	cp_ring->cp_skb = devm_kcalloc(dev, tx_ring->slots,
 				       sizeof(struct sk_buff *), GFP_KERNEL);
 	if (!cp_ring->cp_skb) {
@@ -752,6 +818,22 @@ static const struct net_device_ops xgene_ndev_ops = {
 	.ndo_set_mac_address = xgene_enet_set_mac_address,
 };
 
+static int xgene_get_port_id(struct device *dev, struct xgene_enet_pdata *pdata)
+{
+	u32 id = 0;
+	int ret;
+
+	ret = device_property_read_u32(dev, "port-id", &id);
+	if (!ret && id > 1) {
+		dev_err(dev, "Incorrect port-id specified\n");
+		return -ENODEV;
+	}
+
+	pdata->port_id = id;
+
+	return 0;
+}
+
 static int xgene_get_mac_address(struct device *dev,
 				 unsigned char *addr)
 {
@@ -835,13 +917,9 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 		return -ENOMEM;
 	}
 
-	ret = platform_get_irq(pdev, 0);
-	if (ret <= 0) {
-		dev_err(dev, "Unable to get ENET Rx IRQ\n");
-		ret = ret ? : -ENXIO;
+	ret = xgene_get_port_id(dev, pdata);
+	if (ret)
 		return ret;
-	}
-	pdata->rx_irq = ret;
 
 	if (xgene_get_mac_address(dev, ndev->dev_addr) != ETH_ALEN)
 		eth_hw_addr_random(ndev);
@@ -860,19 +938,37 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 		return -ENODEV;
 	}
 
+	ret = platform_get_irq(pdev, 0);
+	if (ret <= 0) {
+		dev_err(dev, "Unable to get ENET Rx IRQ\n");
+		ret = ret ? : -ENXIO;
+		return ret;
+	}
+	pdata->rx_irq = ret;
+
+	if (pdata->phy_mode != PHY_INTERFACE_MODE_RGMII) {
+		ret = platform_get_irq(pdev, 1);
+		if (ret <= 0) {
+			dev_err(dev, "Unable to get ENET Tx completion IRQ\n");
+			ret = ret ? : -ENXIO;
+			return ret;
+		}
+		pdata->txc_irq = ret;
+	}
+
 	pdata->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(pdata->clk)) {
 		/* Firmware may have set up the clock already. */
 		pdata->clk = NULL;
 	}
 
-	base_addr = pdata->base_addr;
+	base_addr = pdata->base_addr - (pdata->port_id * MAC_OFFSET);
 	pdata->eth_csr_addr = base_addr + BLOCK_ETH_CSR_OFFSET;
 	pdata->eth_ring_if_addr = base_addr + BLOCK_ETH_RING_IF_OFFSET;
 	pdata->eth_diag_csr_addr = base_addr + BLOCK_ETH_DIAG_CSR_OFFSET;
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII ||
 	    pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
-		pdata->mcx_mac_addr = base_addr + BLOCK_ETH_MAC_OFFSET;
+		pdata->mcx_mac_addr = pdata->base_addr + BLOCK_ETH_MAC_OFFSET;
 		pdata->mcx_mac_csr_addr = base_addr + BLOCK_ETH_MAC_CSR_OFFSET;
 	} else {
 		pdata->mcx_mac_addr = base_addr + BLOCK_AXG_MAC_OFFSET;
@@ -928,13 +1024,60 @@ static void xgene_enet_setup_ops(struct xgene_enet_pdata *pdata)
 		pdata->mac_ops = &xgene_sgmac_ops;
 		pdata->port_ops = &xgene_sgport_ops;
 		pdata->rm = RM1;
+		pdata->cq_cnt = XGENE_MAX_TXC_RINGS;
 		break;
 	default:
 		pdata->mac_ops = &xgene_xgmac_ops;
 		pdata->port_ops = &xgene_xgport_ops;
 		pdata->rm = RM0;
+		pdata->cq_cnt = XGENE_MAX_TXC_RINGS;
 		break;
 	}
+
+	switch (pdata->port_id) {
+	case 0:
+		pdata->cpu_bufnum = START_CPU_BUFNUM_0;
+		pdata->eth_bufnum = START_ETH_BUFNUM_0;
+		pdata->bp_bufnum = START_BP_BUFNUM_0;
+		pdata->ring_num = START_RING_NUM_0;
+		break;
+	case 1:
+		pdata->cpu_bufnum = START_CPU_BUFNUM_1;
+		pdata->eth_bufnum = START_ETH_BUFNUM_1;
+		pdata->bp_bufnum = START_BP_BUFNUM_1;
+		pdata->ring_num = START_RING_NUM_1;
+		break;
+	default:
+		break;
+	}
+
+}
+
+static void xgene_enet_napi_add(struct xgene_enet_pdata *pdata)
+{
+	struct napi_struct *napi;
+
+	napi = &pdata->rx_ring->napi;
+	netif_napi_add(pdata->ndev, napi, xgene_enet_napi, NAPI_POLL_WEIGHT);
+
+	if (pdata->cq_cnt) {
+		napi = &pdata->tx_ring->cp_ring->napi;
+		netif_napi_add(pdata->ndev, napi, xgene_enet_napi,
+			       NAPI_POLL_WEIGHT);
+	}
+}
+
+static void xgene_enet_napi_del(struct xgene_enet_pdata *pdata)
+{
+	struct napi_struct *napi;
+
+	napi = &pdata->rx_ring->napi;
+	netif_napi_del(napi);
+
+	if (pdata->cq_cnt) {
+		napi = &pdata->tx_ring->cp_ring->napi;
+		netif_napi_del(napi);
+	}
 }
 
 static int xgene_enet_probe(struct platform_device *pdev)
@@ -942,7 +1085,6 @@ static int xgene_enet_probe(struct platform_device *pdev)
 	struct net_device *ndev;
 	struct xgene_enet_pdata *pdata;
 	struct device *dev = &pdev->dev;
-	struct napi_struct *napi;
 	struct xgene_mac_ops *mac_ops;
 	int ret;
 
@@ -984,8 +1126,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 	if (ret)
 		goto err;
 
-	napi = &pdata->rx_ring->napi;
-	netif_napi_add(ndev, napi, xgene_enet_napi, NAPI_POLL_WEIGHT);
+	xgene_enet_napi_add(pdata);
 	mac_ops = pdata->mac_ops;
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
 		ret = xgene_enet_mdio_config(pdata);
@@ -1012,7 +1153,7 @@ static int xgene_enet_remove(struct platform_device *pdev)
 	mac_ops->rx_disable(pdata);
 	mac_ops->tx_disable(pdata);
 
-	netif_napi_del(&pdata->rx_ring->napi);
+	xgene_enet_napi_del(pdata);
 	xgene_enet_mdio_remove(pdata);
 	xgene_enet_delete_desc_rings(pdata);
 	unregister_netdev(ndev);
@@ -1033,7 +1174,7 @@ MODULE_DEVICE_TABLE(acpi, xgene_enet_acpi_match);
 #endif
 
 #ifdef CONFIG_OF
-static struct of_device_id xgene_enet_of_match[] = {
+static const 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/apm/xgene/xgene_enet_main.h b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
index c2d465c3db66..8f3d232b09bc 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
@@ -41,9 +41,18 @@
 #define SKB_BUFFER_SIZE		(XGENE_ENET_MAX_MTU - NET_IP_ALIGN)
 #define NUM_PKT_BUF	64
 #define NUM_BUFPOOL	32
-#define START_ETH_BUFNUM	2
-#define START_BP_BUFNUM		0x22
-#define START_RING_NUM		8
+
+#define START_CPU_BUFNUM_0	0
+#define START_ETH_BUFNUM_0	2
+#define START_BP_BUFNUM_0	0x22
+#define START_RING_NUM_0	8
+#define START_CPU_BUFNUM_1	12
+#define START_ETH_BUFNUM_1	10
+#define START_BP_BUFNUM_1	0x2A
+#define START_RING_NUM_1	264
+
+#define IRQ_ID_SIZE		16
+#define XGENE_MAX_TXC_RINGS	1
 
 #define PHY_POLL_LINK_ON	(10 * HZ)
 #define PHY_POLL_LINK_OFF	(PHY_POLL_LINK_ON / 5)
@@ -57,6 +66,7 @@ struct xgene_enet_desc_ring {
 	u16 tail;
 	u16 slots;
 	u16 irq;
+	char irq_name[IRQ_ID_SIZE];
 	u32 size;
 	u32 state[NUM_RING_CONFIG];
 	void __iomem *cmd_base;
@@ -111,6 +121,8 @@ struct xgene_enet_pdata {
 	u32 cp_qcnt_hi;
 	u32 cp_qcnt_low;
 	u32 rx_irq;
+	u32 txc_irq;
+	u8 cq_cnt;
 	void __iomem *eth_csr_addr;
 	void __iomem *eth_ring_if_addr;
 	void __iomem *eth_diag_csr_addr;
@@ -125,6 +137,11 @@ struct xgene_enet_pdata {
 	struct xgene_mac_ops *mac_ops;
 	struct xgene_port_ops *port_ops;
 	struct delayed_work link_work;
+	u32 port_id;
+	u8 cpu_bufnum;
+	u8 eth_bufnum;
+	u8 bp_bufnum;
+	u16 ring_num;
 };
 
 struct xgene_indirect_ctl {
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
index f5d4f68c288c..f27fb6f2a93b 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
@@ -226,6 +226,7 @@ static u32 xgene_enet_link_status(struct xgene_enet_pdata *p)
 static void xgene_sgmac_init(struct xgene_enet_pdata *p)
 {
 	u32 data, loop = 10;
+	u32 offset = p->port_id * 4;
 
 	xgene_sgmac_reset(p);
 
@@ -272,9 +273,9 @@ static void xgene_sgmac_init(struct xgene_enet_pdata *p)
 	xgene_enet_wr_csr(p, RSIF_RAM_DBG_REG0_ADDR, 0);
 
 	/* Bypass traffic gating */
-	xgene_enet_wr_csr(p, CFG_LINK_AGGR_RESUME_0_ADDR, TX_PORT0);
+	xgene_enet_wr_csr(p, CFG_LINK_AGGR_RESUME_0_ADDR + offset, TX_PORT0);
 	xgene_enet_wr_csr(p, CFG_BYPASS_ADDR, RESUME_TX);
-	xgene_enet_wr_csr(p, SG_RX_DV_GATE_REG_0_ADDR, RESUME_RX0);
+	xgene_enet_wr_csr(p, SG_RX_DV_GATE_REG_0_ADDR + offset, RESUME_RX0);
 }
 
 static void xgene_sgmac_rxtx(struct xgene_enet_pdata *p, u32 bits, bool set)
@@ -330,13 +331,14 @@ static void xgene_enet_cle_bypass(struct xgene_enet_pdata *p,
 				  u32 dst_ring_num, u16 bufpool_id)
 {
 	u32 data, fpsel;
+	u32 offset = p->port_id * MAC_OFFSET;
 
 	data = CFG_CLE_BYPASS_EN0;
-	xgene_enet_wr_csr(p, CLE_BYPASS_REG0_0_ADDR, data);
+	xgene_enet_wr_csr(p, CLE_BYPASS_REG0_0_ADDR + offset, data);
 
 	fpsel = xgene_enet_ring_bufnum(bufpool_id) - 0x20;
 	data = CFG_CLE_DSTQID0(dst_ring_num) | CFG_CLE_FPSEL0(fpsel);
-	xgene_enet_wr_csr(p, CLE_BYPASS_REG1_0_ADDR, data);
+	xgene_enet_wr_csr(p, CLE_BYPASS_REG1_0_ADDR + offset, data);
 }
 
 static void xgene_enet_shutdown(struct xgene_enet_pdata *p)
diff --git a/drivers/net/ethernet/apple/bmac.c b/drivers/net/ethernet/apple/bmac.c
index c0bd638f84af..a65d7a60f116 100644
--- a/drivers/net/ethernet/apple/bmac.c
+++ b/drivers/net/ethernet/apple/bmac.c
@@ -1621,7 +1621,7 @@ static int bmac_remove(struct macio_dev *mdev)
 	return 0;
 }
 
-static struct of_device_id bmac_match[] =
+static const struct of_device_id bmac_match[] =
 {
 	{
 	.name 		= "bmac",
diff --git a/drivers/net/ethernet/apple/mace.c b/drivers/net/ethernet/apple/mace.c
index 73afe49624f2..e58a7c73766e 100644
--- a/drivers/net/ethernet/apple/mace.c
+++ b/drivers/net/ethernet/apple/mace.c
@@ -720,7 +720,7 @@ static irqreturn_t mace_interrupt(int irq, void *dev_id)
 	    mace_reset(dev);
 		/*
 		 * XXX mace likes to hang the machine after a xmtfs error.
-		 * This is hard to reproduce, reseting *may* help
+		 * This is hard to reproduce, resetting *may* help
 		 */
 	}
 	cp = mp->tx_cmds + NCMDS_TX * i;
@@ -984,7 +984,7 @@ static irqreturn_t mace_rxdma_intr(int irq, void *dev_id)
     return IRQ_HANDLED;
 }
 
-static struct of_device_id mace_match[] =
+static const struct of_device_id mace_match[] =
 {
 	{
 	.name 		= "mace",
diff --git a/drivers/net/ethernet/apple/macmace.c b/drivers/net/ethernet/apple/macmace.c
index 6e66127e6abf..89914ca17a49 100644
--- a/drivers/net/ethernet/apple/macmace.c
+++ b/drivers/net/ethernet/apple/macmace.c
@@ -575,7 +575,7 @@ static irqreturn_t mace_interrupt(int irq, void *dev_id)
 			mace_reset(dev);
 			/*
 			 * XXX mace likes to hang the machine after a xmtfs error.
-			 * This is hard to reproduce, reseting *may* help
+			 * This is hard to reproduce, resetting *may* help
 			 */
 		}
 		/* dma should have finished */
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
index 52fdfe225978..a8b80c56ac25 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
@@ -307,7 +307,7 @@ void atl1c_start_phy_polling(struct atl1c_hw *hw, u16 clk_sel)
 
 /*
  * atl1c_read_phy_core
- * core funtion to read register in PHY via MDIO control regsiter.
+ * core function to read register in PHY via MDIO control regsiter.
  * ext: extension register (see IEEE 802.3)
  * dev: device address (see IEEE 802.3 DEVAD, PRTAD is fixed to 0)
  * reg: reg to read
@@ -356,7 +356,7 @@ int atl1c_read_phy_core(struct atl1c_hw *hw, bool ext, u8 dev,
 
 /*
  * atl1c_write_phy_core
- * core funtion to write to register in PHY via MDIO control regsiter.
+ * core function to write to register in PHY via MDIO control register.
  * ext: extension register (see IEEE 802.3)
  * dev: device address (see IEEE 802.3 DEVAD, PRTAD is fixed to 0)
  * reg: reg to write
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
index 587f63e87588..932bd1862f7a 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
@@ -752,7 +752,7 @@ static void atl1c_patch_assign(struct atl1c_hw *hw)
 
 	if (hw->device_id == PCI_DEVICE_ID_ATHEROS_L2C_B2 &&
 	    hw->revision_id == L2CB_V21) {
-		/* config acess mode */
+		/* config access mode */
 		pci_write_config_dword(pdev, REG_PCIE_IND_ACC_ADDR,
 				       REG_PCIE_DEV_MISC_CTRL);
 		pci_read_config_dword(pdev, REG_PCIE_IND_ACC_DATA, &misc_ctrl);
diff --git a/drivers/net/ethernet/broadcom/Kconfig b/drivers/net/ethernet/broadcom/Kconfig
index 41a3c9804427..a6f9142b9048 100644
--- a/drivers/net/ethernet/broadcom/Kconfig
+++ b/drivers/net/ethernet/broadcom/Kconfig
@@ -71,12 +71,12 @@ config BCMGENET
 	  Broadcom BCM7xxx Set Top Box family chipset.
 
 config BNX2
-	tristate "QLogic NetXtremeII support"
+	tristate "QLogic bnx2 support"
 	depends on PCI
 	select CRC32
 	select FW_LOADER
 	---help---
-	  This driver supports QLogic NetXtremeII gigabit Ethernet cards.
+	  This driver supports QLogic bnx2 gigabit Ethernet cards.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called bnx2.  This is recommended.
@@ -87,8 +87,8 @@ config CNIC
 	select BNX2
 	select UIO
 	---help---
-	  This driver supports offload features of QLogic NetXtremeII
-	  gigabit Ethernet cards.
+	  This driver supports offload features of QLogic bnx2 gigabit
+	  Ethernet cards.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called cnic.  This is recommended.
@@ -142,7 +142,7 @@ config BNX2X_SRIOV
 
 config BGMAC
 	tristate "BCMA bus GBit core support"
-	depends on BCMA_HOST_SOC && HAS_DMA && BCM47XX
+	depends on BCMA_HOST_SOC && HAS_DMA && (BCM47XX || ARCH_BCM_5301X)
 	select PHYLIB
 	---help---
 	  This driver supports GBit MAC and BCM4706 GBit MAC cores on BCMA bus.
diff --git a/drivers/net/ethernet/broadcom/bgmac.c b/drivers/net/ethernet/broadcom/bgmac.c
index 0469f72c6e7e..5cb93d1f50a4 100644
--- a/drivers/net/ethernet/broadcom/bgmac.c
+++ b/drivers/net/ethernet/broadcom/bgmac.c
@@ -14,6 +14,7 @@
 #include <linux/etherdevice.h>
 #include <linux/mii.h>
 #include <linux/phy.h>
+#include <linux/phy_fixed.h>
 #include <linux/interrupt.h>
 #include <linux/dma-mapping.h>
 #include <bcm47xx_nvram.h>
@@ -114,54 +115,89 @@ static void bgmac_dma_tx_enable(struct bgmac *bgmac,
 	bgmac_write(bgmac, ring->mmio_base + BGMAC_DMA_TX_CTL, ctl);
 }
 
+static void
+bgmac_dma_tx_add_buf(struct bgmac *bgmac, struct bgmac_dma_ring *ring,
+		     int i, int len, u32 ctl0)
+{
+	struct bgmac_slot_info *slot;
+	struct bgmac_dma_desc *dma_desc;
+	u32 ctl1;
+
+	if (i == BGMAC_TX_RING_SLOTS - 1)
+		ctl0 |= BGMAC_DESC_CTL0_EOT;
+
+	ctl1 = len & BGMAC_DESC_CTL1_LEN;
+
+	slot = &ring->slots[i];
+	dma_desc = &ring->cpu_base[i];
+	dma_desc->addr_low = cpu_to_le32(lower_32_bits(slot->dma_addr));
+	dma_desc->addr_high = cpu_to_le32(upper_32_bits(slot->dma_addr));
+	dma_desc->ctl0 = cpu_to_le32(ctl0);
+	dma_desc->ctl1 = cpu_to_le32(ctl1);
+}
+
 static netdev_tx_t bgmac_dma_tx_add(struct bgmac *bgmac,
 				    struct bgmac_dma_ring *ring,
 				    struct sk_buff *skb)
 {
 	struct device *dma_dev = bgmac->core->dma_dev;
 	struct net_device *net_dev = bgmac->net_dev;
-	struct bgmac_dma_desc *dma_desc;
-	struct bgmac_slot_info *slot;
-	u32 ctl0, ctl1;
-	int free_slots;
+	int index = ring->end % BGMAC_TX_RING_SLOTS;
+	struct bgmac_slot_info *slot = &ring->slots[index];
+	int nr_frags;
+	u32 flags;
+	int i;
 
 	if (skb->len > BGMAC_DESC_CTL1_LEN) {
 		bgmac_err(bgmac, "Too long skb (%d)\n", skb->len);
-		goto err_stop_drop;
+		goto err_drop;
 	}
 
-	if (ring->start <= ring->end)
-		free_slots = ring->start - ring->end + BGMAC_TX_RING_SLOTS;
-	else
-		free_slots = ring->start - ring->end;
-	if (free_slots == 1) {
+	if (skb->ip_summed == CHECKSUM_PARTIAL)
+		skb_checksum_help(skb);
+
+	nr_frags = skb_shinfo(skb)->nr_frags;
+
+	/* ring->end - ring->start will return the number of valid slots,
+	 * even when ring->end overflows
+	 */
+	if (ring->end - ring->start + nr_frags + 1 >= BGMAC_TX_RING_SLOTS) {
 		bgmac_err(bgmac, "TX ring is full, queue should be stopped!\n");
 		netif_stop_queue(net_dev);
 		return NETDEV_TX_BUSY;
 	}
 
-	slot = &ring->slots[ring->end];
-	slot->skb = skb;
-	slot->dma_addr = dma_map_single(dma_dev, skb->data, skb->len,
+	slot->dma_addr = dma_map_single(dma_dev, skb->data, skb_headlen(skb),
 					DMA_TO_DEVICE);
-	if (dma_mapping_error(dma_dev, slot->dma_addr)) {
-		bgmac_err(bgmac, "Mapping error of skb on ring 0x%X\n",
-			  ring->mmio_base);
-		goto err_stop_drop;
-	}
+	if (unlikely(dma_mapping_error(dma_dev, slot->dma_addr)))
+		goto err_dma_head;
 
-	ctl0 = BGMAC_DESC_CTL0_IOC | BGMAC_DESC_CTL0_SOF | BGMAC_DESC_CTL0_EOF;
-	if (ring->end == ring->num_slots - 1)
-		ctl0 |= BGMAC_DESC_CTL0_EOT;
-	ctl1 = skb->len & BGMAC_DESC_CTL1_LEN;
+	flags = BGMAC_DESC_CTL0_SOF;
+	if (!nr_frags)
+		flags |= BGMAC_DESC_CTL0_EOF | BGMAC_DESC_CTL0_IOC;
 
-	dma_desc = ring->cpu_base;
-	dma_desc += ring->end;
-	dma_desc->addr_low = cpu_to_le32(lower_32_bits(slot->dma_addr));
-	dma_desc->addr_high = cpu_to_le32(upper_32_bits(slot->dma_addr));
-	dma_desc->ctl0 = cpu_to_le32(ctl0);
-	dma_desc->ctl1 = cpu_to_le32(ctl1);
+	bgmac_dma_tx_add_buf(bgmac, ring, index, skb_headlen(skb), flags);
+	flags = 0;
+
+	for (i = 0; i < nr_frags; i++) {
+		struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i];
+		int len = skb_frag_size(frag);
+
+		index = (index + 1) % BGMAC_TX_RING_SLOTS;
+		slot = &ring->slots[index];
+		slot->dma_addr = skb_frag_dma_map(dma_dev, frag, 0,
+						  len, DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(dma_dev, slot->dma_addr)))
+			goto err_dma;
+
+		if (i == nr_frags - 1)
+			flags |= BGMAC_DESC_CTL0_EOF | BGMAC_DESC_CTL0_IOC;
+
+		bgmac_dma_tx_add_buf(bgmac, ring, index, len, flags);
+	}
 
+	slot->skb = skb;
+	ring->end += nr_frags + 1;
 	netdev_sent_queue(net_dev, skb->len);
 
 	wmb();
@@ -169,20 +205,34 @@ static netdev_tx_t bgmac_dma_tx_add(struct bgmac *bgmac,
 	/* Increase ring->end to point empty slot. We tell hardware the first
 	 * slot it should *not* read.
 	 */
-	if (++ring->end >= BGMAC_TX_RING_SLOTS)
-		ring->end = 0;
 	bgmac_write(bgmac, ring->mmio_base + BGMAC_DMA_TX_INDEX,
 		    ring->index_base +
-		    ring->end * sizeof(struct bgmac_dma_desc));
+		    (ring->end % BGMAC_TX_RING_SLOTS) *
+		    sizeof(struct bgmac_dma_desc));
 
-	/* Always keep one slot free to allow detecting bugged calls. */
-	if (--free_slots == 1)
+	if (ring->end - ring->start >= BGMAC_TX_RING_SLOTS - 8)
 		netif_stop_queue(net_dev);
 
 	return NETDEV_TX_OK;
 
-err_stop_drop:
-	netif_stop_queue(net_dev);
+err_dma:
+	dma_unmap_single(dma_dev, slot->dma_addr, skb_headlen(skb),
+			 DMA_TO_DEVICE);
+
+	while (i > 0) {
+		int index = (ring->end + i) % BGMAC_TX_RING_SLOTS;
+		struct bgmac_slot_info *slot = &ring->slots[index];
+		u32 ctl1 = le32_to_cpu(ring->cpu_base[index].ctl1);
+		int len = ctl1 & BGMAC_DESC_CTL1_LEN;
+
+		dma_unmap_page(dma_dev, slot->dma_addr, len, DMA_TO_DEVICE);
+	}
+
+err_dma_head:
+	bgmac_err(bgmac, "Mapping error of skb on ring 0x%X\n",
+		  ring->mmio_base);
+
+err_drop:
 	dev_kfree_skb(skb);
 	return NETDEV_TX_OK;
 }
@@ -202,34 +252,45 @@ static void bgmac_dma_tx_free(struct bgmac *bgmac, struct bgmac_dma_ring *ring)
 	empty_slot &= BGMAC_DMA_TX_STATDPTR;
 	empty_slot /= sizeof(struct bgmac_dma_desc);
 
-	while (ring->start != empty_slot) {
-		struct bgmac_slot_info *slot = &ring->slots[ring->start];
+	while (ring->start != ring->end) {
+		int slot_idx = ring->start % BGMAC_TX_RING_SLOTS;
+		struct bgmac_slot_info *slot = &ring->slots[slot_idx];
+		u32 ctl1;
+		int len;
 
-		if (slot->skb) {
+		if (slot_idx == empty_slot)
+			break;
+
+		ctl1 = le32_to_cpu(ring->cpu_base[slot_idx].ctl1);
+		len = ctl1 & BGMAC_DESC_CTL1_LEN;
+		if (ctl1 & BGMAC_DESC_CTL0_SOF)
 			/* Unmap no longer used buffer */
-			dma_unmap_single(dma_dev, slot->dma_addr,
-					 slot->skb->len, DMA_TO_DEVICE);
-			slot->dma_addr = 0;
+			dma_unmap_single(dma_dev, slot->dma_addr, len,
+					 DMA_TO_DEVICE);
+		else
+			dma_unmap_page(dma_dev, slot->dma_addr, len,
+				       DMA_TO_DEVICE);
 
+		if (slot->skb) {
 			bytes_compl += slot->skb->len;
 			pkts_compl++;
 
 			/* Free memory! :) */
 			dev_kfree_skb(slot->skb);
 			slot->skb = NULL;
-		} else {
-			bgmac_err(bgmac, "Hardware reported transmission for empty TX ring slot %d! End of ring: %d\n",
-				  ring->start, ring->end);
 		}
 
-		if (++ring->start >= BGMAC_TX_RING_SLOTS)
-			ring->start = 0;
+		slot->dma_addr = 0;
+		ring->start++;
 		freed = true;
 	}
 
+	if (!pkts_compl)
+		return;
+
 	netdev_completed_queue(bgmac->net_dev, pkts_compl, bytes_compl);
 
-	if (freed && netif_queue_stopped(bgmac->net_dev))
+	if (netif_queue_stopped(bgmac->net_dev))
 		netif_wake_queue(bgmac->net_dev);
 }
 
@@ -275,43 +336,53 @@ static int bgmac_dma_rx_skb_for_slot(struct bgmac *bgmac,
 				     struct bgmac_slot_info *slot)
 {
 	struct device *dma_dev = bgmac->core->dma_dev;
-	struct sk_buff *skb;
 	dma_addr_t dma_addr;
 	struct bgmac_rx_header *rx;
+	void *buf;
 
 	/* Alloc skb */
-	skb = netdev_alloc_skb(bgmac->net_dev, BGMAC_RX_BUF_SIZE);
-	if (!skb)
+	buf = netdev_alloc_frag(BGMAC_RX_ALLOC_SIZE);
+	if (!buf)
 		return -ENOMEM;
 
 	/* Poison - if everything goes fine, hardware will overwrite it */
-	rx = (struct bgmac_rx_header *)skb->data;
+	rx = buf + BGMAC_RX_BUF_OFFSET;
 	rx->len = cpu_to_le16(0xdead);
 	rx->flags = cpu_to_le16(0xbeef);
 
 	/* Map skb for the DMA */
-	dma_addr = dma_map_single(dma_dev, skb->data,
+	dma_addr = dma_map_single(dma_dev, buf + BGMAC_RX_BUF_OFFSET,
 				  BGMAC_RX_BUF_SIZE, DMA_FROM_DEVICE);
 	if (dma_mapping_error(dma_dev, dma_addr)) {
 		bgmac_err(bgmac, "DMA mapping error\n");
-		dev_kfree_skb(skb);
+		put_page(virt_to_head_page(buf));
 		return -ENOMEM;
 	}
 
 	/* Update the slot */
-	slot->skb = skb;
+	slot->buf = buf;
 	slot->dma_addr = dma_addr;
 
 	return 0;
 }
 
+static void bgmac_dma_rx_update_index(struct bgmac *bgmac,
+				      struct bgmac_dma_ring *ring)
+{
+	dma_wmb();
+
+	bgmac_write(bgmac, ring->mmio_base + BGMAC_DMA_RX_INDEX,
+		    ring->index_base +
+		    ring->end * sizeof(struct bgmac_dma_desc));
+}
+
 static void bgmac_dma_rx_setup_desc(struct bgmac *bgmac,
 				    struct bgmac_dma_ring *ring, int desc_idx)
 {
 	struct bgmac_dma_desc *dma_desc = ring->cpu_base + desc_idx;
 	u32 ctl0 = 0, ctl1 = 0;
 
-	if (desc_idx == ring->num_slots - 1)
+	if (desc_idx == BGMAC_RX_RING_SLOTS - 1)
 		ctl0 |= BGMAC_DESC_CTL0_EOT;
 	ctl1 |= BGMAC_RX_BUF_SIZE & BGMAC_DESC_CTL1_LEN;
 	/* Is there any BGMAC device that requires extension? */
@@ -323,6 +394,21 @@ static void bgmac_dma_rx_setup_desc(struct bgmac *bgmac,
 	dma_desc->addr_high = cpu_to_le32(upper_32_bits(ring->slots[desc_idx].dma_addr));
 	dma_desc->ctl0 = cpu_to_le32(ctl0);
 	dma_desc->ctl1 = cpu_to_le32(ctl1);
+
+	ring->end = desc_idx;
+}
+
+static void bgmac_dma_rx_poison_buf(struct device *dma_dev,
+				    struct bgmac_slot_info *slot)
+{
+	struct bgmac_rx_header *rx = slot->buf + BGMAC_RX_BUF_OFFSET;
+
+	dma_sync_single_for_cpu(dma_dev, slot->dma_addr, BGMAC_RX_BUF_SIZE,
+				DMA_FROM_DEVICE);
+	rx->len = cpu_to_le16(0xdead);
+	rx->flags = cpu_to_le16(0xbeef);
+	dma_sync_single_for_device(dma_dev, slot->dma_addr, BGMAC_RX_BUF_SIZE,
+				   DMA_FROM_DEVICE);
 }
 
 static int bgmac_dma_rx_read(struct bgmac *bgmac, struct bgmac_dma_ring *ring,
@@ -337,70 +423,62 @@ static int bgmac_dma_rx_read(struct bgmac *bgmac, struct bgmac_dma_ring *ring,
 	end_slot &= BGMAC_DMA_RX_STATDPTR;
 	end_slot /= sizeof(struct bgmac_dma_desc);
 
-	ring->end = end_slot;
-
-	while (ring->start != ring->end) {
+	while (ring->start != end_slot) {
 		struct device *dma_dev = bgmac->core->dma_dev;
 		struct bgmac_slot_info *slot = &ring->slots[ring->start];
-		struct sk_buff *skb = slot->skb;
-		struct bgmac_rx_header *rx;
+		struct bgmac_rx_header *rx = slot->buf + BGMAC_RX_BUF_OFFSET;
+		struct sk_buff *skb;
+		void *buf = slot->buf;
+		dma_addr_t dma_addr = slot->dma_addr;
 		u16 len, flags;
 
-		/* Unmap buffer to make it accessible to the CPU */
-		dma_sync_single_for_cpu(dma_dev, slot->dma_addr,
-					BGMAC_RX_BUF_SIZE, DMA_FROM_DEVICE);
+		do {
+			/* Prepare new skb as replacement */
+			if (bgmac_dma_rx_skb_for_slot(bgmac, slot)) {
+				bgmac_dma_rx_poison_buf(dma_dev, slot);
+				break;
+			}
 
-		/* Get info from the header */
-		rx = (struct bgmac_rx_header *)skb->data;
-		len = le16_to_cpu(rx->len);
-		flags = le16_to_cpu(rx->flags);
+			/* Unmap buffer to make it accessible to the CPU */
+			dma_unmap_single(dma_dev, dma_addr,
+					 BGMAC_RX_BUF_SIZE, DMA_FROM_DEVICE);
 
-		do {
-			dma_addr_t old_dma_addr = slot->dma_addr;
-			int err;
+			/* Get info from the header */
+			len = le16_to_cpu(rx->len);
+			flags = le16_to_cpu(rx->flags);
 
 			/* Check for poison and drop or pass the packet */
 			if (len == 0xdead && flags == 0xbeef) {
 				bgmac_err(bgmac, "Found poisoned packet at slot %d, DMA issue!\n",
 					  ring->start);
-				dma_sync_single_for_device(dma_dev,
-							   slot->dma_addr,
-							   BGMAC_RX_BUF_SIZE,
-							   DMA_FROM_DEVICE);
+				put_page(virt_to_head_page(buf));
 				break;
 			}
 
-			/* Omit CRC. */
-			len -= ETH_FCS_LEN;
-
-			/* Prepare new skb as replacement */
-			err = bgmac_dma_rx_skb_for_slot(bgmac, slot);
-			if (err) {
-				/* Poison the old skb */
-				rx->len = cpu_to_le16(0xdead);
-				rx->flags = cpu_to_le16(0xbeef);
-
-				dma_sync_single_for_device(dma_dev,
-							   slot->dma_addr,
-							   BGMAC_RX_BUF_SIZE,
-							   DMA_FROM_DEVICE);
+			if (len > BGMAC_RX_ALLOC_SIZE) {
+				bgmac_err(bgmac, "Found oversized packet at slot %d, DMA issue!\n",
+					  ring->start);
+				put_page(virt_to_head_page(buf));
 				break;
 			}
-			bgmac_dma_rx_setup_desc(bgmac, ring, ring->start);
 
-			/* Unmap old skb, we'll pass it to the netfif */
-			dma_unmap_single(dma_dev, old_dma_addr,
-					 BGMAC_RX_BUF_SIZE, DMA_FROM_DEVICE);
+			/* Omit CRC. */
+			len -= ETH_FCS_LEN;
 
-			skb_put(skb, BGMAC_RX_FRAME_OFFSET + len);
-			skb_pull(skb, BGMAC_RX_FRAME_OFFSET);
+			skb = build_skb(buf, BGMAC_RX_ALLOC_SIZE);
+			skb_put(skb, BGMAC_RX_FRAME_OFFSET +
+				BGMAC_RX_BUF_OFFSET + len);
+			skb_pull(skb, BGMAC_RX_FRAME_OFFSET +
+				 BGMAC_RX_BUF_OFFSET);
 
 			skb_checksum_none_assert(skb);
 			skb->protocol = eth_type_trans(skb, bgmac->net_dev);
-			netif_receive_skb(skb);
+			napi_gro_receive(&bgmac->napi, skb);
 			handled++;
 		} while (0);
 
+		bgmac_dma_rx_setup_desc(bgmac, ring, ring->start);
+
 		if (++ring->start >= BGMAC_RX_RING_SLOTS)
 			ring->start = 0;
 
@@ -408,6 +486,8 @@ static int bgmac_dma_rx_read(struct bgmac *bgmac, struct bgmac_dma_ring *ring,
 			break;
 	}
 
+	bgmac_dma_rx_update_index(bgmac, ring);
+
 	return handled;
 }
 
@@ -433,40 +513,90 @@ static bool bgmac_dma_unaligned(struct bgmac *bgmac,
 	return false;
 }
 
-static void bgmac_dma_ring_free(struct bgmac *bgmac,
-				struct bgmac_dma_ring *ring)
+static void bgmac_dma_tx_ring_free(struct bgmac *bgmac,
+				   struct bgmac_dma_ring *ring)
 {
 	struct device *dma_dev = bgmac->core->dma_dev;
+	struct bgmac_dma_desc *dma_desc = ring->cpu_base;
 	struct bgmac_slot_info *slot;
-	int size;
 	int i;
 
-	for (i = 0; i < ring->num_slots; i++) {
+	for (i = 0; i < BGMAC_TX_RING_SLOTS; i++) {
+		int len = dma_desc[i].ctl1 & BGMAC_DESC_CTL1_LEN;
+
 		slot = &ring->slots[i];
-		if (slot->skb) {
-			if (slot->dma_addr)
-				dma_unmap_single(dma_dev, slot->dma_addr,
-						 slot->skb->len, DMA_TO_DEVICE);
-			dev_kfree_skb(slot->skb);
-		}
+		dev_kfree_skb(slot->skb);
+
+		if (!slot->dma_addr)
+			continue;
+
+		if (slot->skb)
+			dma_unmap_single(dma_dev, slot->dma_addr,
+					 len, DMA_TO_DEVICE);
+		else
+			dma_unmap_page(dma_dev, slot->dma_addr,
+				       len, DMA_TO_DEVICE);
 	}
+}
 
-	if (ring->cpu_base) {
-		/* Free ring of descriptors */
-		size = ring->num_slots * sizeof(struct bgmac_dma_desc);
-		dma_free_coherent(dma_dev, size, ring->cpu_base,
-				  ring->dma_base);
+static void bgmac_dma_rx_ring_free(struct bgmac *bgmac,
+				   struct bgmac_dma_ring *ring)
+{
+	struct device *dma_dev = bgmac->core->dma_dev;
+	struct bgmac_slot_info *slot;
+	int i;
+
+	for (i = 0; i < BGMAC_RX_RING_SLOTS; i++) {
+		slot = &ring->slots[i];
+		if (!slot->dma_addr)
+			continue;
+
+		dma_unmap_single(dma_dev, slot->dma_addr,
+				 BGMAC_RX_BUF_SIZE,
+				 DMA_FROM_DEVICE);
+		put_page(virt_to_head_page(slot->buf));
+		slot->dma_addr = 0;
 	}
 }
 
+static void bgmac_dma_ring_desc_free(struct bgmac *bgmac,
+				     struct bgmac_dma_ring *ring,
+				     int num_slots)
+{
+	struct device *dma_dev = bgmac->core->dma_dev;
+	int size;
+
+	if (!ring->cpu_base)
+	    return;
+
+	/* Free ring of descriptors */
+	size = num_slots * sizeof(struct bgmac_dma_desc);
+	dma_free_coherent(dma_dev, size, ring->cpu_base,
+			  ring->dma_base);
+}
+
+static void bgmac_dma_cleanup(struct bgmac *bgmac)
+{
+	int i;
+
+	for (i = 0; i < BGMAC_MAX_TX_RINGS; i++)
+		bgmac_dma_tx_ring_free(bgmac, &bgmac->tx_ring[i]);
+
+	for (i = 0; i < BGMAC_MAX_RX_RINGS; i++)
+		bgmac_dma_rx_ring_free(bgmac, &bgmac->rx_ring[i]);
+}
+
 static void bgmac_dma_free(struct bgmac *bgmac)
 {
 	int i;
 
 	for (i = 0; i < BGMAC_MAX_TX_RINGS; i++)
-		bgmac_dma_ring_free(bgmac, &bgmac->tx_ring[i]);
+		bgmac_dma_ring_desc_free(bgmac, &bgmac->tx_ring[i],
+					 BGMAC_TX_RING_SLOTS);
+
 	for (i = 0; i < BGMAC_MAX_RX_RINGS; i++)
-		bgmac_dma_ring_free(bgmac, &bgmac->rx_ring[i]);
+		bgmac_dma_ring_desc_free(bgmac, &bgmac->rx_ring[i],
+					 BGMAC_RX_RING_SLOTS);
 }
 
 static int bgmac_dma_alloc(struct bgmac *bgmac)
@@ -489,11 +619,10 @@ static int bgmac_dma_alloc(struct bgmac *bgmac)
 
 	for (i = 0; i < BGMAC_MAX_TX_RINGS; i++) {
 		ring = &bgmac->tx_ring[i];
-		ring->num_slots = BGMAC_TX_RING_SLOTS;
 		ring->mmio_base = ring_base[i];
 
 		/* Alloc ring of descriptors */
-		size = ring->num_slots * sizeof(struct bgmac_dma_desc);
+		size = BGMAC_TX_RING_SLOTS * sizeof(struct bgmac_dma_desc);
 		ring->cpu_base = dma_zalloc_coherent(dma_dev, size,
 						     &ring->dma_base,
 						     GFP_KERNEL);
@@ -514,14 +643,11 @@ static int bgmac_dma_alloc(struct bgmac *bgmac)
 	}
 
 	for (i = 0; i < BGMAC_MAX_RX_RINGS; i++) {
-		int j;
-
 		ring = &bgmac->rx_ring[i];
-		ring->num_slots = BGMAC_RX_RING_SLOTS;
 		ring->mmio_base = ring_base[i];
 
 		/* Alloc ring of descriptors */
-		size = ring->num_slots * sizeof(struct bgmac_dma_desc);
+		size = BGMAC_RX_RING_SLOTS * sizeof(struct bgmac_dma_desc);
 		ring->cpu_base = dma_zalloc_coherent(dma_dev, size,
 						     &ring->dma_base,
 						     GFP_KERNEL);
@@ -538,15 +664,6 @@ static int bgmac_dma_alloc(struct bgmac *bgmac)
 			ring->index_base = lower_32_bits(ring->dma_base);
 		else
 			ring->index_base = 0;
-
-		/* Alloc RX slots */
-		for (j = 0; j < ring->num_slots; j++) {
-			err = bgmac_dma_rx_skb_for_slot(bgmac, &ring->slots[j]);
-			if (err) {
-				bgmac_err(bgmac, "Can't allocate skb for slot in RX ring\n");
-				goto err_dma_free;
-			}
-		}
 	}
 
 	return 0;
@@ -556,10 +673,10 @@ err_dma_free:
 	return -ENOMEM;
 }
 
-static void bgmac_dma_init(struct bgmac *bgmac)
+static int bgmac_dma_init(struct bgmac *bgmac)
 {
 	struct bgmac_dma_ring *ring;
-	int i;
+	int i, err;
 
 	for (i = 0; i < BGMAC_MAX_TX_RINGS; i++) {
 		ring = &bgmac->tx_ring[i];
@@ -591,16 +708,24 @@ static void bgmac_dma_init(struct bgmac *bgmac)
 		if (ring->unaligned)
 			bgmac_dma_rx_enable(bgmac, ring);
 
-		for (j = 0; j < ring->num_slots; j++)
-			bgmac_dma_rx_setup_desc(bgmac, ring, j);
-
-		bgmac_write(bgmac, ring->mmio_base + BGMAC_DMA_RX_INDEX,
-			    ring->index_base +
-			    ring->num_slots * sizeof(struct bgmac_dma_desc));
-
 		ring->start = 0;
 		ring->end = 0;
+		for (j = 0; j < BGMAC_RX_RING_SLOTS; j++) {
+			err = bgmac_dma_rx_skb_for_slot(bgmac, &ring->slots[j]);
+			if (err)
+				goto error;
+
+			bgmac_dma_rx_setup_desc(bgmac, ring, j);
+		}
+
+		bgmac_dma_rx_update_index(bgmac, ring);
 	}
+
+	return 0;
+
+error:
+	bgmac_dma_cleanup(bgmac);
+	return err;
 }
 
 /**************************************************
@@ -1008,8 +1133,6 @@ static void bgmac_chip_reset(struct bgmac *bgmac)
 	bgmac_phy_init(bgmac);
 
 	netdev_reset_queue(bgmac->net_dev);
-
-	bgmac->int_status = 0;
 }
 
 static void bgmac_chip_intrs_on(struct bgmac *bgmac)
@@ -1078,11 +1201,8 @@ static void bgmac_enable(struct bgmac *bgmac)
 }
 
 /* http://bcm-v4.sipsolutions.net/mac-gbit/gmac/chipinit */
-static void bgmac_chip_init(struct bgmac *bgmac, bool full_init)
+static void bgmac_chip_init(struct bgmac *bgmac)
 {
-	struct bgmac_dma_ring *ring;
-	int i;
-
 	/* 1 interrupt per received frame */
 	bgmac_write(bgmac, BGMAC_INT_RECV_LAZY, 1 << BGMAC_IRL_FC_SHIFT);
 
@@ -1100,16 +1220,7 @@ static void bgmac_chip_init(struct bgmac *bgmac, bool full_init)
 
 	bgmac_write(bgmac, BGMAC_RXMAX_LENGTH, 32 + ETHER_MAX_LEN);
 
-	if (full_init) {
-		bgmac_dma_init(bgmac);
-		if (1) /* FIXME: is there any case we don't want IRQs? */
-			bgmac_chip_intrs_on(bgmac);
-	} else {
-		for (i = 0; i < BGMAC_MAX_RX_RINGS; i++) {
-			ring = &bgmac->rx_ring[i];
-			bgmac_dma_rx_enable(bgmac, ring);
-		}
-	}
+	bgmac_chip_intrs_on(bgmac);
 
 	bgmac_enable(bgmac);
 }
@@ -1124,14 +1235,13 @@ static irqreturn_t bgmac_interrupt(int irq, void *dev_id)
 	if (!int_status)
 		return IRQ_NONE;
 
-	/* Ack */
-	bgmac_write(bgmac, BGMAC_INT_STATUS, int_status);
+	int_status &= ~(BGMAC_IS_TX0 | BGMAC_IS_RX);
+	if (int_status)
+		bgmac_err(bgmac, "Unknown IRQs: 0x%08X\n", int_status);
 
 	/* Disable new interrupts until handling existing ones */
 	bgmac_chip_intrs_off(bgmac);
 
-	bgmac->int_status = int_status;
-
 	napi_schedule(&bgmac->napi);
 
 	return IRQ_HANDLED;
@@ -1140,25 +1250,17 @@ static irqreturn_t bgmac_interrupt(int irq, void *dev_id)
 static int bgmac_poll(struct napi_struct *napi, int weight)
 {
 	struct bgmac *bgmac = container_of(napi, struct bgmac, napi);
-	struct bgmac_dma_ring *ring;
 	int handled = 0;
 
-	if (bgmac->int_status & BGMAC_IS_TX0) {
-		ring = &bgmac->tx_ring[0];
-		bgmac_dma_tx_free(bgmac, ring);
-		bgmac->int_status &= ~BGMAC_IS_TX0;
-	}
+	/* Ack */
+	bgmac_write(bgmac, BGMAC_INT_STATUS, ~0);
 
-	if (bgmac->int_status & BGMAC_IS_RX) {
-		ring = &bgmac->rx_ring[0];
-		handled += bgmac_dma_rx_read(bgmac, ring, weight);
-		bgmac->int_status &= ~BGMAC_IS_RX;
-	}
+	bgmac_dma_tx_free(bgmac, &bgmac->tx_ring[0]);
+	handled += bgmac_dma_rx_read(bgmac, &bgmac->rx_ring[0], weight);
 
-	if (bgmac->int_status) {
-		bgmac_err(bgmac, "Unknown IRQs: 0x%08X\n", bgmac->int_status);
-		bgmac->int_status = 0;
-	}
+	/* Poll again if more events arrived in the meantime */
+	if (bgmac_read(bgmac, BGMAC_INT_STATUS) & (BGMAC_IS_TX0 | BGMAC_IS_RX))
+		return handled;
 
 	if (handled < weight) {
 		napi_complete(napi);
@@ -1178,23 +1280,27 @@ static int bgmac_open(struct net_device *net_dev)
 	int err = 0;
 
 	bgmac_chip_reset(bgmac);
+
+	err = bgmac_dma_init(bgmac);
+	if (err)
+		return err;
+
 	/* Specs say about reclaiming rings here, but we do that in DMA init */
-	bgmac_chip_init(bgmac, true);
+	bgmac_chip_init(bgmac);
 
 	err = request_irq(bgmac->core->irq, bgmac_interrupt, IRQF_SHARED,
 			  KBUILD_MODNAME, net_dev);
 	if (err < 0) {
 		bgmac_err(bgmac, "IRQ request error: %d!\n", err);
-		goto err_out;
+		bgmac_dma_cleanup(bgmac);
+		return err;
 	}
 	napi_enable(&bgmac->napi);
 
 	phy_start(bgmac->phy_dev);
 
 	netif_carrier_on(net_dev);
-
-err_out:
-	return err;
+	return 0;
 }
 
 static int bgmac_stop(struct net_device *net_dev)
@@ -1210,6 +1316,7 @@ static int bgmac_stop(struct net_device *net_dev)
 	free_irq(bgmac->core->irq, net_dev);
 
 	bgmac_chip_reset(bgmac);
+	bgmac_dma_cleanup(bgmac);
 
 	return 0;
 }
@@ -1330,13 +1437,46 @@ static void bgmac_adjust_link(struct net_device *net_dev)
 	}
 }
 
+static int bgmac_fixed_phy_register(struct bgmac *bgmac)
+{
+	struct fixed_phy_status fphy_status = {
+		.link = 1,
+		.speed = SPEED_1000,
+		.duplex = DUPLEX_FULL,
+	};
+	struct phy_device *phy_dev;
+	int err;
+
+	phy_dev = fixed_phy_register(PHY_POLL, &fphy_status, NULL);
+	if (!phy_dev || IS_ERR(phy_dev)) {
+		bgmac_err(bgmac, "Failed to register fixed PHY device\n");
+		return -ENODEV;
+	}
+
+	err = phy_connect_direct(bgmac->net_dev, phy_dev, bgmac_adjust_link,
+				 PHY_INTERFACE_MODE_MII);
+	if (err) {
+		bgmac_err(bgmac, "Connecting PHY failed\n");
+		return err;
+	}
+
+	bgmac->phy_dev = phy_dev;
+
+	return err;
+}
+
 static int bgmac_mii_register(struct bgmac *bgmac)
 {
+	struct bcma_chipinfo *ci = &bgmac->core->bus->chipinfo;
 	struct mii_bus *mii_bus;
 	struct phy_device *phy_dev;
 	char bus_id[MII_BUS_ID_SIZE + 3];
 	int i, err = 0;
 
+	if (ci->id == BCMA_CHIP_ID_BCM4707 ||
+	    ci->id == BCMA_CHIP_ID_BCM53018)
+		return bgmac_fixed_phy_register(bgmac);
+
 	mii_bus = mdiobus_alloc();
 	if (!mii_bus)
 		return -ENOMEM;
@@ -1517,6 +1657,10 @@ static int bgmac_probe(struct bcma_device *core)
 		goto err_dma_free;
 	}
 
+	net_dev->features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
+	net_dev->hw_features = net_dev->features;
+	net_dev->vlan_features = net_dev->features;
+
 	err = register_netdev(bgmac->net_dev);
 	if (err) {
 		bgmac_err(bgmac, "Cannot register net device\n");
diff --git a/drivers/net/ethernet/broadcom/bgmac.h b/drivers/net/ethernet/broadcom/bgmac.h
index 89fa5bc69c51..db27febbb215 100644
--- a/drivers/net/ethernet/broadcom/bgmac.h
+++ b/drivers/net/ethernet/broadcom/bgmac.h
@@ -345,8 +345,8 @@
 
 #define BGMAC_DESC_CTL0_EOT			0x10000000	/* End of ring */
 #define BGMAC_DESC_CTL0_IOC			0x20000000	/* IRQ on complete */
-#define BGMAC_DESC_CTL0_SOF			0x40000000	/* Start of frame */
-#define BGMAC_DESC_CTL0_EOF			0x80000000	/* End of frame */
+#define BGMAC_DESC_CTL0_EOF			0x40000000	/* End of frame */
+#define BGMAC_DESC_CTL0_SOF			0x80000000	/* Start of frame */
 #define BGMAC_DESC_CTL1_LEN			0x00001FFF
 
 #define BGMAC_PHY_NOREGS			0x1E
@@ -356,12 +356,16 @@
 #define BGMAC_MAX_RX_RINGS			1
 
 #define BGMAC_TX_RING_SLOTS			128
-#define BGMAC_RX_RING_SLOTS			512 - 1		/* Why -1? Well, Broadcom does that... */
+#define BGMAC_RX_RING_SLOTS			512
 
 #define BGMAC_RX_HEADER_LEN			28		/* Last 24 bytes are unused. Well... */
 #define BGMAC_RX_FRAME_OFFSET			30		/* There are 2 unused bytes between header and real data */
+#define BGMAC_RX_BUF_OFFSET			(NET_SKB_PAD + NET_IP_ALIGN - \
+						 BGMAC_RX_FRAME_OFFSET)
 #define BGMAC_RX_MAX_FRAME_SIZE			1536		/* Copied from b44/tg3 */
 #define BGMAC_RX_BUF_SIZE			(BGMAC_RX_FRAME_OFFSET + BGMAC_RX_MAX_FRAME_SIZE)
+#define BGMAC_RX_ALLOC_SIZE			(SKB_DATA_ALIGN(BGMAC_RX_BUF_SIZE + BGMAC_RX_BUF_OFFSET) + \
+						 SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
 
 #define BGMAC_BFL_ENETROBO			0x0010		/* has ephy roboswitch spi */
 #define BGMAC_BFL_ENETADM			0x0080		/* has ADMtek switch */
@@ -383,7 +387,10 @@
 #define ETHER_MAX_LEN   1518
 
 struct bgmac_slot_info {
-	struct sk_buff *skb;
+	union {
+		struct sk_buff *skb;
+		void *buf;
+	};
 	dma_addr_t dma_addr;
 };
 
@@ -409,14 +416,13 @@ enum bgmac_dma_ring_type {
  * empty.
  */
 struct bgmac_dma_ring {
-	u16 num_slots;
-	u16 start;
-	u16 end;
+	u32 start;
+	u32 end;
 
-	u16 mmio_base;
 	struct bgmac_dma_desc *cpu_base;
 	dma_addr_t dma_base;
 	u32 index_base; /* Used for unaligned rings only, otherwise 0 */
+	u16 mmio_base;
 	bool unaligned;
 
 	struct bgmac_slot_info slots[BGMAC_RX_RING_SLOTS];
@@ -447,7 +453,6 @@ struct bgmac {
 
 	/* Int */
 	u32 int_mask;
-	u32 int_status;
 
 	/* Current MAC state */
 	int mac_speed;
diff --git a/drivers/net/ethernet/broadcom/bnx2.c b/drivers/net/ethernet/broadcom/bnx2.c
index 02bf0b86995b..2b66ef3d8217 100644
--- a/drivers/net/ethernet/broadcom/bnx2.c
+++ b/drivers/net/ethernet/broadcom/bnx2.c
@@ -1,7 +1,7 @@
-/* bnx2.c: QLogic NX2 network driver.
+/* bnx2.c: QLogic bnx2 network driver.
  *
  * Copyright (c) 2004-2014 Broadcom Corporation
- * Copyright (c) 2014 QLogic Corporation
+ * Copyright (c) 2014-2015 QLogic 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
@@ -58,8 +58,8 @@
 #include "bnx2_fw.h"
 
 #define DRV_MODULE_NAME		"bnx2"
-#define DRV_MODULE_VERSION	"2.2.5"
-#define DRV_MODULE_RELDATE	"December 20, 2013"
+#define DRV_MODULE_VERSION	"2.2.6"
+#define DRV_MODULE_RELDATE	"January 29, 2014"
 #define FW_MIPS_FILE_06		"bnx2/bnx2-mips-06-6.2.3.fw"
 #define FW_RV2P_FILE_06		"bnx2/bnx2-rv2p-06-6.0.15.fw"
 #define FW_MIPS_FILE_09		"bnx2/bnx2-mips-09-6.2.1b.fw"
@@ -72,10 +72,10 @@
 #define TX_TIMEOUT  (5*HZ)
 
 static char version[] =
-	"QLogic NetXtreme II Gigabit Ethernet Driver " DRV_MODULE_NAME " v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
+	"QLogic " DRV_MODULE_NAME " Gigabit Ethernet Driver v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
 
 MODULE_AUTHOR("Michael Chan <mchan@broadcom.com>");
-MODULE_DESCRIPTION("QLogic NetXtreme II BCM5706/5708/5709/5716 Driver");
+MODULE_DESCRIPTION("QLogic BCM5706/5708/5709/5716 Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_MODULE_VERSION);
 MODULE_FIRMWARE(FW_MIPS_FILE_06);
@@ -4984,8 +4984,6 @@ bnx2_init_chip(struct bnx2 *bp)
 
 	bp->idle_chk_status_idx = 0xffff;
 
-	bp->rx_mode = BNX2_EMAC_RX_MODE_SORT_MODE;
-
 	/* Set up how to generate a link change interrupt. */
 	BNX2_WR(bp, BNX2_EMAC_ATTENTION_ENA, BNX2_EMAC_ATTENTION_ENA_LINK);
 
@@ -7710,17 +7708,6 @@ bnx2_set_phys_id(struct net_device *dev, enum ethtool_phys_id_state state)
 	return 0;
 }
 
-static netdev_features_t
-bnx2_fix_features(struct net_device *dev, netdev_features_t features)
-{
-	struct bnx2 *bp = netdev_priv(dev);
-
-	if (!(bp->flags & BNX2_FLAG_CAN_KEEP_VLAN))
-		features |= NETIF_F_HW_VLAN_CTAG_RX;
-
-	return features;
-}
-
 static int
 bnx2_set_features(struct net_device *dev, netdev_features_t features)
 {
@@ -8527,7 +8514,6 @@ static const struct net_device_ops bnx2_netdev_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_set_mac_address	= bnx2_change_mac_addr,
 	.ndo_change_mtu		= bnx2_change_mtu,
-	.ndo_fix_features	= bnx2_fix_features,
 	.ndo_set_features	= bnx2_set_features,
 	.ndo_tx_timeout		= bnx2_tx_timeout,
 #ifdef CONFIG_NET_POLL_CONTROLLER
@@ -8578,6 +8564,9 @@ bnx2_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	dev->features |= dev->hw_features;
 	dev->priv_flags |= IFF_UNICAST_FLT;
 
+	if (!(bp->flags & BNX2_FLAG_CAN_KEEP_VLAN))
+		dev->hw_features &= ~NETIF_F_HW_VLAN_CTAG_RX;
+
 	if ((rc = register_netdev(dev))) {
 		dev_err(&pdev->dev, "Cannot register net device\n");
 		goto error;
diff --git a/drivers/net/ethernet/broadcom/bnx2.h b/drivers/net/ethernet/broadcom/bnx2.h
index 28df35d35893..f92f76c44756 100644
--- a/drivers/net/ethernet/broadcom/bnx2.h
+++ b/drivers/net/ethernet/broadcom/bnx2.h
@@ -1,7 +1,7 @@
-/* bnx2.h: QLogic NX2 network driver.
+/* bnx2.h: QLogic bnx2 network driver.
  *
  * Copyright (c) 2004-2014 Broadcom Corporation
- * Copyright (c) 2014 QLogic Corporation
+ * Copyright (c) 2014-2015 QLogic 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
diff --git a/drivers/net/ethernet/broadcom/bnx2_fw.h b/drivers/net/ethernet/broadcom/bnx2_fw.h
index 7db79c28b5ff..b0f2ccadaffd 100644
--- a/drivers/net/ethernet/broadcom/bnx2_fw.h
+++ b/drivers/net/ethernet/broadcom/bnx2_fw.h
@@ -1,7 +1,7 @@
-/* bnx2_fw.h: QLogic NX2 network driver.
+/* bnx2_fw.h: QLogic bnx2 network driver.
  *
  * Copyright (c) 2004, 2005, 2006, 2007 Broadcom Corporation
- * Copyright (c) 2014 QLogic Corporation
+ * Copyright (c) 2014-2015 QLogic 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
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_ethtool.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
index ffe4e003e636..e3d853cab7c9 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
@@ -2446,7 +2446,7 @@ static int bnx2x_run_loopback(struct bnx2x *bp, int loopback_mode)
 	}
 	packet = skb_put(skb, pkt_size);
 	memcpy(packet, bp->dev->dev_addr, ETH_ALEN);
-	memset(packet + ETH_ALEN, 0, ETH_ALEN);
+	eth_zero_addr(packet + ETH_ALEN);
 	memset(packet + 2*ETH_ALEN, 0x77, (ETH_HLEN - 2*ETH_ALEN));
 	for (i = ETH_HLEN; i < pkt_size; i++)
 		packet[i] = (unsigned char) (i & 0xff);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
index 583591d52497..058bc7328220 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
@@ -521,6 +521,17 @@ struct port_hw_cfg {		    /* port 0: 0x12c  port 1: 0x2bc */
 	 */
 	#define PORT_HW_CFG_TX_DRV_BROADCAST_MASK                     0x000F0000
 	#define PORT_HW_CFG_TX_DRV_BROADCAST_SHIFT                    16
+	/*  Set non-default values for TXFIR in SFP mode. */
+	#define PORT_HW_CFG_TX_DRV_IFIR_MASK                          0x00F00000
+	#define PORT_HW_CFG_TX_DRV_IFIR_SHIFT                         20
+
+	/*  Set non-default values for IPREDRIVER in SFP mode. */
+	#define PORT_HW_CFG_TX_DRV_IPREDRIVER_MASK                    0x0F000000
+	#define PORT_HW_CFG_TX_DRV_IPREDRIVER_SHIFT                   24
+
+	/*  Set non-default values for POST2 in SFP mode. */
+	#define PORT_HW_CFG_TX_DRV_POST2_MASK                         0xF0000000
+	#define PORT_HW_CFG_TX_DRV_POST2_SHIFT                        28
 
 	u32 reserved0[5];				    /* 0x17c */
 
@@ -2247,8 +2258,8 @@ struct shmem2_region {
 	#define LINK_SFP_EEPROM_COMP_CODE_LRM	0x00004000
 
 	u32 reserved5[2];
-	u32 reserved6[PORT_MAX];
-
+	u32 link_change_count[PORT_MAX];        /* Offset 0x160-0x164 */
+	#define LINK_CHANGE_COUNT_MASK 0xff     /* Offset 0x168 */
 	/* driver version for each personality */
 	struct os_drv_ver func_os_drv_ver[E2_FUNC_MAX]; /* Offset 0x16c */
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h
index bd90e50bd8e6..d6e1975b7b69 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_init.h
@@ -278,7 +278,7 @@ static inline void bnx2x_dcb_config_qm(struct bnx2x *bp, enum cos_mode mode,
 }
 
 
-/* congestion managment port init api description
+/* congestion management port init api description
  * the api works as follows:
  * the driver should pass the cmng_init_input struct, the port_init function
  * will prepare the required internal ram structure which will be passed back
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index 778e4cd32571..21a0d6afca4a 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -195,6 +195,10 @@ typedef int (*read_sfp_module_eeprom_func_p)(struct bnx2x_phy *phy,
 
 #define MAX_PACKET_SIZE					(9700)
 #define MAX_KR_LINK_RETRY				4
+#define DEFAULT_TX_DRV_BRDCT		2
+#define DEFAULT_TX_DRV_IFIR		0
+#define DEFAULT_TX_DRV_POST2		3
+#define DEFAULT_TX_DRV_IPRE_DRIVER	6
 
 /**********************************************************/
 /*                     INTERFACE                          */
@@ -563,7 +567,7 @@ static void bnx2x_ets_e3b0_set_credit_upper_bound_nig(
 *	Will return the NIG ETS registers to init values.Except
 *	credit_upper_bound.
 *	That isn't used in this configuration (No WFQ is enabled) and will be
-*	configured acording to spec
+*	configured according to spec
 *.
 ******************************************************************************/
 static void bnx2x_ets_e3b0_nig_disabled(const struct link_params *params,
@@ -680,7 +684,7 @@ static void bnx2x_ets_e3b0_set_credit_upper_bound_pbf(
 *	Will return the PBF ETS registers to init values.Except
 *	credit_upper_bound.
 *	That isn't used in this configuration (No WFQ is enabled) and will be
-*	configured acording to spec
+*	configured according to spec
 *.
 ******************************************************************************/
 static void bnx2x_ets_e3b0_pbf_disabled(const struct link_params *params)
@@ -738,7 +742,7 @@ static void bnx2x_ets_e3b0_pbf_disabled(const struct link_params *params)
 }
 /******************************************************************************
 * Description:
-*	E3B0 disable will return basicly the values to init values.
+*	E3B0 disable will return basically the values to init values.
 *.
 ******************************************************************************/
 static int bnx2x_ets_e3b0_disabled(const struct link_params *params,
@@ -761,7 +765,7 @@ static int bnx2x_ets_e3b0_disabled(const struct link_params *params,
 
 /******************************************************************************
 * Description:
-*	Disable will return basicly the values to init values.
+*	Disable will return basically the values to init values.
 *
 ******************************************************************************/
 int bnx2x_ets_disabled(struct link_params *params,
@@ -2938,7 +2942,7 @@ static int bnx2x_eee_initial_config(struct link_params *params,
 {
 	vars->eee_status |= ((u32) mode) << SHMEM_EEE_SUPPORTED_SHIFT;
 
-	/* Propogate params' bits --> vars (for migration exposure) */
+	/* Propagate params' bits --> vars (for migration exposure) */
 	if (params->eee_mode & EEE_MODE_ENABLE_LPI)
 		vars->eee_status |= SHMEM_EEE_LPI_REQUESTED_BIT;
 	else
@@ -3595,10 +3599,11 @@ static u8 bnx2x_ext_phy_resolve_fc(struct bnx2x_phy *phy,
  * init configuration, and set/clear SGMII flag. Internal
  * phy init is done purely in phy_init stage.
  */
-#define WC_TX_DRIVER(post2, idriver, ipre) \
+#define WC_TX_DRIVER(post2, idriver, ipre, ifir) \
 	((post2 << MDIO_WC_REG_TX0_TX_DRIVER_POST2_COEFF_OFFSET) | \
 	 (idriver << MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET) | \
-	 (ipre << MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET))
+	 (ipre << MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET) | \
+	 (ifir << MDIO_WC_REG_TX0_TX_DRIVER_IFIR_OFFSET))
 
 #define WC_TX_FIR(post, main, pre) \
 	((post << MDIO_WC_REG_TX_FIR_TAP_POST_TAP_OFFSET) | \
@@ -3765,12 +3770,12 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 	lane = bnx2x_get_warpcore_lane(phy, params);
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_TX0_TX_DRIVER + 0x10*lane,
-			 WC_TX_DRIVER(0x02, 0x06, 0x09));
+			 WC_TX_DRIVER(0x02, 0x06, 0x09, 0));
 	/* Configure the next lane if dual mode */
 	if (phy->flags & FLAGS_WC_DUAL_MODE)
 		bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 				 MDIO_WC_REG_TX0_TX_DRIVER + 0x10*(lane+1),
-				 WC_TX_DRIVER(0x02, 0x06, 0x09));
+				 WC_TX_DRIVER(0x02, 0x06, 0x09, 0));
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_CL72_USERB0_CL72_OS_DEF_CTRL,
 			 0x03f0);
@@ -3933,6 +3938,7 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 misc1_val, tap_val, tx_driver_val, lane, val;
 	u32 cfg_tap_val, tx_drv_brdct, tx_equal;
+	u32 ifir_val, ipost2_val, ipre_driver_val;
 
 	/* Hold rxSeqStart */
 	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
@@ -3978,7 +3984,7 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 	if (is_xfi) {
 		misc1_val |= 0x5;
 		tap_val = WC_TX_FIR(0x08, 0x37, 0x00);
-		tx_driver_val = WC_TX_DRIVER(0x00, 0x02, 0x03);
+		tx_driver_val = WC_TX_DRIVER(0x00, 0x02, 0x03, 0);
 	} else {
 		cfg_tap_val = REG_RD(bp, params->shmem_base +
 				     offsetof(struct shmem_region, dev_info.
@@ -3987,10 +3993,6 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 
 		tx_equal = cfg_tap_val & PORT_HW_CFG_TX_EQUALIZATION_MASK;
 
-		tx_drv_brdct = (cfg_tap_val &
-				PORT_HW_CFG_TX_DRV_BROADCAST_MASK) >>
-			       PORT_HW_CFG_TX_DRV_BROADCAST_SHIFT;
-
 		misc1_val |= 0x9;
 
 		/* TAP values are controlled by nvram, if value there isn't 0 */
@@ -3999,11 +4001,36 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 		else
 			tap_val = WC_TX_FIR(0x0f, 0x2b, 0x02);
 
-		if (tx_drv_brdct)
-			tx_driver_val = WC_TX_DRIVER(0x03, (u16)tx_drv_brdct,
-						     0x06);
-		else
-			tx_driver_val = WC_TX_DRIVER(0x03, 0x02, 0x06);
+		ifir_val = DEFAULT_TX_DRV_IFIR;
+		ipost2_val = DEFAULT_TX_DRV_POST2;
+		ipre_driver_val = DEFAULT_TX_DRV_IPRE_DRIVER;
+		tx_drv_brdct = DEFAULT_TX_DRV_BRDCT;
+
+		/* If any of the IFIR/IPRE_DRIVER/POST@ is set, apply all
+		 * configuration.
+		 */
+		if (cfg_tap_val & (PORT_HW_CFG_TX_DRV_IFIR_MASK |
+				   PORT_HW_CFG_TX_DRV_IPREDRIVER_MASK |
+				   PORT_HW_CFG_TX_DRV_POST2_MASK)) {
+			ifir_val = (cfg_tap_val &
+				    PORT_HW_CFG_TX_DRV_IFIR_MASK) >>
+				PORT_HW_CFG_TX_DRV_IFIR_SHIFT;
+			ipre_driver_val = (cfg_tap_val &
+					   PORT_HW_CFG_TX_DRV_IPREDRIVER_MASK)
+			>> PORT_HW_CFG_TX_DRV_IPREDRIVER_SHIFT;
+			ipost2_val = (cfg_tap_val &
+				      PORT_HW_CFG_TX_DRV_POST2_MASK) >>
+				PORT_HW_CFG_TX_DRV_POST2_SHIFT;
+		}
+
+		if (cfg_tap_val & PORT_HW_CFG_TX_DRV_BROADCAST_MASK) {
+			tx_drv_brdct = (cfg_tap_val &
+					PORT_HW_CFG_TX_DRV_BROADCAST_MASK) >>
+				PORT_HW_CFG_TX_DRV_BROADCAST_SHIFT;
+		}
+
+		tx_driver_val = WC_TX_DRIVER(ipost2_val, tx_drv_brdct,
+					     ipre_driver_val, ifir_val);
 	}
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_SERDESDIGITAL_MISC1, misc1_val);
@@ -4144,7 +4171,7 @@ static void bnx2x_warpcore_set_20G_DXGXS(struct bnx2x *bp,
 			  MDIO_WC_REG_TX_FIR_TAP_ENABLE));
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_TX0_TX_DRIVER + 0x10*lane,
-			 WC_TX_DRIVER(0x02, 0x02, 0x02));
+			 WC_TX_DRIVER(0x02, 0x02, 0x02, 0));
 }
 
 static void bnx2x_warpcore_set_sgmii_speed(struct bnx2x_phy *phy,
@@ -6731,6 +6758,25 @@ static int bnx2x_update_link_up(struct link_params *params,
 	msleep(20);
 	return rc;
 }
+
+static void bnx2x_chng_link_count(struct link_params *params, bool clear)
+{
+	struct bnx2x *bp = params->bp;
+	u32 addr, val;
+
+	/* Verify the link_change_count is supported by the MFW */
+	if (!(SHMEM2_HAS(bp, link_change_count)))
+		return;
+
+	addr = params->shmem2_base +
+		offsetof(struct shmem2_region, link_change_count[params->port]);
+	if (clear)
+		val = 0;
+	else
+		val = REG_RD(bp, addr) + 1;
+	REG_WR(bp, addr, val);
+}
+
 /* The bnx2x_link_update function should be called upon link
  * interrupt.
  * Link is considered up as follows:
@@ -6749,6 +6795,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	struct link_vars phy_vars[MAX_PHYS];
 	u8 port = params->port;
 	u8 link_10g_plus, phy_index;
+	u32 prev_link_status = vars->link_status;
 	u8 ext_phy_link_up = 0, cur_link_up;
 	int rc = 0;
 	u8 is_mi_int = 0;
@@ -6988,6 +7035,9 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	else
 		rc = bnx2x_update_link_down(params, vars);
 
+	if ((prev_link_status ^ vars->link_status) & LINK_STATUS_LINK_UP)
+		bnx2x_chng_link_count(params, false);
+
 	/* Update MCP link status was changed */
 	if (params->feature_config_flags & FEATURE_CONFIG_BC_SUPPORTS_AFEX)
 		bnx2x_fw_command(bp, DRV_MSG_CODE_LINK_STATUS_CHANGED, 0);
@@ -12631,6 +12681,7 @@ int bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 	params->link_flags = PHY_INITIALIZED;
 	/* Driver opens NIG-BRB filters */
 	bnx2x_set_rx_filter(params, 1);
+	bnx2x_chng_link_count(params, true);
 	/* Check if link flap can be avoided */
 	lfa_status = bnx2x_check_lfa(params);
 
@@ -12705,6 +12756,7 @@ int bnx2x_link_reset(struct link_params *params, struct link_vars *vars,
 	DP(NETIF_MSG_LINK, "Resetting the link of port %d\n", port);
 	/* Disable attentions */
 	vars->link_status = 0;
+	bnx2x_chng_link_count(params, true);
 	bnx2x_update_mng(params, vars->link_status);
 	vars->eee_status &= ~(SHMEM_EEE_LP_ADV_STATUS_MASK |
 			      SHMEM_EEE_ACTIVE_BIT);
@@ -13308,7 +13360,7 @@ static void bnx2x_check_over_curr(struct link_params *params,
 		vars->phy_flags &= ~PHY_OVER_CURRENT_FLAG;
 }
 
-/* Returns 0 if no change occured since last check; 1 otherwise. */
+/* Returns 0 if no change occurred since last check; 1 otherwise. */
 static u8 bnx2x_analyze_link_error(struct link_params *params,
 				    struct link_vars *vars, u32 status,
 				    u32 phy_flag, u32 link_flag, u8 notify)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index bef750a09027..b9f85fccb419 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) ?
@@ -11546,13 +11556,13 @@ static void bnx2x_get_cnic_mac_hwinfo(struct bnx2x *bp)
 	/* Disable iSCSI OOO if MAC configuration is invalid. */
 	if (!is_valid_ether_addr(iscsi_mac)) {
 		bp->flags |= NO_ISCSI_OOO_FLAG | NO_ISCSI_FLAG;
-		memset(iscsi_mac, 0, ETH_ALEN);
+		eth_zero_addr(iscsi_mac);
 	}
 
 	/* Disable FCoE if MAC configuration is invalid. */
 	if (!is_valid_ether_addr(fip_mac)) {
 		bp->flags |= NO_FCOE_FLAG;
-		memset(bp->fip_mac, 0, ETH_ALEN);
+		eth_zero_addr(bp->fip_mac);
 	}
 }
 
@@ -11563,7 +11573,7 @@ static void bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 	int port = BP_PORT(bp);
 
 	/* Zero primary MAC configuration */
-	memset(bp->dev->dev_addr, 0, ETH_ALEN);
+	eth_zero_addr(bp->dev->dev_addr);
 
 	if (BP_NOMCP(bp)) {
 		BNX2X_ERROR("warning: random MAC workaround active\n");
@@ -11610,7 +11620,7 @@ static bool bnx2x_get_dropless_info(struct bnx2x *bp)
 	u32 cfg;
 
 	if (IS_VF(bp))
-		return 0;
+		return false;
 
 	if (IS_MF(bp) && !CHIP_IS_E1x(bp)) {
 		/* Take function: tmp = func */
@@ -11650,6 +11660,13 @@ static int bnx2x_get_hwinfo(struct bnx2x *bp)
 	u32 val = 0, val2 = 0;
 	int rc = 0;
 
+	/* Validate that chip access is feasible */
+	if (REG_RD(bp, MISC_REG_CHIP_NUM) == 0xffffffff) {
+		dev_err(&bp->pdev->dev,
+			"Chip read returns all Fs. Preventing probe from continuing\n");
+		return -EINVAL;
+	}
+
 	bnx2x_get_common_hwinfo(bp);
 
 	/*
@@ -12037,9 +12054,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);
@@ -12557,6 +12573,7 @@ static netdev_features_t bnx2x_features_check(struct sk_buff *skb,
 					      struct net_device *dev,
 					      netdev_features_t features)
 {
+	features = vlan_features_check(skb, features);
 	return vxlan_features_check(skb, features);
 }
 
@@ -12769,7 +12786,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 =
@@ -13278,30 +13295,27 @@ static int bnx2x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int bnx2x_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int bnx2x_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
 	u64 ns;
-	u32 remainder;
 
 	ns = timecounter_read(&bp->timecounter);
 
 	DP(BNX2X_MSG_PTP, "PTP gettime called, ns = %llu\n", ns);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
 
 static int bnx2x_ptp_settime(struct ptp_clock_info *ptp,
-			     const struct timespec *ts)
+			     const struct timespec64 *ts)
 {
 	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
 	u64 ns;
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 
 	DP(BNX2X_MSG_PTP, "PTP settime called, ns = %llu\n", ns);
 
@@ -13333,8 +13347,8 @@ static void bnx2x_register_phc(struct bnx2x *bp)
 	bp->ptp_clock_info.pps = 0;
 	bp->ptp_clock_info.adjfreq = bnx2x_ptp_adjfreq;
 	bp->ptp_clock_info.adjtime = bnx2x_ptp_adjtime;
-	bp->ptp_clock_info.gettime = bnx2x_ptp_gettime;
-	bp->ptp_clock_info.settime = bnx2x_ptp_settime;
+	bp->ptp_clock_info.gettime64 = bnx2x_ptp_gettime;
+	bp->ptp_clock_info.settime64 = bnx2x_ptp_settime;
 	bp->ptp_clock_info.enable = bnx2x_ptp_enable;
 
 	bp->ptp_clock = ptp_clock_register(&bp->ptp_clock_info, &bp->pdev->dev);
@@ -13668,9 +13682,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_reg.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
index 6fe547c93e74..49d511092c82 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
@@ -29,7 +29,7 @@
 #define ATC_ATC_INT_STS_REG_ATC_TCPL_TO_NOT_PEND		 (0x1<<1)
 /* [RW 1] Initiate the ATC array - reset all the valid bits */
 #define ATC_REG_ATC_INIT_ARRAY					 0x1100b8
-/* [R 1] ATC initalization done */
+/* [R 1] ATC initialization done */
 #define ATC_REG_ATC_INIT_DONE					 0x1100bc
 /* [RC 6] Interrupt register #0 read clear */
 #define ATC_REG_ATC_INT_STS_CLR					 0x1101c0
@@ -7341,6 +7341,8 @@ Theotherbitsarereservedandshouldbezero*/
 #define MDIO_WC_REG_TX2_ANA_CTRL0			0x8081
 #define MDIO_WC_REG_TX3_ANA_CTRL0			0x8091
 #define MDIO_WC_REG_TX0_TX_DRIVER			0x8067
+#define MDIO_WC_REG_TX0_TX_DRIVER_IFIR_OFFSET			0x01
+#define MDIO_WC_REG_TX0_TX_DRIVER_IFIR_MASK				0x000e
 #define MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET		0x04
 #define MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_MASK			0x00f0
 #define MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET		0x08
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index e5aca2de1871..f67348d16966 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -592,7 +592,7 @@ int bnx2x_vf_mcast(struct bnx2x *bp, struct bnx2x_virtf *vf,
 		mc = kzalloc(mc_num * sizeof(struct bnx2x_mcast_list_elem),
 			     GFP_KERNEL);
 		if (!mc) {
-			BNX2X_ERR("Cannot Configure mulicasts due to lack of memory\n");
+			BNX2X_ERR("Cannot Configure multicasts due to lack of memory\n");
 			return -ENOMEM;
 		}
 	}
@@ -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");
@@ -2693,7 +2695,7 @@ int bnx2x_get_vf_config(struct net_device *dev, int vfidx,
 			memcpy(&ivi->mac, bulletin->mac, ETH_ALEN);
 		else
 			/* function has not been loaded yet. Show mac as 0s */
-			memset(&ivi->mac, 0, ETH_ALEN);
+			eth_zero_addr(ivi->mac);
 
 		/* vlan */
 		if (bulletin->valid_bitmap & (1 << VLAN_VALID))
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
index d1608297c773..266b055c2360 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",
@@ -1620,7 +1583,7 @@ void bnx2x_memset_stats(struct bnx2x *bp)
 	if (bp->port.pmf && bp->port.port_stx)
 		bnx2x_port_stats_base_init(bp);
 
-	/* mark the end of statistics initializiation */
+	/* mark the end of statistics initialization */
 	bp->stats_init = false;
 }
 
@@ -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/bnx2x/bnx2x_vfpf.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
index be40eabc5304..15b2d1647560 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
@@ -800,7 +800,7 @@ int bnx2x_vfpf_config_rss(struct bnx2x *bp,
 	req->rss_key_size = T_ETH_RSS_KEY;
 	req->rss_result_mask = params->rss_result_mask;
 
-	/* flags handled individually for backward/forward compatability */
+	/* flags handled individually for backward/forward compatibility */
 	if (params->rss_flags & (1 << BNX2X_RSS_MODE_DISABLED))
 		req->rss_flags |= VFPF_RSS_MODE_DISABLED;
 	if (params->rss_flags & (1 << BNX2X_RSS_MODE_REGULAR))
@@ -1869,7 +1869,7 @@ static void bnx2x_vf_mbx_update_rss(struct bnx2x *bp, struct bnx2x_virtf *vf,
 	rss.rss_obj = &vf->rss_conf_obj;
 	rss.rss_result_mask = rss_tlv->rss_result_mask;
 
-	/* flags handled individually for backward/forward compatability */
+	/* flags handled individually for backward/forward compatibility */
 	rss.rss_flags = 0;
 	rss.ramrod_flags = 0;
 
diff --git a/drivers/net/ethernet/broadcom/cnic.c b/drivers/net/ethernet/broadcom/cnic.c
index f05fab65d78a..17c145fdf3ff 100644
--- a/drivers/net/ethernet/broadcom/cnic.c
+++ b/drivers/net/ethernet/broadcom/cnic.c
@@ -1,7 +1,7 @@
 /* cnic.c: QLogic CNIC core network driver.
  *
  * Copyright (c) 2006-2014 Broadcom Corporation
- * Copyright (c) 2014 QLogic Corporation
+ * Copyright (c) 2014-2015 QLogic 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
@@ -58,11 +58,11 @@
 #define CNIC_MODULE_NAME	"cnic"
 
 static char version[] =
-	"QLogic NetXtreme II CNIC Driver " CNIC_MODULE_NAME " v" CNIC_MODULE_VERSION " (" CNIC_MODULE_RELDATE ")\n";
+	"QLogic " CNIC_MODULE_NAME "Driver v" CNIC_MODULE_VERSION " (" CNIC_MODULE_RELDATE ")\n";
 
 MODULE_AUTHOR("Michael Chan <mchan@broadcom.com> and John(Zongxi) "
 	      "Chen (zongxi@broadcom.com");
-MODULE_DESCRIPTION("QLogic NetXtreme II CNIC Driver");
+MODULE_DESCRIPTION("QLogic cnic Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(CNIC_MODULE_VERSION);
 
diff --git a/drivers/net/ethernet/broadcom/cnic_if.h b/drivers/net/ethernet/broadcom/cnic_if.h
index 8bb36c1c4d68..ef6125b0ee3e 100644
--- a/drivers/net/ethernet/broadcom/cnic_if.h
+++ b/drivers/net/ethernet/broadcom/cnic_if.h
@@ -1,7 +1,7 @@
-/* cnic_if.h: QLogic CNIC core network driver.
+/* cnic_if.h: QLogic cnic core network driver.
  *
  * Copyright (c) 2006-2014 Broadcom Corporation
- * Copyright (c) 2014 QLogic Corporation
+ * Copyright (c) 2014-2015 QLogic 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
@@ -15,8 +15,8 @@
 
 #include "bnx2x/bnx2x_mfw_req.h"
 
-#define CNIC_MODULE_VERSION	"2.5.20"
-#define CNIC_MODULE_RELDATE	"March 14, 2014"
+#define CNIC_MODULE_VERSION	"2.5.21"
+#define CNIC_MODULE_RELDATE	"January 29, 2015"
 
 #define CNIC_ULP_RDMA		0
 #define CNIC_ULP_ISCSI		1
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index 6befde61c203..6043734ea613 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -54,8 +54,10 @@
 /* Default highest priority queue for multi queue support */
 #define GENET_Q0_PRIORITY	0
 
-#define GENET_DEFAULT_BD_CNT	\
-	(TOTAL_DESC - priv->hw_params->tx_queues * priv->hw_params->bds_cnt)
+#define GENET_Q16_RX_BD_CNT	\
+	(TOTAL_DESC - priv->hw_params->rx_queues * priv->hw_params->rx_bds_per_q)
+#define GENET_Q16_TX_BD_CNT	\
+	(TOTAL_DESC - priv->hw_params->tx_queues * priv->hw_params->tx_bds_per_q)
 
 #define RX_BUF_LENGTH		2048
 #define SKB_ALIGNMENT		32
@@ -195,6 +197,14 @@ enum dma_reg {
 	DMA_PRIORITY_0,
 	DMA_PRIORITY_1,
 	DMA_PRIORITY_2,
+	DMA_INDEX2RING_0,
+	DMA_INDEX2RING_1,
+	DMA_INDEX2RING_2,
+	DMA_INDEX2RING_3,
+	DMA_INDEX2RING_4,
+	DMA_INDEX2RING_5,
+	DMA_INDEX2RING_6,
+	DMA_INDEX2RING_7,
 };
 
 static const u8 bcmgenet_dma_regs_v3plus[] = {
@@ -206,6 +216,14 @@ static const u8 bcmgenet_dma_regs_v3plus[] = {
 	[DMA_PRIORITY_0]	= 0x30,
 	[DMA_PRIORITY_1]	= 0x34,
 	[DMA_PRIORITY_2]	= 0x38,
+	[DMA_INDEX2RING_0]	= 0x70,
+	[DMA_INDEX2RING_1]	= 0x74,
+	[DMA_INDEX2RING_2]	= 0x78,
+	[DMA_INDEX2RING_3]	= 0x7C,
+	[DMA_INDEX2RING_4]	= 0x80,
+	[DMA_INDEX2RING_5]	= 0x84,
+	[DMA_INDEX2RING_6]	= 0x88,
+	[DMA_INDEX2RING_7]	= 0x8C,
 };
 
 static const u8 bcmgenet_dma_regs_v2[] = {
@@ -829,9 +847,10 @@ static struct ethtool_ops bcmgenet_ethtool_ops = {
 };
 
 /* Power down the unimac, based on mode. */
-static void bcmgenet_power_down(struct bcmgenet_priv *priv,
+static int bcmgenet_power_down(struct bcmgenet_priv *priv,
 				enum bcmgenet_power_mode mode)
 {
+	int ret = 0;
 	u32 reg;
 
 	switch (mode) {
@@ -840,7 +859,7 @@ static void bcmgenet_power_down(struct bcmgenet_priv *priv,
 		break;
 
 	case GENET_POWER_WOL_MAGIC:
-		bcmgenet_wol_power_down_cfg(priv, mode);
+		ret = bcmgenet_wol_power_down_cfg(priv, mode);
 		break;
 
 	case GENET_POWER_PASSIVE:
@@ -850,11 +869,15 @@ static void bcmgenet_power_down(struct bcmgenet_priv *priv,
 			reg |= (EXT_PWR_DOWN_PHY |
 				EXT_PWR_DOWN_DLL | EXT_PWR_DOWN_BIAS);
 			bcmgenet_ext_writel(priv, reg, EXT_EXT_PWR_MGMT);
+
+			bcmgenet_phy_power_set(priv->dev, false);
 		}
 		break;
 	default:
 		break;
 	}
+
+	return 0;
 }
 
 static void bcmgenet_power_up(struct bcmgenet_priv *priv,
@@ -923,7 +946,7 @@ static struct enet_cb *bcmgenet_get_txcb(struct bcmgenet_priv *priv,
 
 	tx_cb_ptr = ring->cbs;
 	tx_cb_ptr += ring->write_ptr - ring->cb_ptr;
-	tx_cb_ptr->bd_addr = priv->tx_bds + ring->write_ptr * DMA_DESC_SIZE;
+
 	/* Advancing local write pointer */
 	if (ring->write_ptr == ring->end_ptr)
 		ring->write_ptr = ring->cb_ptr;
@@ -941,36 +964,54 @@ static void bcmgenet_free_cb(struct enet_cb *cb)
 	dma_unmap_addr_set(cb, dma_addr, 0);
 }
 
-static inline void bcmgenet_tx_ring16_int_disable(struct bcmgenet_priv *priv,
-						  struct bcmgenet_tx_ring *ring)
+static inline void bcmgenet_rx_ring16_int_disable(struct bcmgenet_rx_ring *ring)
 {
-	bcmgenet_intrl2_0_writel(priv,
-				 UMAC_IRQ_TXDMA_BDONE | UMAC_IRQ_TXDMA_PDONE,
+	bcmgenet_intrl2_0_writel(ring->priv, UMAC_IRQ_RXDMA_DONE,
 				 INTRL2_CPU_MASK_SET);
 }
 
-static inline void bcmgenet_tx_ring16_int_enable(struct bcmgenet_priv *priv,
-						 struct bcmgenet_tx_ring *ring)
+static inline void bcmgenet_rx_ring16_int_enable(struct bcmgenet_rx_ring *ring)
 {
-	bcmgenet_intrl2_0_writel(priv,
-				 UMAC_IRQ_TXDMA_BDONE | UMAC_IRQ_TXDMA_PDONE,
+	bcmgenet_intrl2_0_writel(ring->priv, UMAC_IRQ_RXDMA_DONE,
 				 INTRL2_CPU_MASK_CLEAR);
 }
 
-static inline void bcmgenet_tx_ring_int_enable(struct bcmgenet_priv *priv,
-					       struct bcmgenet_tx_ring *ring)
+static inline void bcmgenet_rx_ring_int_disable(struct bcmgenet_rx_ring *ring)
+{
+	bcmgenet_intrl2_1_writel(ring->priv,
+				 1 << (UMAC_IRQ1_RX_INTR_SHIFT + ring->index),
+				 INTRL2_CPU_MASK_SET);
+}
+
+static inline void bcmgenet_rx_ring_int_enable(struct bcmgenet_rx_ring *ring)
 {
-	bcmgenet_intrl2_1_writel(priv, (1 << ring->index),
+	bcmgenet_intrl2_1_writel(ring->priv,
+				 1 << (UMAC_IRQ1_RX_INTR_SHIFT + ring->index),
 				 INTRL2_CPU_MASK_CLEAR);
-	priv->int1_mask &= ~(1 << ring->index);
 }
 
-static inline void bcmgenet_tx_ring_int_disable(struct bcmgenet_priv *priv,
-						struct bcmgenet_tx_ring *ring)
+static inline void bcmgenet_tx_ring16_int_disable(struct bcmgenet_tx_ring *ring)
 {
-	bcmgenet_intrl2_1_writel(priv, (1 << ring->index),
+	bcmgenet_intrl2_0_writel(ring->priv, UMAC_IRQ_TXDMA_DONE,
+				 INTRL2_CPU_MASK_SET);
+}
+
+static inline void bcmgenet_tx_ring16_int_enable(struct bcmgenet_tx_ring *ring)
+{
+	bcmgenet_intrl2_0_writel(ring->priv, UMAC_IRQ_TXDMA_DONE,
+				 INTRL2_CPU_MASK_CLEAR);
+}
+
+static inline void bcmgenet_tx_ring_int_enable(struct bcmgenet_tx_ring *ring)
+{
+	bcmgenet_intrl2_1_writel(ring->priv, 1 << ring->index,
+				 INTRL2_CPU_MASK_CLEAR);
+}
+
+static inline void bcmgenet_tx_ring_int_disable(struct bcmgenet_tx_ring *ring)
+{
+	bcmgenet_intrl2_1_writel(ring->priv, 1 << ring->index,
 				 INTRL2_CPU_MASK_SET);
-	priv->int1_mask |= (1 << ring->index);
 }
 
 /* Unlocked version of the reclaim routine */
@@ -978,39 +1019,32 @@ 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;
+	unsigned int txbds_ready;
+	unsigned int txbds_processed = 0;
 
 	/* Compute how many buffers are transmitted since last xmit call */
 	c_index = bcmgenet_tdma_ring_readl(priv, ring->index, TDMA_CONS_INDEX);
-	txq = netdev_get_tx_queue(dev, ring->queue);
-
-	last_c_index = ring->c_index;
-	num_tx_bds = ring->size;
+	c_index &= DMA_C_INDEX_MASK;
 
-	c_index &= (num_tx_bds - 1);
-
-	if (c_index >= last_c_index)
-		last_tx_cn = c_index - last_c_index;
+	if (likely(c_index >= ring->c_index))
+		txbds_ready = c_index - ring->c_index;
 	else
-		last_tx_cn = num_tx_bds - last_c_index + c_index;
+		txbds_ready = (DMA_C_INDEX_MASK + 1) - ring->c_index + c_index;
 
 	netif_dbg(priv, tx_done, dev,
-		  "%s ring=%d index=%d last_tx_cn=%d last_index=%d\n",
-		  __func__, ring->index,
-		  c_index, last_tx_cn, last_c_index);
+		  "%s ring=%d old_c_index=%u c_index=%u txbds_ready=%u\n",
+		  __func__, ring->index, ring->c_index, c_index, txbds_ready);
 
 	/* Reclaim transmitted buffers */
-	while (last_tx_cn-- > 0) {
-		tx_cb_ptr = ring->cbs + last_c_index;
-		bds_compl = 0;
+	while (txbds_processed < txbds_ready) {
+		tx_cb_ptr = &priv->tx_cbs[ring->clean_ptr];
 		if (tx_cb_ptr->skb) {
 			pkts_compl++;
-			bds_compl = skb_shinfo(tx_cb_ptr->skb)->nr_frags + 1;
+			dev->stats.tx_packets++;
 			dev->stats.tx_bytes += tx_cb_ptr->skb->len;
 			dma_unmap_single(&dev->dev,
 					 dma_unmap_addr(tx_cb_ptr, dma_addr),
@@ -1026,20 +1060,23 @@ static unsigned int __bcmgenet_tx_reclaim(struct net_device *dev,
 				       DMA_TO_DEVICE);
 			dma_unmap_addr_set(tx_cb_ptr, dma_addr, 0);
 		}
-		dev->stats.tx_packets++;
-		ring->free_bds += bds_compl;
 
-		last_c_index++;
-		last_c_index &= (num_tx_bds - 1);
+		txbds_processed++;
+		if (likely(ring->clean_ptr < ring->end_ptr))
+			ring->clean_ptr++;
+		else
+			ring->clean_ptr = ring->cb_ptr;
 	}
 
+	ring->free_bds += txbds_processed;
+	ring->c_index = (ring->c_index + txbds_processed) & DMA_C_INDEX_MASK;
+
 	if (ring->free_bds > (MAX_SKB_FRAGS + 1)) {
+		txq = netdev_get_tx_queue(dev, ring->queue);
 		if (netif_tx_queue_stopped(txq))
 			netif_tx_wake_queue(txq);
 	}
 
-	ring->c_index = c_index;
-
 	return pkts_compl;
 }
 
@@ -1066,7 +1103,7 @@ static int bcmgenet_tx_poll(struct napi_struct *napi, int budget)
 
 	if (work_done == 0) {
 		napi_complete(napi);
-		ring->int_enable(ring->priv, ring);
+		ring->int_enable(ring);
 
 		return 0;
 	}
@@ -1132,11 +1169,6 @@ static int bcmgenet_xmit_single(struct net_device *dev,
 
 	dmadesc_set(priv, tx_cb_ptr->bd_addr, mapping, length_status);
 
-	/* Decrement total BD count and advance our write pointer */
-	ring->free_bds -= 1;
-	ring->prod_index += 1;
-	ring->prod_index &= DMA_P_INDEX_MASK;
-
 	return 0;
 }
 
@@ -1175,11 +1207,6 @@ static int bcmgenet_xmit_frag(struct net_device *dev,
 		    (frag->size << DMA_BUFLENGTH_SHIFT) | dma_desc_flags |
 		    (priv->hw_params->qtag_mask << DMA_TX_QTAG_SHIFT));
 
-
-	ring->free_bds -= 1;
-	ring->prod_index += 1;
-	ring->prod_index &= DMA_P_INDEX_MASK;
-
 	return 0;
 }
 
@@ -1323,119 +1350,128 @@ static netdev_tx_t bcmgenet_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	skb_tx_timestamp(skb);
 
-	/* we kept a software copy of how much we should advance the TDMA
-	 * producer index, now write it down to the hardware
-	 */
-	bcmgenet_tdma_ring_writel(priv, ring->index,
-				  ring->prod_index, TDMA_PROD_INDEX);
+	/* Decrement total BD count and advance our write pointer */
+	ring->free_bds -= nr_frags + 1;
+	ring->prod_index += nr_frags + 1;
+	ring->prod_index &= DMA_P_INDEX_MASK;
 
 	if (ring->free_bds <= (MAX_SKB_FRAGS + 1))
 		netif_tx_stop_queue(txq);
 
+	if (!skb->xmit_more || netif_xmit_stopped(txq))
+		/* Packets are ready, update producer index */
+		bcmgenet_tdma_ring_writel(priv, ring->index,
+					  ring->prod_index, TDMA_PROD_INDEX);
 out:
 	spin_unlock_irqrestore(&ring->lock, flags);
 
 	return ret;
 }
 
-
-static int bcmgenet_rx_refill(struct bcmgenet_priv *priv, struct enet_cb *cb)
+static struct sk_buff *bcmgenet_rx_refill(struct bcmgenet_priv *priv,
+					  struct enet_cb *cb)
 {
 	struct device *kdev = &priv->pdev->dev;
 	struct sk_buff *skb;
+	struct sk_buff *rx_skb;
 	dma_addr_t mapping;
-	int ret;
 
+	/* Allocate a new Rx skb */
 	skb = netdev_alloc_skb(priv->dev, priv->rx_buf_len + SKB_ALIGNMENT);
-	if (!skb)
-		return -ENOMEM;
+	if (!skb) {
+		priv->mib.alloc_rx_buff_failed++;
+		netif_err(priv, rx_err, priv->dev,
+			  "%s: Rx skb allocation failed\n", __func__);
+		return NULL;
+	}
 
-	/* a caller did not release this control block */
-	WARN_ON(cb->skb != NULL);
-	cb->skb = skb;
-	mapping = dma_map_single(kdev, skb->data,
-				 priv->rx_buf_len, DMA_FROM_DEVICE);
-	ret = dma_mapping_error(kdev, mapping);
-	if (ret) {
+	/* DMA-map the new Rx skb */
+	mapping = dma_map_single(kdev, skb->data, priv->rx_buf_len,
+				 DMA_FROM_DEVICE);
+	if (dma_mapping_error(kdev, mapping)) {
 		priv->mib.rx_dma_failed++;
-		bcmgenet_free_cb(cb);
+		dev_kfree_skb_any(skb);
 		netif_err(priv, rx_err, priv->dev,
-			  "%s DMA map failed\n", __func__);
-		return ret;
+			  "%s: Rx skb DMA mapping failed\n", __func__);
+		return NULL;
 	}
 
-	dma_unmap_addr_set(cb, dma_addr, mapping);
-	/* assign packet, prepare descriptor, and advance pointer */
-
-	dmadesc_set_addr(priv, priv->rx_bd_assign_ptr, mapping);
-
-	/* turn on the newly assigned BD for DMA to use */
-	priv->rx_bd_assign_index++;
-	priv->rx_bd_assign_index &= (priv->num_rx_bds - 1);
+	/* Grab the current Rx skb from the ring and DMA-unmap it */
+	rx_skb = cb->skb;
+	if (likely(rx_skb))
+		dma_unmap_single(kdev, dma_unmap_addr(cb, dma_addr),
+				 priv->rx_buf_len, DMA_FROM_DEVICE);
 
-	priv->rx_bd_assign_ptr = priv->rx_bds +
-		(priv->rx_bd_assign_index * DMA_DESC_SIZE);
+	/* Put the new Rx skb on the ring */
+	cb->skb = skb;
+	dma_unmap_addr_set(cb, dma_addr, mapping);
+	dmadesc_set_addr(priv, cb->bd_addr, mapping);
 
-	return 0;
+	/* Return the current Rx skb to caller */
+	return rx_skb;
 }
 
 /* bcmgenet_desc_rx - descriptor based rx process.
  * this could be called from bottom half, or from NAPI polling method.
  */
-static unsigned int bcmgenet_desc_rx(struct bcmgenet_priv *priv,
+static unsigned int bcmgenet_desc_rx(struct bcmgenet_rx_ring *ring,
 				     unsigned int budget)
 {
+	struct bcmgenet_priv *priv = ring->priv;
 	struct net_device *dev = priv->dev;
 	struct enet_cb *cb;
 	struct sk_buff *skb;
 	u32 dma_length_status;
 	unsigned long dma_flag;
-	int len, err;
+	int len;
 	unsigned int rxpktprocessed = 0, rxpkttoprocess;
 	unsigned int p_index;
+	unsigned int discards;
 	unsigned int chksum_ok = 0;
 
-	p_index = bcmgenet_rdma_ring_readl(priv, DESC_INDEX, RDMA_PROD_INDEX);
+	p_index = bcmgenet_rdma_ring_readl(priv, ring->index, RDMA_PROD_INDEX);
+
+	discards = (p_index >> DMA_P_INDEX_DISCARD_CNT_SHIFT) &
+		   DMA_P_INDEX_DISCARD_CNT_MASK;
+	if (discards > ring->old_discards) {
+		discards = discards - ring->old_discards;
+		dev->stats.rx_missed_errors += discards;
+		dev->stats.rx_errors += discards;
+		ring->old_discards += discards;
+
+		/* Clear HW register when we reach 75% of maximum 0xFFFF */
+		if (ring->old_discards >= 0xC000) {
+			ring->old_discards = 0;
+			bcmgenet_rdma_ring_writel(priv, ring->index, 0,
+						  RDMA_PROD_INDEX);
+		}
+	}
+
 	p_index &= DMA_P_INDEX_MASK;
 
-	if (p_index < priv->rx_c_index)
-		rxpkttoprocess = (DMA_C_INDEX_MASK + 1) -
-			priv->rx_c_index + p_index;
+	if (likely(p_index >= ring->c_index))
+		rxpkttoprocess = p_index - ring->c_index;
 	else
-		rxpkttoprocess = p_index - priv->rx_c_index;
+		rxpkttoprocess = (DMA_C_INDEX_MASK + 1) - ring->c_index +
+				 p_index;
 
 	netif_dbg(priv, rx_status, dev,
 		  "RDMA: rxpkttoprocess=%d\n", rxpkttoprocess);
 
 	while ((rxpktprocessed < rxpkttoprocess) &&
 	       (rxpktprocessed < budget)) {
-		cb = &priv->rx_cbs[priv->rx_read_ptr];
-		skb = cb->skb;
+		cb = &priv->rx_cbs[ring->read_ptr];
+		skb = bcmgenet_rx_refill(priv, cb);
 
-		/* We do not have a backing SKB, so we do not have a
-		 * corresponding DMA mapping for this incoming packet since
-		 * bcmgenet_rx_refill always either has both skb and mapping or
-		 * none.
-		 */
 		if (unlikely(!skb)) {
 			dev->stats.rx_dropped++;
 			dev->stats.rx_errors++;
-			goto refill;
+			goto next;
 		}
 
-		/* Unmap the packet contents such that we can use the
-		 * RSV from the 64 bytes descriptor when enabled and save
-		 * a 32-bits register read
-		 */
-		dma_unmap_single(&dev->dev, dma_unmap_addr(cb, dma_addr),
-				 priv->rx_buf_len, DMA_FROM_DEVICE);
-
 		if (!priv->desc_64b_en) {
 			dma_length_status =
-				dmadesc_get_length_status(priv,
-							  priv->rx_bds +
-							  (priv->rx_read_ptr *
-							   DMA_DESC_SIZE));
+				dmadesc_get_length_status(priv, cb->bd_addr);
 		} else {
 			struct status_64 *status;
 
@@ -1451,18 +1487,18 @@ static unsigned int bcmgenet_desc_rx(struct bcmgenet_priv *priv,
 
 		netif_dbg(priv, rx_status, dev,
 			  "%s:p_ind=%d c_ind=%d read_ptr=%d len_stat=0x%08x\n",
-			  __func__, p_index, priv->rx_c_index,
-			  priv->rx_read_ptr, dma_length_status);
+			  __func__, p_index, ring->c_index,
+			  ring->read_ptr, dma_length_status);
 
 		if (unlikely(!(dma_flag & DMA_EOP) || !(dma_flag & DMA_SOP))) {
 			netif_err(priv, rx_status, dev,
 				  "dropping fragmented packet!\n");
 			dev->stats.rx_dropped++;
 			dev->stats.rx_errors++;
-			dev_kfree_skb_any(cb->skb);
-			cb->skb = NULL;
-			goto refill;
+			dev_kfree_skb_any(skb);
+			goto next;
 		}
+
 		/* report errors */
 		if (unlikely(dma_flag & (DMA_RX_CRC_ERROR |
 						DMA_RX_OV |
@@ -1481,11 +1517,8 @@ static unsigned int bcmgenet_desc_rx(struct bcmgenet_priv *priv,
 				dev->stats.rx_length_errors++;
 			dev->stats.rx_dropped++;
 			dev->stats.rx_errors++;
-
-			/* discard the packet and advance consumer index.*/
-			dev_kfree_skb_any(cb->skb);
-			cb->skb = NULL;
-			goto refill;
+			dev_kfree_skb_any(skb);
+			goto next;
 		} /* error packet */
 
 		chksum_ok = (dma_flag & priv->dma_rx_chk_bit) &&
@@ -1517,47 +1550,61 @@ static unsigned int bcmgenet_desc_rx(struct bcmgenet_priv *priv,
 			dev->stats.multicast++;
 
 		/* Notify kernel */
-		napi_gro_receive(&priv->napi, skb);
-		cb->skb = NULL;
+		napi_gro_receive(&ring->napi, skb);
 		netif_dbg(priv, rx_status, dev, "pushed up to kernel\n");
 
-		/* refill RX path on the current control block */
-refill:
-		err = bcmgenet_rx_refill(priv, cb);
-		if (err) {
-			priv->mib.alloc_rx_buff_failed++;
-			netif_err(priv, rx_err, dev, "Rx refill failed\n");
-		}
-
+next:
 		rxpktprocessed++;
-		priv->rx_read_ptr++;
-		priv->rx_read_ptr &= (priv->num_rx_bds - 1);
+		if (likely(ring->read_ptr < ring->end_ptr))
+			ring->read_ptr++;
+		else
+			ring->read_ptr = ring->cb_ptr;
+
+		ring->c_index = (ring->c_index + 1) & DMA_C_INDEX_MASK;
+		bcmgenet_rdma_ring_writel(priv, ring->index, ring->c_index, RDMA_CONS_INDEX);
 	}
 
 	return rxpktprocessed;
 }
 
+/* Rx NAPI polling method */
+static int bcmgenet_rx_poll(struct napi_struct *napi, int budget)
+{
+	struct bcmgenet_rx_ring *ring = container_of(napi,
+			struct bcmgenet_rx_ring, napi);
+	unsigned int work_done;
+
+	work_done = bcmgenet_desc_rx(ring, budget);
+
+	if (work_done < budget) {
+		napi_complete(napi);
+		ring->int_enable(ring);
+	}
+
+	return work_done;
+}
+
 /* Assign skb to RX DMA descriptor. */
-static int bcmgenet_alloc_rx_buffers(struct bcmgenet_priv *priv)
+static int bcmgenet_alloc_rx_buffers(struct bcmgenet_priv *priv,
+				     struct bcmgenet_rx_ring *ring)
 {
 	struct enet_cb *cb;
-	int ret = 0;
+	struct sk_buff *skb;
 	int i;
 
-	netif_dbg(priv, hw, priv->dev, "%s:\n", __func__);
+	netif_dbg(priv, hw, priv->dev, "%s\n", __func__);
 
 	/* loop here for each buffer needing assign */
-	for (i = 0; i < priv->num_rx_bds; i++) {
-		cb = &priv->rx_cbs[priv->rx_bd_assign_index];
-		if (cb->skb)
-			continue;
-
-		ret = bcmgenet_rx_refill(priv, cb);
-		if (ret)
-			break;
+	for (i = 0; i < ring->size; i++) {
+		cb = ring->cbs + i;
+		skb = bcmgenet_rx_refill(priv, cb);
+		if (skb)
+			dev_kfree_skb_any(skb);
+		if (!cb->skb)
+			return -ENOMEM;
 	}
 
-	return ret;
+	return 0;
 }
 
 static void bcmgenet_free_rx_buffers(struct bcmgenet_priv *priv)
@@ -1645,8 +1692,10 @@ static int init_umac(struct bcmgenet_priv *priv)
 {
 	struct device *kdev = &priv->pdev->dev;
 	int ret;
-	u32 reg, cpu_mask_clear;
-	int index;
+	u32 reg;
+	u32 int0_enable = 0;
+	u32 int1_enable = 0;
+	int i;
 
 	dev_dbg(&priv->pdev->dev, "bcmgenet: init_umac\n");
 
@@ -1673,16 +1722,21 @@ static int init_umac(struct bcmgenet_priv *priv)
 
 	bcmgenet_intr_disable(priv);
 
-	cpu_mask_clear = UMAC_IRQ_RXDMA_BDONE | UMAC_IRQ_TXDMA_BDONE;
+	/* Enable Rx default queue 16 interrupts */
+	int0_enable |= UMAC_IRQ_RXDMA_DONE;
 
-	dev_dbg(kdev, "%s:Enabling RXDMA_BDONE interrupt\n", __func__);
+	/* Enable Tx default queue 16 interrupts */
+	int0_enable |= UMAC_IRQ_TXDMA_DONE;
 
 	/* Monitor cable plug/unplugged event for internal PHY */
 	if (phy_is_internal(priv->phydev)) {
-		cpu_mask_clear |= (UMAC_IRQ_LINK_DOWN | UMAC_IRQ_LINK_UP);
+		int0_enable |= UMAC_IRQ_LINK_EVENT;
 	} else if (priv->ext_phy) {
-		cpu_mask_clear |= (UMAC_IRQ_LINK_DOWN | UMAC_IRQ_LINK_UP);
+		int0_enable |= UMAC_IRQ_LINK_EVENT;
 	} else if (priv->phy_interface == PHY_INTERFACE_MODE_MOCA) {
+		if (priv->hw_params->flags & GENET_HAS_MOCA_LINK_DET)
+			int0_enable |= UMAC_IRQ_LINK_EVENT;
+
 		reg = bcmgenet_bp_mc_get(priv);
 		reg |= BIT(priv->hw_params->bp_in_en_shift);
 
@@ -1696,13 +1750,18 @@ static int init_umac(struct bcmgenet_priv *priv)
 
 	/* Enable MDIO interrupts on GENET v3+ */
 	if (priv->hw_params->flags & GENET_HAS_MDIO_INTR)
-		cpu_mask_clear |= UMAC_IRQ_MDIO_DONE | UMAC_IRQ_MDIO_ERROR;
+		int0_enable |= (UMAC_IRQ_MDIO_DONE | UMAC_IRQ_MDIO_ERROR);
 
-	bcmgenet_intrl2_0_writel(priv, cpu_mask_clear, INTRL2_CPU_MASK_CLEAR);
+	/* Enable Rx priority queue interrupts */
+	for (i = 0; i < priv->hw_params->rx_queues; ++i)
+		int1_enable |= (1 << (UMAC_IRQ1_RX_INTR_SHIFT + i));
 
-	for (index = 0; index < priv->hw_params->tx_queues; index++)
-		bcmgenet_intrl2_1_writel(priv, (1 << index),
-					 INTRL2_CPU_MASK_CLEAR);
+	/* Enable Tx priority queue interrupts */
+	for (i = 0; i < priv->hw_params->tx_queues; ++i)
+		int1_enable |= (1 << i);
+
+	bcmgenet_intrl2_0_writel(priv, int0_enable, INTRL2_CPU_MASK_CLEAR);
+	bcmgenet_intrl2_1_writel(priv, int1_enable, INTRL2_CPU_MASK_CLEAR);
 
 	/* Enable rx/tx engine.*/
 	dev_dbg(kdev, "done init umac\n");
@@ -1710,21 +1769,17 @@ static int init_umac(struct bcmgenet_priv *priv)
 	return 0;
 }
 
-/* Initialize all house-keeping variables for a TX ring, along
- * with corresponding hardware registers
- */
+/* Initialize a Tx ring along with corresponding hardware registers */
 static void bcmgenet_init_tx_ring(struct bcmgenet_priv *priv,
 				  unsigned int index, unsigned int size,
-				  unsigned int write_ptr, unsigned int end_ptr)
+				  unsigned int start_ptr, unsigned int end_ptr)
 {
 	struct bcmgenet_tx_ring *ring = &priv->tx_rings[index];
 	u32 words_per_bd = WORDS_PER_BD(priv);
 	u32 flow_period_val = 0;
-	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;
@@ -1735,12 +1790,13 @@ static void bcmgenet_init_tx_ring(struct bcmgenet_priv *priv,
 		ring->int_enable = bcmgenet_tx_ring_int_enable;
 		ring->int_disable = bcmgenet_tx_ring_int_disable;
 	}
-	ring->cbs = priv->tx_cbs + write_ptr;
+	ring->cbs = priv->tx_cbs + start_ptr;
 	ring->size = size;
+	ring->clean_ptr = start_ptr;
 	ring->c_index = 0;
 	ring->free_bds = size;
-	ring->write_ptr = write_ptr;
-	ring->cb_ptr = write_ptr;
+	ring->write_ptr = start_ptr;
+	ring->cb_ptr = start_ptr;
 	ring->end_ptr = end_ptr - 1;
 	ring->prod_index = 0;
 
@@ -1754,149 +1810,319 @@ static void bcmgenet_init_tx_ring(struct bcmgenet_priv *priv,
 	/* Disable rate control for now */
 	bcmgenet_tdma_ring_writel(priv, index, flow_period_val,
 				  TDMA_FLOW_PERIOD);
-	/* Unclassified traffic goes to ring 16 */
 	bcmgenet_tdma_ring_writel(priv, index,
 				  ((size << DMA_RING_SIZE_SHIFT) |
 				   RX_BUF_LENGTH), DMA_RING_BUF_SIZE);
 
-	first_bd = write_ptr;
-
 	/* Set start and end address, read and write pointers */
-	bcmgenet_tdma_ring_writel(priv, index, first_bd * words_per_bd,
+	bcmgenet_tdma_ring_writel(priv, index, start_ptr * words_per_bd,
 				  DMA_START_ADDR);
-	bcmgenet_tdma_ring_writel(priv, index, first_bd * words_per_bd,
+	bcmgenet_tdma_ring_writel(priv, index, start_ptr * words_per_bd,
 				  TDMA_READ_PTR);
-	bcmgenet_tdma_ring_writel(priv, index, first_bd,
+	bcmgenet_tdma_ring_writel(priv, index, start_ptr * words_per_bd,
 				  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 */
 static int bcmgenet_init_rx_ring(struct bcmgenet_priv *priv,
-				 unsigned int index, unsigned int size)
+				 unsigned int index, unsigned int size,
+				 unsigned int start_ptr, unsigned int end_ptr)
 {
+	struct bcmgenet_rx_ring *ring = &priv->rx_rings[index];
 	u32 words_per_bd = WORDS_PER_BD(priv);
 	int ret;
 
-	priv->num_rx_bds = TOTAL_DESC;
-	priv->rx_bds = priv->base + priv->hw_params->rdma_offset;
-	priv->rx_bd_assign_ptr = priv->rx_bds;
-	priv->rx_bd_assign_index = 0;
-	priv->rx_c_index = 0;
-	priv->rx_read_ptr = 0;
-	priv->rx_cbs = kcalloc(priv->num_rx_bds, sizeof(struct enet_cb),
-			       GFP_KERNEL);
-	if (!priv->rx_cbs)
-		return -ENOMEM;
+	ring->priv = priv;
+	ring->index = index;
+	if (index == DESC_INDEX) {
+		ring->int_enable = bcmgenet_rx_ring16_int_enable;
+		ring->int_disable = bcmgenet_rx_ring16_int_disable;
+	} else {
+		ring->int_enable = bcmgenet_rx_ring_int_enable;
+		ring->int_disable = bcmgenet_rx_ring_int_disable;
+	}
+	ring->cbs = priv->rx_cbs + start_ptr;
+	ring->size = size;
+	ring->c_index = 0;
+	ring->read_ptr = start_ptr;
+	ring->cb_ptr = start_ptr;
+	ring->end_ptr = end_ptr - 1;
 
-	ret = bcmgenet_alloc_rx_buffers(priv);
-	if (ret) {
-		kfree(priv->rx_cbs);
+	ret = bcmgenet_alloc_rx_buffers(priv, ring);
+	if (ret)
 		return ret;
-	}
 
-	bcmgenet_rdma_ring_writel(priv, index, 0, RDMA_WRITE_PTR);
 	bcmgenet_rdma_ring_writel(priv, index, 0, RDMA_PROD_INDEX);
 	bcmgenet_rdma_ring_writel(priv, index, 0, RDMA_CONS_INDEX);
+	bcmgenet_rdma_ring_writel(priv, index, 1, DMA_MBUF_DONE_THRESH);
 	bcmgenet_rdma_ring_writel(priv, index,
 				  ((size << DMA_RING_SIZE_SHIFT) |
 				   RX_BUF_LENGTH), DMA_RING_BUF_SIZE);
-	bcmgenet_rdma_ring_writel(priv, index, 0, DMA_START_ADDR);
-	bcmgenet_rdma_ring_writel(priv, index,
-				  words_per_bd * size - 1, DMA_END_ADDR);
 	bcmgenet_rdma_ring_writel(priv, index,
 				  (DMA_FC_THRESH_LO <<
 				   DMA_XOFF_THRESHOLD_SHIFT) |
 				   DMA_FC_THRESH_HI, RDMA_XON_XOFF_THRESH);
-	bcmgenet_rdma_ring_writel(priv, index, 0, RDMA_READ_PTR);
+
+	/* Set start and end address, read and write pointers */
+	bcmgenet_rdma_ring_writel(priv, index, start_ptr * words_per_bd,
+				  DMA_START_ADDR);
+	bcmgenet_rdma_ring_writel(priv, index, start_ptr * words_per_bd,
+				  RDMA_READ_PTR);
+	bcmgenet_rdma_ring_writel(priv, index, start_ptr * words_per_bd,
+				  RDMA_WRITE_PTR);
+	bcmgenet_rdma_ring_writel(priv, index, end_ptr * words_per_bd - 1,
+				  DMA_END_ADDR);
 
 	return ret;
 }
 
-/* init multi xmit queues, only available for GENET2+
- * the queue is partitioned as follows:
+static void bcmgenet_init_tx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_tx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->tx_queues; ++i) {
+		ring = &priv->tx_rings[i];
+		netif_napi_add(priv->dev, &ring->napi, bcmgenet_tx_poll, 64);
+	}
+
+	ring = &priv->tx_rings[DESC_INDEX];
+	netif_napi_add(priv->dev, &ring->napi, bcmgenet_tx_poll, 64);
+}
+
+static void bcmgenet_enable_tx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_tx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->tx_queues; ++i) {
+		ring = &priv->tx_rings[i];
+		napi_enable(&ring->napi);
+	}
+
+	ring = &priv->tx_rings[DESC_INDEX];
+	napi_enable(&ring->napi);
+}
+
+static void bcmgenet_disable_tx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_tx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->tx_queues; ++i) {
+		ring = &priv->tx_rings[i];
+		napi_disable(&ring->napi);
+	}
+
+	ring = &priv->tx_rings[DESC_INDEX];
+	napi_disable(&ring->napi);
+}
+
+static void bcmgenet_fini_tx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_tx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->tx_queues; ++i) {
+		ring = &priv->tx_rings[i];
+		netif_napi_del(&ring->napi);
+	}
+
+	ring = &priv->tx_rings[DESC_INDEX];
+	netif_napi_del(&ring->napi);
+}
+
+/* Initialize Tx queues
  *
- * queue 0 - 3 is priority based, each one has 32 descriptors,
+ * Queues 0-3 are priority-based, each one has 32 descriptors,
  * with queue 0 being the highest priority queue.
  *
- * queue 16 is the default tx queue with GENET_DEFAULT_BD_CNT
- * descriptors: 256 - (number of tx queues * bds per queues) = 128
- * descriptors.
+ * Queue 16 is the default Tx queue with
+ * GENET_Q16_TX_BD_CNT = 256 - 4 * 32 = 128 descriptors.
  *
- * The transmit control block pool is then partitioned as following:
- * - tx_cbs[0...127] are for queue 16
- * - tx_ring_cbs[0] points to tx_cbs[128..159]
- * - tx_ring_cbs[1] points to tx_cbs[160..191]
- * - tx_ring_cbs[2] points to tx_cbs[192..223]
- * - tx_ring_cbs[3] points to tx_cbs[224..255]
+ * The transmit control block pool is then partitioned as follows:
+ * - Tx queue 0 uses tx_cbs[0..31]
+ * - Tx queue 1 uses tx_cbs[32..63]
+ * - Tx queue 2 uses tx_cbs[64..95]
+ * - Tx queue 3 uses tx_cbs[96..127]
+ * - Tx queue 16 uses tx_cbs[128..255]
  */
-static void bcmgenet_init_multiq(struct net_device *dev)
+static void bcmgenet_init_tx_queues(struct net_device *dev)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
-	unsigned int i, dma_enable;
-	u32 reg, dma_ctrl, ring_cfg = 0;
+	u32 i, dma_enable;
+	u32 dma_ctrl, ring_cfg;
 	u32 dma_priority[3] = {0, 0, 0};
 
-	if (!netif_is_multiqueue(dev)) {
-		netdev_warn(dev, "called with non multi queue aware HW\n");
-		return;
-	}
-
 	dma_ctrl = bcmgenet_tdma_readl(priv, DMA_CTRL);
 	dma_enable = dma_ctrl & DMA_EN;
 	dma_ctrl &= ~DMA_EN;
 	bcmgenet_tdma_writel(priv, dma_ctrl, DMA_CTRL);
 
+	dma_ctrl = 0;
+	ring_cfg = 0;
+
 	/* Enable strict priority arbiter mode */
 	bcmgenet_tdma_writel(priv, DMA_ARBITER_SP, DMA_ARB_CTRL);
 
+	/* Initialize Tx priority queues */
 	for (i = 0; i < priv->hw_params->tx_queues; i++) {
-		/* first 64 tx_cbs are reserved for default tx queue
-		 * (ring 16)
-		 */
-		bcmgenet_init_tx_ring(priv, i, priv->hw_params->bds_cnt,
-				      i * priv->hw_params->bds_cnt,
-				      (i + 1) * priv->hw_params->bds_cnt);
-
-		/* Configure ring as descriptor ring and setup priority */
-		ring_cfg |= 1 << i;
-		dma_ctrl |= 1 << (i + DMA_RING_BUF_EN_SHIFT);
-
+		bcmgenet_init_tx_ring(priv, i, priv->hw_params->tx_bds_per_q,
+				      i * priv->hw_params->tx_bds_per_q,
+				      (i + 1) * priv->hw_params->tx_bds_per_q);
+		ring_cfg |= (1 << i);
+		dma_ctrl |= (1 << (i + DMA_RING_BUF_EN_SHIFT));
 		dma_priority[DMA_PRIO_REG_INDEX(i)] |=
 			((GENET_Q0_PRIORITY + i) << DMA_PRIO_REG_SHIFT(i));
 	}
 
-	/* Set ring 16 priority and program the hardware registers */
+	/* Initialize Tx default queue 16 */
+	bcmgenet_init_tx_ring(priv, DESC_INDEX, GENET_Q16_TX_BD_CNT,
+			      priv->hw_params->tx_queues *
+			      priv->hw_params->tx_bds_per_q,
+			      TOTAL_DESC);
+	ring_cfg |= (1 << DESC_INDEX);
+	dma_ctrl |= (1 << (DESC_INDEX + DMA_RING_BUF_EN_SHIFT));
 	dma_priority[DMA_PRIO_REG_INDEX(DESC_INDEX)] |=
 		((GENET_Q0_PRIORITY + priv->hw_params->tx_queues) <<
 		 DMA_PRIO_REG_SHIFT(DESC_INDEX));
+
+	/* Set Tx queue priorities */
 	bcmgenet_tdma_writel(priv, dma_priority[0], DMA_PRIORITY_0);
 	bcmgenet_tdma_writel(priv, dma_priority[1], DMA_PRIORITY_1);
 	bcmgenet_tdma_writel(priv, dma_priority[2], DMA_PRIORITY_2);
 
+	/* Initialize Tx NAPI */
+	bcmgenet_init_tx_napi(priv);
+
+	/* Enable Tx queues */
+	bcmgenet_tdma_writel(priv, ring_cfg, DMA_RING_CFG);
+
+	/* Enable Tx DMA */
+	if (dma_enable)
+		dma_ctrl |= DMA_EN;
+	bcmgenet_tdma_writel(priv, dma_ctrl, DMA_CTRL);
+}
+
+static void bcmgenet_init_rx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_rx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->rx_queues; ++i) {
+		ring = &priv->rx_rings[i];
+		netif_napi_add(priv->dev, &ring->napi, bcmgenet_rx_poll, 64);
+	}
+
+	ring = &priv->rx_rings[DESC_INDEX];
+	netif_napi_add(priv->dev, &ring->napi, bcmgenet_rx_poll, 64);
+}
+
+static void bcmgenet_enable_rx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_rx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->rx_queues; ++i) {
+		ring = &priv->rx_rings[i];
+		napi_enable(&ring->napi);
+	}
+
+	ring = &priv->rx_rings[DESC_INDEX];
+	napi_enable(&ring->napi);
+}
+
+static void bcmgenet_disable_rx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_rx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->rx_queues; ++i) {
+		ring = &priv->rx_rings[i];
+		napi_disable(&ring->napi);
+	}
+
+	ring = &priv->rx_rings[DESC_INDEX];
+	napi_disable(&ring->napi);
+}
+
+static void bcmgenet_fini_rx_napi(struct bcmgenet_priv *priv)
+{
+	unsigned int i;
+	struct bcmgenet_rx_ring *ring;
+
+	for (i = 0; i < priv->hw_params->rx_queues; ++i) {
+		ring = &priv->rx_rings[i];
+		netif_napi_del(&ring->napi);
+	}
+
+	ring = &priv->rx_rings[DESC_INDEX];
+	netif_napi_del(&ring->napi);
+}
+
+/* Initialize Rx queues
+ *
+ * Queues 0-15 are priority queues. Hardware Filtering Block (HFB) can be
+ * used to direct traffic to these queues.
+ *
+ * Queue 16 is the default Rx queue with GENET_Q16_RX_BD_CNT descriptors.
+ */
+static int bcmgenet_init_rx_queues(struct net_device *dev)
+{
+	struct bcmgenet_priv *priv = netdev_priv(dev);
+	u32 i;
+	u32 dma_enable;
+	u32 dma_ctrl;
+	u32 ring_cfg;
+	int ret;
+
+	dma_ctrl = bcmgenet_rdma_readl(priv, DMA_CTRL);
+	dma_enable = dma_ctrl & DMA_EN;
+	dma_ctrl &= ~DMA_EN;
+	bcmgenet_rdma_writel(priv, dma_ctrl, DMA_CTRL);
+
+	dma_ctrl = 0;
+	ring_cfg = 0;
+
+	/* Initialize Rx priority queues */
+	for (i = 0; i < priv->hw_params->rx_queues; i++) {
+		ret = bcmgenet_init_rx_ring(priv, i,
+					    priv->hw_params->rx_bds_per_q,
+					    i * priv->hw_params->rx_bds_per_q,
+					    (i + 1) *
+					    priv->hw_params->rx_bds_per_q);
+		if (ret)
+			return ret;
+
+		ring_cfg |= (1 << i);
+		dma_ctrl |= (1 << (i + DMA_RING_BUF_EN_SHIFT));
+	}
+
+	/* Initialize Rx default queue 16 */
+	ret = bcmgenet_init_rx_ring(priv, DESC_INDEX, GENET_Q16_RX_BD_CNT,
+				    priv->hw_params->rx_queues *
+				    priv->hw_params->rx_bds_per_q,
+				    TOTAL_DESC);
+	if (ret)
+		return ret;
+
+	ring_cfg |= (1 << DESC_INDEX);
+	dma_ctrl |= (1 << (DESC_INDEX + DMA_RING_BUF_EN_SHIFT));
+
+	/* Initialize Rx NAPI */
+	bcmgenet_init_rx_napi(priv);
+
 	/* Enable rings */
-	reg = bcmgenet_tdma_readl(priv, DMA_RING_CFG);
-	reg |= ring_cfg;
-	bcmgenet_tdma_writel(priv, reg, DMA_RING_CFG);
+	bcmgenet_rdma_writel(priv, ring_cfg, DMA_RING_CFG);
 
 	/* Configure ring as descriptor ring and re-enable DMA if enabled */
-	reg = bcmgenet_tdma_readl(priv, DMA_CTRL);
-	reg |= dma_ctrl;
 	if (dma_enable)
-		reg |= DMA_EN;
-	bcmgenet_tdma_writel(priv, reg, DMA_CTRL);
+		dma_ctrl |= DMA_EN;
+	bcmgenet_rdma_writel(priv, dma_ctrl, DMA_CTRL);
+
+	return 0;
 }
 
 static int bcmgenet_dma_teardown(struct bcmgenet_priv *priv)
@@ -1950,10 +2176,13 @@ 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;
 
+	bcmgenet_fini_rx_napi(priv);
+	bcmgenet_fini_tx_napi(priv);
+
 	/* disable DMA */
 	bcmgenet_dma_teardown(priv);
 
@@ -1969,37 +2198,27 @@ 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)
 {
 	int ret;
+	unsigned int i;
+	struct enet_cb *cb;
 
-	netif_dbg(priv, hw, priv->dev, "bcmgenet: init_edma\n");
-
-	/* by default, enable ring 16 (descriptor based) */
-	ret = bcmgenet_init_rx_ring(priv, DESC_INDEX, TOTAL_DESC);
-	if (ret) {
-		netdev_err(priv->dev, "failed to initialize RX ring\n");
-		return ret;
-	}
+	netif_dbg(priv, hw, priv->dev, "%s\n", __func__);
 
-	/* init rDma */
-	bcmgenet_rdma_writel(priv, DMA_MAX_BURST_LENGTH, DMA_SCB_BURST_SIZE);
+	/* Initialize common Rx ring structures */
+	priv->rx_bds = priv->base + priv->hw_params->rdma_offset;
+	priv->num_rx_bds = TOTAL_DESC;
+	priv->rx_cbs = kcalloc(priv->num_rx_bds, sizeof(struct enet_cb),
+			       GFP_KERNEL);
+	if (!priv->rx_cbs)
+		return -ENOMEM;
 
-	/* Init tDma */
-	bcmgenet_tdma_writel(priv, DMA_MAX_BURST_LENGTH, DMA_SCB_BURST_SIZE);
+	for (i = 0; i < priv->num_rx_bds; i++) {
+		cb = priv->rx_cbs + i;
+		cb->bd_addr = priv->rx_bds + i * DMA_DESC_SIZE;
+	}
 
 	/* Initialize common TX ring structures */
 	priv->tx_bds = priv->base + priv->hw_params->tdma_offset;
@@ -2007,43 +2226,35 @@ 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);
+		kfree(priv->rx_cbs);
 		return -ENOMEM;
 	}
 
-	/* initialize multi xmit queue */
-	bcmgenet_init_multiq(priv->dev);
-
-	/* initialize special ring 16 */
-	bcmgenet_init_tx_ring(priv, DESC_INDEX, GENET_DEFAULT_BD_CNT,
-			      priv->hw_params->tx_queues *
-			      priv->hw_params->bds_cnt,
-			      TOTAL_DESC);
+	for (i = 0; i < priv->num_tx_bds; i++) {
+		cb = priv->tx_cbs + i;
+		cb->bd_addr = priv->tx_bds + i * DMA_DESC_SIZE;
+	}
 
-	return 0;
-}
+	/* Init rDma */
+	bcmgenet_rdma_writel(priv, DMA_MAX_BURST_LENGTH, DMA_SCB_BURST_SIZE);
 
-/* NAPI polling method*/
-static int bcmgenet_poll(struct napi_struct *napi, int budget)
-{
-	struct bcmgenet_priv *priv = container_of(napi,
-			struct bcmgenet_priv, napi);
-	unsigned int work_done;
+	/* Initialize Rx queues */
+	ret = bcmgenet_init_rx_queues(priv->dev);
+	if (ret) {
+		netdev_err(priv->dev, "failed to initialize Rx queues\n");
+		bcmgenet_free_rx_buffers(priv);
+		kfree(priv->rx_cbs);
+		kfree(priv->tx_cbs);
+		return ret;
+	}
 
-	work_done = bcmgenet_desc_rx(priv, budget);
+	/* Init tDma */
+	bcmgenet_tdma_writel(priv, DMA_MAX_BURST_LENGTH, DMA_SCB_BURST_SIZE);
 
-	/* Advancing our consumer index*/
-	priv->rx_c_index += work_done;
-	priv->rx_c_index &= DMA_C_INDEX_MASK;
-	bcmgenet_rdma_ring_writel(priv, DESC_INDEX,
-				  priv->rx_c_index, RDMA_CONS_INDEX);
-	if (work_done < budget) {
-		napi_complete(napi);
-		bcmgenet_intrl2_0_writel(priv, UMAC_IRQ_RXDMA_BDONE,
-					 INTRL2_CPU_MASK_CLEAR);
-	}
+	/* Initialize Tx queues */
+	bcmgenet_init_tx_queues(priv->dev);
 
-	return work_done;
+	return 0;
 }
 
 /* Interrupt bottom half */
@@ -2063,87 +2274,100 @@ static void bcmgenet_irq_task(struct work_struct *work)
 
 	/* Link UP/DOWN event */
 	if ((priv->hw_params->flags & GENET_HAS_MDIO_INTR) &&
-	    (priv->irq0_stat & (UMAC_IRQ_LINK_UP|UMAC_IRQ_LINK_DOWN))) {
+	    (priv->irq0_stat & UMAC_IRQ_LINK_EVENT)) {
 		phy_mac_interrupt(priv->phydev,
-				  priv->irq0_stat & UMAC_IRQ_LINK_UP);
-		priv->irq0_stat &= ~(UMAC_IRQ_LINK_UP|UMAC_IRQ_LINK_DOWN);
+				  !!(priv->irq0_stat & UMAC_IRQ_LINK_UP));
+		priv->irq0_stat &= ~UMAC_IRQ_LINK_EVENT;
 	}
 }
 
-/* bcmgenet_isr1: interrupt handler for ring buffer. */
+/* bcmgenet_isr1: handle Rx and Tx priority queues */
 static irqreturn_t bcmgenet_isr1(int irq, void *dev_id)
 {
 	struct bcmgenet_priv *priv = dev_id;
-	struct bcmgenet_tx_ring *ring;
+	struct bcmgenet_rx_ring *rx_ring;
+	struct bcmgenet_tx_ring *tx_ring;
 	unsigned int index;
 
 	/* Save irq status for bottom-half processing. */
 	priv->irq1_stat =
 		bcmgenet_intrl2_1_readl(priv, INTRL2_CPU_STAT) &
 		~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
-	 */
+	/* Check Rx priority queue interrupts */
+	for (index = 0; index < priv->hw_params->rx_queues; index++) {
+		if (!(priv->irq1_stat & BIT(UMAC_IRQ1_RX_INTR_SHIFT + index)))
+			continue;
+
+		rx_ring = &priv->rx_rings[index];
+
+		if (likely(napi_schedule_prep(&rx_ring->napi))) {
+			rx_ring->int_disable(rx_ring);
+			__napi_schedule(&rx_ring->napi);
+		}
+	}
+
+	/* Check Tx priority queue interrupts */
 	for (index = 0; index < priv->hw_params->tx_queues; index++) {
 		if (!(priv->irq1_stat & BIT(index)))
 			continue;
 
-		ring = &priv->tx_rings[index];
+		tx_ring = &priv->tx_rings[index];
 
-		if (likely(napi_schedule_prep(&ring->napi))) {
-			ring->int_disable(priv, ring);
-			__napi_schedule(&ring->napi);
+		if (likely(napi_schedule_prep(&tx_ring->napi))) {
+			tx_ring->int_disable(tx_ring);
+			__napi_schedule(&tx_ring->napi);
 		}
 	}
 
 	return IRQ_HANDLED;
 }
 
-/* bcmgenet_isr0: Handle various interrupts. */
+/* bcmgenet_isr0: handle Rx and Tx default queues + other stuff */
 static irqreturn_t bcmgenet_isr0(int irq, void *dev_id)
 {
 	struct bcmgenet_priv *priv = dev_id;
+	struct bcmgenet_rx_ring *rx_ring;
+	struct bcmgenet_tx_ring *tx_ring;
 
 	/* Save irq status for bottom-half processing. */
 	priv->irq0_stat =
 		bcmgenet_intrl2_0_readl(priv, INTRL2_CPU_STAT) &
 		~bcmgenet_intrl2_0_readl(priv, INTRL2_CPU_MASK_STATUS);
+
 	/* clear interrupts */
 	bcmgenet_intrl2_0_writel(priv, priv->irq0_stat, INTRL2_CPU_CLEAR);
 
 	netif_dbg(priv, intr, priv->dev,
 		  "IRQ=0x%x\n", priv->irq0_stat);
 
-	if (priv->irq0_stat & (UMAC_IRQ_RXDMA_BDONE | UMAC_IRQ_RXDMA_PDONE)) {
-		/* We use NAPI(software interrupt throttling, if
-		 * Rx Descriptor throttling is not used.
-		 * Disable interrupt, will be enabled in the poll method.
-		 */
-		if (likely(napi_schedule_prep(&priv->napi))) {
-			bcmgenet_intrl2_0_writel(priv, UMAC_IRQ_RXDMA_BDONE,
-						 INTRL2_CPU_MASK_SET);
-			__napi_schedule(&priv->napi);
+	if (priv->irq0_stat & UMAC_IRQ_RXDMA_DONE) {
+		rx_ring = &priv->rx_rings[DESC_INDEX];
+
+		if (likely(napi_schedule_prep(&rx_ring->napi))) {
+			rx_ring->int_disable(rx_ring);
+			__napi_schedule(&rx_ring->napi);
 		}
 	}
-	if (priv->irq0_stat &
-			(UMAC_IRQ_TXDMA_BDONE | UMAC_IRQ_TXDMA_PDONE)) {
-		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_TXDMA_DONE) {
+		tx_ring = &priv->tx_rings[DESC_INDEX];
+
+		if (likely(napi_schedule_prep(&tx_ring->napi))) {
+			tx_ring->int_disable(tx_ring);
+			__napi_schedule(&tx_ring->napi);
 		}
 	}
+
 	if (priv->irq0_stat & (UMAC_IRQ_PHY_DET_R |
 				UMAC_IRQ_PHY_DET_F |
-				UMAC_IRQ_LINK_UP |
-				UMAC_IRQ_LINK_DOWN |
+				UMAC_IRQ_LINK_EVENT |
 				UMAC_IRQ_HFB_SM |
 				UMAC_IRQ_HFB_MM |
 				UMAC_IRQ_MPD_R)) {
@@ -2227,18 +2451,170 @@ static void bcmgenet_enable_dma(struct bcmgenet_priv *priv, u32 dma_ctrl)
 	bcmgenet_tdma_writel(priv, reg, DMA_CTRL);
 }
 
+static bool bcmgenet_hfb_is_filter_enabled(struct bcmgenet_priv *priv,
+					   u32 f_index)
+{
+	u32 offset;
+	u32 reg;
+
+	offset = HFB_FLT_ENABLE_V3PLUS + (f_index < 32) * sizeof(u32);
+	reg = bcmgenet_hfb_reg_readl(priv, offset);
+	return !!(reg & (1 << (f_index % 32)));
+}
+
+static void bcmgenet_hfb_enable_filter(struct bcmgenet_priv *priv, u32 f_index)
+{
+	u32 offset;
+	u32 reg;
+
+	offset = HFB_FLT_ENABLE_V3PLUS + (f_index < 32) * sizeof(u32);
+	reg = bcmgenet_hfb_reg_readl(priv, offset);
+	reg |= (1 << (f_index % 32));
+	bcmgenet_hfb_reg_writel(priv, reg, offset);
+}
+
+static void bcmgenet_hfb_set_filter_rx_queue_mapping(struct bcmgenet_priv *priv,
+						     u32 f_index, u32 rx_queue)
+{
+	u32 offset;
+	u32 reg;
+
+	offset = f_index / 8;
+	reg = bcmgenet_rdma_readl(priv, DMA_INDEX2RING_0 + offset);
+	reg &= ~(0xF << (4 * (f_index % 8)));
+	reg |= ((rx_queue & 0xF) << (4 * (f_index % 8)));
+	bcmgenet_rdma_writel(priv, reg, DMA_INDEX2RING_0 + offset);
+}
+
+static void bcmgenet_hfb_set_filter_length(struct bcmgenet_priv *priv,
+					   u32 f_index, u32 f_length)
+{
+	u32 offset;
+	u32 reg;
+
+	offset = HFB_FLT_LEN_V3PLUS +
+		 ((priv->hw_params->hfb_filter_cnt - 1 - f_index) / 4) *
+		 sizeof(u32);
+	reg = bcmgenet_hfb_reg_readl(priv, offset);
+	reg &= ~(0xFF << (8 * (f_index % 4)));
+	reg |= ((f_length & 0xFF) << (8 * (f_index % 4)));
+	bcmgenet_hfb_reg_writel(priv, reg, offset);
+}
+
+static int bcmgenet_hfb_find_unused_filter(struct bcmgenet_priv *priv)
+{
+	u32 f_index;
+
+	for (f_index = 0; f_index < priv->hw_params->hfb_filter_cnt; f_index++)
+		if (!bcmgenet_hfb_is_filter_enabled(priv, f_index))
+			return f_index;
+
+	return -ENOMEM;
+}
+
+/* bcmgenet_hfb_add_filter
+ *
+ * Add new filter to Hardware Filter Block to match and direct Rx traffic to
+ * desired Rx queue.
+ *
+ * f_data is an array of unsigned 32-bit integers where each 32-bit integer
+ * provides filter data for 2 bytes (4 nibbles) of Rx frame:
+ *
+ * bits 31:20 - unused
+ * bit  19    - nibble 0 match enable
+ * bit  18    - nibble 1 match enable
+ * bit  17    - nibble 2 match enable
+ * bit  16    - nibble 3 match enable
+ * bits 15:12 - nibble 0 data
+ * bits 11:8  - nibble 1 data
+ * bits 7:4   - nibble 2 data
+ * bits 3:0   - nibble 3 data
+ *
+ * Example:
+ * In order to match:
+ * - Ethernet frame type = 0x0800 (IP)
+ * - IP version field = 4
+ * - IP protocol field = 0x11 (UDP)
+ *
+ * The following filter is needed:
+ * u32 hfb_filter_ipv4_udp[] = {
+ *   Rx frame offset 0x00: 0x00000000, 0x00000000, 0x00000000, 0x00000000,
+ *   Rx frame offset 0x08: 0x00000000, 0x00000000, 0x000F0800, 0x00084000,
+ *   Rx frame offset 0x10: 0x00000000, 0x00000000, 0x00000000, 0x00030011,
+ * };
+ *
+ * To add the filter to HFB and direct the traffic to Rx queue 0, call:
+ * bcmgenet_hfb_add_filter(priv, hfb_filter_ipv4_udp,
+ *                         ARRAY_SIZE(hfb_filter_ipv4_udp), 0);
+ */
+int bcmgenet_hfb_add_filter(struct bcmgenet_priv *priv, u32 *f_data,
+			    u32 f_length, u32 rx_queue)
+{
+	int f_index;
+	u32 i;
+
+	f_index = bcmgenet_hfb_find_unused_filter(priv);
+	if (f_index < 0)
+		return -ENOMEM;
+
+	if (f_length > priv->hw_params->hfb_filter_size)
+		return -EINVAL;
+
+	for (i = 0; i < f_length; i++)
+		bcmgenet_hfb_writel(priv, f_data[i],
+			(f_index * priv->hw_params->hfb_filter_size + i) *
+			sizeof(u32));
+
+	bcmgenet_hfb_set_filter_length(priv, f_index, 2 * f_length);
+	bcmgenet_hfb_set_filter_rx_queue_mapping(priv, f_index, rx_queue);
+	bcmgenet_hfb_enable_filter(priv, f_index);
+	bcmgenet_hfb_reg_writel(priv, 0x1, HFB_CTRL);
+
+	return 0;
+}
+
+/* bcmgenet_hfb_clear
+ *
+ * Clear Hardware Filter Block and disable all filtering.
+ */
+static void bcmgenet_hfb_clear(struct bcmgenet_priv *priv)
+{
+	u32 i;
+
+	bcmgenet_hfb_reg_writel(priv, 0x0, HFB_CTRL);
+	bcmgenet_hfb_reg_writel(priv, 0x0, HFB_FLT_ENABLE_V3PLUS);
+	bcmgenet_hfb_reg_writel(priv, 0x0, HFB_FLT_ENABLE_V3PLUS + 4);
+
+	for (i = DMA_INDEX2RING_0; i <= DMA_INDEX2RING_7; i++)
+		bcmgenet_rdma_writel(priv, 0x0, i);
+
+	for (i = 0; i < (priv->hw_params->hfb_filter_cnt / 4); i++)
+		bcmgenet_hfb_reg_writel(priv, 0x0,
+					HFB_FLT_LEN_V3PLUS + i * sizeof(u32));
+
+	for (i = 0; i < priv->hw_params->hfb_filter_cnt *
+			priv->hw_params->hfb_filter_size; i++)
+		bcmgenet_hfb_writel(priv, 0x0, i * sizeof(u32));
+}
+
+static void bcmgenet_hfb_init(struct bcmgenet_priv *priv)
+{
+	if (GENET_IS_V1(priv) || GENET_IS_V2(priv))
+		return;
+
+	bcmgenet_hfb_clear(priv);
+}
+
 static void bcmgenet_netif_start(struct net_device *dev)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 
 	/* Start the network engine */
-	napi_enable(&priv->napi);
+	bcmgenet_enable_rx_napi(priv);
+	bcmgenet_enable_tx_napi(priv);
 
 	umac_enable_set(priv, CMD_TX_EN | CMD_RX_EN, true);
 
-	if (phy_is_internal(priv->phydev))
-		bcmgenet_power_up(priv, GENET_POWER_PASSIVE);
-
 	netif_tx_start_all_queues(dev);
 
 	phy_start(priv->phydev);
@@ -2257,6 +2633,12 @@ static int bcmgenet_open(struct net_device *dev)
 	if (!IS_ERR(priv->clk))
 		clk_prepare_enable(priv->clk);
 
+	/* If this is an internal GPHY, power it back on now, before UniMAC is
+	 * brought out of reset as absolutely no UniMAC activity is allowed
+	 */
+	if (phy_is_internal(priv->phydev))
+		bcmgenet_power_up(priv, GENET_POWER_PASSIVE);
+
 	/* take MAC out of reset */
 	bcmgenet_umac_reset(priv);
 
@@ -2286,12 +2668,15 @@ static int bcmgenet_open(struct net_device *dev)
 	ret = bcmgenet_init_dma(priv);
 	if (ret) {
 		netdev_err(dev, "failed to initialize DMA\n");
-		goto err_fini_dma;
+		goto err_clk_disable;
 	}
 
 	/* Always enable ring 16 - descriptor ring */
 	bcmgenet_enable_dma(priv, dma_ctrl);
 
+	/* HFB init */
+	bcmgenet_hfb_init(priv);
+
 	ret = request_irq(priv->irq0, bcmgenet_isr0, IRQF_SHARED,
 			  dev->name, priv);
 	if (ret < 0) {
@@ -2331,10 +2716,10 @@ static void bcmgenet_netif_stop(struct net_device *dev)
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 
 	netif_tx_stop_all_queues(dev);
-	napi_disable(&priv->napi);
 	phy_stop(priv->phydev);
-
 	bcmgenet_intr_disable(priv);
+	bcmgenet_disable_rx_napi(priv);
+	bcmgenet_disable_tx_napi(priv);
 
 	/* Wait for pending work items to complete. Since interrupts are
 	 * disabled no new work will be scheduled.
@@ -2377,12 +2762,12 @@ static int bcmgenet_close(struct net_device *dev)
 	free_irq(priv->irq1, priv);
 
 	if (phy_is_internal(priv->phydev))
-		bcmgenet_power_down(priv, GENET_POWER_PASSIVE);
+		ret = bcmgenet_power_down(priv, GENET_POWER_PASSIVE);
 
 	if (!IS_ERR(priv->clk))
 		clk_disable_unprepare(priv->clk);
 
-	return 0;
+	return ret;
 }
 
 static void bcmgenet_timeout(struct net_device *dev)
@@ -2499,8 +2884,9 @@ static const struct net_device_ops bcmgenet_netdev_ops = {
 static struct bcmgenet_hw_params bcmgenet_hw_params[] = {
 	[GENET_V1] = {
 		.tx_queues = 0,
+		.tx_bds_per_q = 0,
 		.rx_queues = 0,
-		.bds_cnt = 0,
+		.rx_bds_per_q = 0,
 		.bp_in_en_shift = 16,
 		.bp_in_mask = 0xffff,
 		.hfb_filter_cnt = 16,
@@ -2512,8 +2898,9 @@ static struct bcmgenet_hw_params bcmgenet_hw_params[] = {
 	},
 	[GENET_V2] = {
 		.tx_queues = 4,
-		.rx_queues = 4,
-		.bds_cnt = 32,
+		.tx_bds_per_q = 32,
+		.rx_queues = 0,
+		.rx_bds_per_q = 0,
 		.bp_in_en_shift = 16,
 		.bp_in_mask = 0xffff,
 		.hfb_filter_cnt = 16,
@@ -2528,11 +2915,13 @@ static struct bcmgenet_hw_params bcmgenet_hw_params[] = {
 	},
 	[GENET_V3] = {
 		.tx_queues = 4,
-		.rx_queues = 4,
-		.bds_cnt = 32,
+		.tx_bds_per_q = 32,
+		.rx_queues = 0,
+		.rx_bds_per_q = 0,
 		.bp_in_en_shift = 17,
 		.bp_in_mask = 0x1ffff,
 		.hfb_filter_cnt = 48,
+		.hfb_filter_size = 128,
 		.qtag_mask = 0x3F,
 		.tbuf_offset = 0x0600,
 		.hfb_offset = 0x8000,
@@ -2540,15 +2929,18 @@ static struct bcmgenet_hw_params bcmgenet_hw_params[] = {
 		.rdma_offset = 0x10000,
 		.tdma_offset = 0x11000,
 		.words_per_bd = 2,
-		.flags = GENET_HAS_EXT | GENET_HAS_MDIO_INTR,
+		.flags = GENET_HAS_EXT | GENET_HAS_MDIO_INTR |
+			 GENET_HAS_MOCA_LINK_DET,
 	},
 	[GENET_V4] = {
 		.tx_queues = 4,
-		.rx_queues = 4,
-		.bds_cnt = 32,
+		.tx_bds_per_q = 32,
+		.rx_queues = 0,
+		.rx_bds_per_q = 0,
 		.bp_in_en_shift = 17,
 		.bp_in_mask = 0x1ffff,
 		.hfb_filter_cnt = 48,
+		.hfb_filter_size = 128,
 		.qtag_mask = 0x3F,
 		.tbuf_offset = 0x0600,
 		.hfb_offset = 0x8000,
@@ -2556,7 +2948,8 @@ static struct bcmgenet_hw_params bcmgenet_hw_params[] = {
 		.rdma_offset = 0x2000,
 		.tdma_offset = 0x4000,
 		.words_per_bd = 3,
-		.flags = GENET_HAS_40BITS | GENET_HAS_EXT | GENET_HAS_MDIO_INTR,
+		.flags = GENET_HAS_40BITS | GENET_HAS_EXT |
+			 GENET_HAS_MDIO_INTR | GENET_HAS_MOCA_LINK_DET,
 	},
 };
 
@@ -2645,14 +3038,15 @@ static void bcmgenet_set_hw_params(struct bcmgenet_priv *priv)
 #endif
 
 	pr_debug("Configuration for version: %d\n"
-		"TXq: %1d, RXq: %1d, BDs: %1d\n"
+		"TXq: %1d, TXqBDs: %1d, RXq: %1d, RXqBDs: %1d\n"
 		"BP << en: %2d, BP msk: 0x%05x\n"
 		"HFB count: %2d, QTAQ msk: 0x%05x\n"
 		"TBUF: 0x%04x, HFB: 0x%04x, HFBreg: 0x%04x\n"
 		"RDMA: 0x%05x, TDMA: 0x%05x\n"
 		"Words/BD: %d\n",
 		priv->version,
-		params->tx_queues, params->rx_queues, params->bds_cnt,
+		params->tx_queues, params->tx_bds_per_q,
+		params->rx_queues, params->rx_bds_per_q,
 		params->bp_in_en_shift, params->bp_in_mask,
 		params->hfb_filter_cnt, params->qtag_mask,
 		params->tbuf_offset, params->hfb_offset,
@@ -2680,8 +3074,9 @@ static int bcmgenet_probe(struct platform_device *pdev)
 	struct resource *r;
 	int err = -EIO;
 
-	/* Up to GENET_MAX_MQ_CNT + 1 TX queues and a single RX queue */
-	dev = alloc_etherdev_mqs(sizeof(*priv), GENET_MAX_MQ_CNT + 1, 1);
+	/* Up to GENET_MAX_MQ_CNT + 1 TX queues and RX queues */
+	dev = alloc_etherdev_mqs(sizeof(*priv), GENET_MAX_MQ_CNT + 1,
+				 GENET_MAX_MQ_CNT + 1);
 	if (!dev) {
 		dev_err(&pdev->dev, "can't allocate net device\n");
 		return -ENOMEM;
@@ -2727,7 +3122,6 @@ static int bcmgenet_probe(struct platform_device *pdev)
 	dev->watchdog_timeo = 2 * HZ;
 	dev->ethtool_ops = &bcmgenet_ethtool_ops;
 	dev->netdev_ops = &bcmgenet_netdev_ops;
-	netif_napi_add(dev, &priv->napi, bcmgenet_poll, 64);
 
 	priv->msg_enable = netif_msg_init(-1, GENET_MSG_DEFAULT);
 
@@ -2860,14 +3254,16 @@ static int bcmgenet_suspend(struct device *d)
 
 	/* Prepare the device for Wake-on-LAN and switch to the slow clock */
 	if (device_may_wakeup(d) && priv->wolopts) {
-		bcmgenet_power_down(priv, GENET_POWER_WOL_MAGIC);
+		ret = bcmgenet_power_down(priv, GENET_POWER_WOL_MAGIC);
 		clk_prepare_enable(priv->clk_wol);
+	} else if (phy_is_internal(priv->phydev)) {
+		ret = bcmgenet_power_down(priv, GENET_POWER_PASSIVE);
 	}
 
 	/* Turn off the clocks */
 	clk_disable_unprepare(priv->clk);
 
-	return 0;
+	return ret;
 }
 
 static int bcmgenet_resume(struct device *d)
@@ -2886,6 +3282,12 @@ static int bcmgenet_resume(struct device *d)
 	if (ret)
 		return ret;
 
+	/* If this is an internal GPHY, power it back on now, before UniMAC is
+	 * brought out of reset as absolutely no UniMAC activity is allowed
+	 */
+	if (phy_is_internal(priv->phydev))
+		bcmgenet_power_up(priv, GENET_POWER_PASSIVE);
+
 	bcmgenet_umac_reset(priv);
 
 	ret = init_umac(priv);
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.h b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
index 0d370d168aee..6f2887a5e0be 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.h
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
@@ -293,6 +293,7 @@ struct bcmgenet_mib_counters {
 #define UMAC_IRQ_PHY_DET_F		(1 << 3)
 #define UMAC_IRQ_LINK_UP		(1 << 4)
 #define UMAC_IRQ_LINK_DOWN		(1 << 5)
+#define UMAC_IRQ_LINK_EVENT		(UMAC_IRQ_LINK_UP | UMAC_IRQ_LINK_DOWN)
 #define UMAC_IRQ_UMAC			(1 << 6)
 #define UMAC_IRQ_UMAC_TSV		(1 << 7)
 #define UMAC_IRQ_TBUF_UNDERRUN		(1 << 8)
@@ -303,13 +304,22 @@ struct bcmgenet_mib_counters {
 #define UMAC_IRQ_RXDMA_MBDONE		(1 << 13)
 #define UMAC_IRQ_RXDMA_PDONE		(1 << 14)
 #define UMAC_IRQ_RXDMA_BDONE		(1 << 15)
+#define UMAC_IRQ_RXDMA_DONE		(UMAC_IRQ_RXDMA_PDONE | \
+					 UMAC_IRQ_RXDMA_BDONE)
 #define UMAC_IRQ_TXDMA_MBDONE		(1 << 16)
 #define UMAC_IRQ_TXDMA_PDONE		(1 << 17)
 #define UMAC_IRQ_TXDMA_BDONE		(1 << 18)
+#define UMAC_IRQ_TXDMA_DONE		(UMAC_IRQ_TXDMA_PDONE | \
+					 UMAC_IRQ_TXDMA_BDONE)
 /* Only valid for GENETv3+ */
 #define UMAC_IRQ_MDIO_DONE		(1 << 23)
 #define UMAC_IRQ_MDIO_ERROR		(1 << 24)
 
+/* INTRL2 instance 1 definitions */
+#define UMAC_IRQ1_TX_INTR_MASK		0xFFFF
+#define UMAC_IRQ1_RX_INTR_MASK		0xFFFF
+#define UMAC_IRQ1_RX_INTR_SHIFT		16
+
 /* Register block offsets */
 #define GENET_SYS_OFF			0x0000
 #define GENET_GR_BRIDGE_OFF		0x0040
@@ -354,6 +364,7 @@ struct bcmgenet_mib_counters {
 #define EXT_GPHY_CTRL			0x1C
 #define  EXT_CFG_IDDQ_BIAS		(1 << 0)
 #define  EXT_CFG_PWR_DOWN		(1 << 1)
+#define  EXT_CK25_DIS			(1 << 4)
 #define  EXT_GPHY_RESET			(1 << 5)
 
 /* DMA rings size */
@@ -497,17 +508,20 @@ enum bcmgenet_version {
 #define GENET_HAS_40BITS	(1 << 0)
 #define GENET_HAS_EXT		(1 << 1)
 #define GENET_HAS_MDIO_INTR	(1 << 2)
+#define GENET_HAS_MOCA_LINK_DET	(1 << 3)
 
 /* BCMGENET hardware parameters, keep this structure nicely aligned
  * since it is going to be used in hot paths
  */
 struct bcmgenet_hw_params {
 	u8		tx_queues;
+	u8		tx_bds_per_q;
 	u8		rx_queues;
-	u8		bds_cnt;
+	u8		rx_bds_per_q;
 	u8		bp_in_en_shift;
 	u32		bp_in_mask;
 	u8		hfb_filter_cnt;
+	u8		hfb_filter_size;
 	u8		qtag_mask;
 	u16		tbuf_offset;
 	u32		hfb_offset;
@@ -525,16 +539,30 @@ struct bcmgenet_tx_ring {
 	unsigned int	queue;		/* queue index */
 	struct enet_cb	*cbs;		/* tx ring buffer control block*/
 	unsigned int	size;		/* size of each tx ring */
+	unsigned int    clean_ptr;      /* Tx ring clean pointer */
 	unsigned int	c_index;	/* last consumer index of each ring*/
 	unsigned int	free_bds;	/* # of free bds for each ring */
 	unsigned int	write_ptr;	/* Tx ring write pointer SW copy */
 	unsigned int	prod_index;	/* Tx ring producer index SW copy */
 	unsigned int	cb_ptr;		/* Tx ring initial CB ptr */
 	unsigned int	end_ptr;	/* Tx ring end CB ptr */
-	void (*int_enable)(struct bcmgenet_priv *priv,
-			   struct bcmgenet_tx_ring *);
-	void (*int_disable)(struct bcmgenet_priv *priv,
-			    struct bcmgenet_tx_ring *);
+	void (*int_enable)(struct bcmgenet_tx_ring *);
+	void (*int_disable)(struct bcmgenet_tx_ring *);
+	struct bcmgenet_priv *priv;
+};
+
+struct bcmgenet_rx_ring {
+	struct napi_struct napi;	/* Rx NAPI struct */
+	unsigned int	index;		/* Rx ring index */
+	struct enet_cb	*cbs;		/* Rx ring buffer control block */
+	unsigned int	size;		/* Rx ring size */
+	unsigned int	c_index;	/* Rx last consumer index */
+	unsigned int	read_ptr;	/* Rx ring read pointer */
+	unsigned int	cb_ptr;		/* Rx ring initial CB ptr */
+	unsigned int	end_ptr;	/* Rx ring end CB ptr */
+	unsigned int	old_discards;
+	void (*int_enable)(struct bcmgenet_rx_ring *);
+	void (*int_disable)(struct bcmgenet_rx_ring *);
 	struct bcmgenet_priv *priv;
 };
 
@@ -543,11 +571,6 @@ struct bcmgenet_priv {
 	void __iomem *base;
 	enum bcmgenet_version version;
 	struct net_device *dev;
-	u32 int0_mask;
-	u32 int1_mask;
-
-	/* NAPI for descriptor based rx */
-	struct napi_struct napi ____cacheline_aligned;
 
 	/* transmit variables */
 	void __iomem *tx_bds;
@@ -558,13 +581,11 @@ struct bcmgenet_priv {
 
 	/* receive variables */
 	void __iomem *rx_bds;
-	void __iomem *rx_bd_assign_ptr;
-	int rx_bd_assign_index;
 	struct enet_cb *rx_cbs;
 	unsigned int num_rx_bds;
 	unsigned int rx_buf_len;
-	unsigned int rx_read_ptr;
-	unsigned int rx_c_index;
+
+	struct bcmgenet_rx_ring rx_rings[DESC_INDEX + 1];
 
 	/* other misc variables */
 	struct bcmgenet_hw_params *hw_params;
@@ -651,6 +672,7 @@ int bcmgenet_mii_init(struct net_device *dev);
 int bcmgenet_mii_config(struct net_device *dev, bool init);
 void bcmgenet_mii_exit(struct net_device *dev);
 void bcmgenet_mii_reset(struct net_device *dev);
+void bcmgenet_phy_power_set(struct net_device *dev, bool enable);
 void bcmgenet_mii_setup(struct net_device *dev);
 
 /* Wake-on-LAN routines */
diff --git a/drivers/net/ethernet/broadcom/genet/bcmmii.c b/drivers/net/ethernet/broadcom/genet/bcmmii.c
index 446889cc3c6a..e7651b3c6c57 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmmii.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmmii.c
@@ -168,7 +168,7 @@ void bcmgenet_mii_reset(struct net_device *dev)
 	}
 }
 
-static void bcmgenet_ephy_power_up(struct net_device *dev)
+void bcmgenet_phy_power_set(struct net_device *dev, bool enable)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 	u32 reg = 0;
@@ -178,14 +178,25 @@ static void bcmgenet_ephy_power_up(struct net_device *dev)
 		return;
 
 	reg = bcmgenet_ext_readl(priv, EXT_GPHY_CTRL);
-	reg &= ~(EXT_CFG_IDDQ_BIAS | EXT_CFG_PWR_DOWN);
-	reg |= EXT_GPHY_RESET;
-	bcmgenet_ext_writel(priv, reg, EXT_GPHY_CTRL);
-	mdelay(2);
+	if (enable) {
+		reg &= ~EXT_CK25_DIS;
+		bcmgenet_ext_writel(priv, reg, EXT_GPHY_CTRL);
+		mdelay(1);
+
+		reg &= ~(EXT_CFG_IDDQ_BIAS | EXT_CFG_PWR_DOWN);
+		reg |= EXT_GPHY_RESET;
+		bcmgenet_ext_writel(priv, reg, EXT_GPHY_CTRL);
+		mdelay(1);
 
-	reg &= ~EXT_GPHY_RESET;
+		reg &= ~EXT_GPHY_RESET;
+	} else {
+		reg |= EXT_CFG_IDDQ_BIAS | EXT_CFG_PWR_DOWN | EXT_GPHY_RESET;
+		bcmgenet_ext_writel(priv, reg, EXT_GPHY_CTRL);
+		mdelay(1);
+		reg |= EXT_CK25_DIS;
+	}
 	bcmgenet_ext_writel(priv, reg, EXT_GPHY_CTRL);
-	udelay(20);
+	udelay(60);
 }
 
 static void bcmgenet_internal_phy_setup(struct net_device *dev)
@@ -193,8 +204,8 @@ static void bcmgenet_internal_phy_setup(struct net_device *dev)
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 	u32 reg;
 
-	/* Power up EPHY */
-	bcmgenet_ephy_power_up(dev);
+	/* Power up PHY */
+	bcmgenet_phy_power_set(dev, true);
 	/* enable APD */
 	reg = bcmgenet_ext_readl(priv, EXT_EXT_PWR_MGMT);
 	reg |= EXT_PWR_DN_EN_LD;
@@ -451,6 +462,15 @@ static int bcmgenet_mii_of_init(struct bcmgenet_priv *priv)
 	return 0;
 }
 
+static int bcmgenet_fixed_phy_link_update(struct net_device *dev,
+					  struct fixed_phy_status *status)
+{
+	if (dev && dev->phydev && status)
+		status->link = dev->phydev->link;
+
+	return 0;
+}
+
 static int bcmgenet_mii_pd_init(struct bcmgenet_priv *priv)
 {
 	struct device *kdev = &priv->pdev->dev;
@@ -502,6 +522,13 @@ static int bcmgenet_mii_pd_init(struct bcmgenet_priv *priv)
 			dev_err(kdev, "failed to register fixed PHY device\n");
 			return -ENODEV;
 		}
+
+		if (priv->hw_params->flags & GENET_HAS_MOCA_LINK_DET) {
+			ret = fixed_phy_set_link_update(
+				phydev, bcmgenet_fixed_phy_link_update);
+			if (!ret)
+				phydev->link = 0;
+		}
 	}
 
 	priv->phydev = phydev;
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index 23a019cee279..1270b189a9a2 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -6217,10 +6217,9 @@ static int tg3_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int tg3_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int tg3_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	u64 ns;
-	u32 remainder;
 	struct tg3 *tp = container_of(ptp, struct tg3, ptp_info);
 
 	tg3_full_lock(tp, 0);
@@ -6228,19 +6227,18 @@ static int tg3_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 	ns += tp->ptp_adjust;
 	tg3_full_unlock(tp);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
 
 static int tg3_ptp_settime(struct ptp_clock_info *ptp,
-			   const struct timespec *ts)
+			   const struct timespec64 *ts)
 {
 	u64 ns;
 	struct tg3 *tp = container_of(ptp, struct tg3, ptp_info);
 
-	ns = timespec_to_ns(ts);
+	ns = timespec64_to_ns(ts);
 
 	tg3_full_lock(tp, 0);
 	tg3_refclk_write(tp, ns);
@@ -6320,8 +6318,8 @@ static const struct ptp_clock_info tg3_ptp_caps = {
 	.pps		= 0,
 	.adjfreq	= tg3_ptp_adjfreq,
 	.adjtime	= tg3_ptp_adjtime,
-	.gettime	= tg3_ptp_gettime,
-	.settime	= tg3_ptp_settime,
+	.gettime64	= tg3_ptp_gettime,
+	.settime64	= tg3_ptp_settime,
 	.enable		= tg3_ptp_enable,
 };
 
@@ -7244,7 +7242,7 @@ static int tg3_poll_msix(struct napi_struct *napi, int budget)
 			if (tnapi == &tp->napi[1] && tp->rx_refill)
 				continue;
 
-			napi_complete(napi);
+			napi_complete_done(napi, work_done);
 			/* Reenable interrupts. */
 			tw32_mailbox(tnapi->int_mbox, tnapi->last_tag << 24);
 
@@ -7337,7 +7335,7 @@ static int tg3_poll(struct napi_struct *napi, int budget)
 			sblk->status &= ~SD_STATUS_UPDATED;
 
 		if (likely(!tg3_has_work(tnapi))) {
-			napi_complete(napi);
+			napi_complete_done(napi, work_done);
 			tg3_int_reenable(tnapi);
 			break;
 		}
diff --git a/drivers/net/ethernet/brocade/Kconfig b/drivers/net/ethernet/brocade/Kconfig
index 264155778857..4e8c0b6c57d0 100644
--- a/drivers/net/ethernet/brocade/Kconfig
+++ b/drivers/net/ethernet/brocade/Kconfig
@@ -1,9 +1,9 @@
 #
-# Brocade device configuration
+# QLogic BR-series device configuration
 #
 
 config NET_VENDOR_BROCADE
-	bool "Brocade devices"
+	bool "QLogic BR-series devices"
 	default y
 	depends on PCI
 	---help---
@@ -13,8 +13,8 @@ config NET_VENDOR_BROCADE
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
-	  the questions about Brocade cards. If you say Y, you will be asked for
-	  your specific card in the following questions.
+	  the questions about QLogic BR-series cards. If you say Y, you will be
+	  asked for your specific card in the following questions.
 
 if NET_VENDOR_BROCADE
 
diff --git a/drivers/net/ethernet/brocade/Makefile b/drivers/net/ethernet/brocade/Makefile
index b58238d2df6a..fec10f9b4558 100644
--- a/drivers/net/ethernet/brocade/Makefile
+++ b/drivers/net/ethernet/brocade/Makefile
@@ -1,5 +1,5 @@
 #
-# Makefile for the Brocade device drivers.
+# Makefile for the QLogic BR-series device drivers.
 #
 
 obj-$(CONFIG_BNA) += bna/
diff --git a/drivers/net/ethernet/brocade/bna/Kconfig b/drivers/net/ethernet/brocade/bna/Kconfig
index dc2eb526fbf7..fe01279a8843 100644
--- a/drivers/net/ethernet/brocade/bna/Kconfig
+++ b/drivers/net/ethernet/brocade/bna/Kconfig
@@ -1,17 +1,17 @@
 #
-# Brocade network device configuration
+# QLogic BR-series network device configuration
 #
 
 config BNA
-	tristate "Brocade 1010/1020 10Gb Ethernet Driver support"
+	tristate "QLogic BR-series 1010/1020/1860 10Gb Ethernet Driver support"
 	depends on PCI
 	---help---
-	  This driver supports Brocade 1010/1020 10Gb CEE capable Ethernet
-	  cards.
+	  This driver supports QLogic BR-series 1010/1020/1860 10Gb CEE capable
+	  Ethernet cards.
 	  To compile this driver as a module, choose M here: the module
 	  will be called bna.
 
-	  For general information and support, go to the Brocade support
+	  For general information and support, go to the QLogic support
 	  website at:
 
-	  <http://support.brocade.com>
+	  <http://support.qlogic.com>
diff --git a/drivers/net/ethernet/brocade/bna/Makefile b/drivers/net/ethernet/brocade/bna/Makefile
index 6027302ae73a..6e10b99733a2 100644
--- a/drivers/net/ethernet/brocade/bna/Makefile
+++ b/drivers/net/ethernet/brocade/bna/Makefile
@@ -1,5 +1,6 @@
 #
-# Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+# Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+# Copyright (c) 2014-2015 QLogic Corporation.
 # All rights reserved.
 #
 
diff --git a/drivers/net/ethernet/brocade/bna/bfa_cee.c b/drivers/net/ethernet/brocade/bna/bfa_cee.c
index 550d2521ba76..cf9f3956f198 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_cee.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_cee.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #include "bfa_cee.h"
diff --git a/drivers/net/ethernet/brocade/bna/bfa_cee.h b/drivers/net/ethernet/brocade/bna/bfa_cee.h
index 93fde633d6f3..d04eef5d5a77 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_cee.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_cee.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #ifndef __BFA_CEE_H__
diff --git a/drivers/net/ethernet/brocade/bna/bfa_cs.h b/drivers/net/ethernet/brocade/bna/bfa_cs.h
index ad004a4c3897..af25d8e8fae0 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_cs.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_cs.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 /* BFA common services */
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs.h b/drivers/net/ethernet/brocade/bna/bfa_defs.h
index b7d8127c198f..3bfd9da92630 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #ifndef __BFA_DEFS_H__
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h b/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
index b39c5f23974b..a37326d44fbb 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BFA_DEFS_CNA_H__
 #define __BFA_DEFS_CNA_H__
@@ -134,7 +135,7 @@ struct bfa_cee_lldp_str {
 	u8 value[BFA_CEE_LLDP_MAX_STRING_LEN];
 };
 
-/* LLDP paramters */
+/* LLDP parameters */
 struct bfa_cee_lldp_cfg {
 	struct bfa_cee_lldp_str chassis_id;
 	struct bfa_cee_lldp_str port_id;
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h b/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
index 7fb396fe679d..7a45cd0b594d 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BFA_DEFS_MFG_COMM_H__
 #define __BFA_DEFS_MFG_COMM_H__
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_status.h b/drivers/net/ethernet/brocade/bna/bfa_defs_status.h
index ea9af9ae754d..a43b56002752 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_status.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_status.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BFA_DEFS_STATUS_H__
 #define __BFA_DEFS_STATUS_H__
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.c b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
index 354ae9792bad..594a2ab36d31 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #include "bfa_ioc.h"
@@ -1339,7 +1340,7 @@ bfa_ioc_fwver_md5_check(struct bfi_ioc_image_hdr *fwhdr_1,
 	return true;
 }
 
-/* Returns TRUE if major minor and maintainence are same.
+/* Returns TRUE if major minor and maintenance are same.
  * If patch version are same, check for MD5 Checksum to be same.
  */
 static bool
@@ -2763,7 +2764,7 @@ bfa_nw_ioc_notify_register(struct bfa_ioc *ioc,
 	list_add_tail(&notify->qe, &ioc->notify_q);
 }
 
-#define BFA_MFG_NAME "Brocade"
+#define BFA_MFG_NAME "QLogic"
 static void
 bfa_ioc_get_adapter_attr(struct bfa_ioc *ioc,
 			 struct bfa_adapter_attr *ad_attr)
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.h b/drivers/net/ethernet/brocade/bna/bfa_ioc.h
index 20cff7df4b55..effb7156e7a4 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #ifndef __BFA_IOC_H__
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
index d639558455cb..2e72445dbb4f 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #include "bfa_ioc.h"
@@ -698,7 +699,7 @@ bfa_ioc_ct2_sclk_init(void __iomem *rb)
 
 	/*
 	 * Ignore mode and program for the max clock (which is FC16)
-	 * Firmware/NFC will do the PLL init appropiately
+	 * Firmware/NFC will do the PLL init appropriately
 	 */
 	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
 	r32 &= ~(__APP_PLL_SCLK_REFCLK_SEL | __APP_PLL_SCLK_CLK_DIV2);
diff --git a/drivers/net/ethernet/brocade/bna/bfa_msgq.c b/drivers/net/ethernet/brocade/bna/bfa_msgq.c
index 55067d0d25cf..c07d5b9372f4 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_msgq.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_msgq.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 /* MSGQ module source file. */
diff --git a/drivers/net/ethernet/brocade/bna/bfa_msgq.h b/drivers/net/ethernet/brocade/bna/bfa_msgq.h
index a6a565a366dc..66bc8b5acd57 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_msgq.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_msgq.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #ifndef __BFA_MSGQ_H__
diff --git a/drivers/net/ethernet/brocade/bna/bfi.h b/drivers/net/ethernet/brocade/bna/bfi.h
index 8c563a77cdf6..2bcde4042268 100644
--- a/drivers/net/ethernet/brocade/bna/bfi.h
+++ b/drivers/net/ethernet/brocade/bna/bfi.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BFI_H__
 #define __BFI_H__
@@ -158,8 +159,8 @@ enum bfi_asic_gen {
 };
 
 enum bfi_asic_mode {
-	BFI_ASIC_MODE_FC	= 1,	/* FC upto 8G speed		*/
-	BFI_ASIC_MODE_FC16	= 2,	/* FC upto 16G speed		*/
+	BFI_ASIC_MODE_FC	= 1,	/* FC up to 8G speed		*/
+	BFI_ASIC_MODE_FC16	= 2,	/* FC up to 16G speed		*/
 	BFI_ASIC_MODE_ETH	= 3,	/* Ethernet ports		*/
 	BFI_ASIC_MODE_COMBO	= 4,	/* FC 16G and Ethernet 10G port	*/
 };
diff --git a/drivers/net/ethernet/brocade/bna/bfi_cna.h b/drivers/net/ethernet/brocade/bna/bfi_cna.h
index 6704a4392973..bd605bee72ee 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_cna.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_cna.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BFI_CNA_H__
 #define __BFI_CNA_H__
diff --git a/drivers/net/ethernet/brocade/bna/bfi_enet.h b/drivers/net/ethernet/brocade/bna/bfi_enet.h
index ae072dc5d238..bccca3bbadb8 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_enet.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_enet.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 /* BNA Hardware and Firmware Interface */
diff --git a/drivers/net/ethernet/brocade/bna/bfi_reg.h b/drivers/net/ethernet/brocade/bna/bfi_reg.h
index c49fa312ddbd..2835b51eabec 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_reg.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_reg.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,13 +11,14 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 /*
- * bfi_reg.h ASIC register defines for all Brocade adapter ASICs
+ * bfi_reg.h ASIC register defines for all QLogic BR-series adapter ASICs
  */
 
 #ifndef __BFI_REG_H__
@@ -221,7 +222,7 @@ enum {
 #define __PMM_1T_RESET_P		0x00000001
 #define PMM_1T_RESET_REG_P1		0x00023c1c
 
-/* Brocade 1860 Adapter specific defines */
+/* QLogic BR-series 1860 Adapter specific defines */
 #define CT2_PCI_CPQ_BASE		0x00030000
 #define CT2_PCI_APP_BASE		0x00030100
 #define CT2_PCI_ETH_BASE		0x00030400
@@ -264,7 +265,7 @@ enum {
 #define CT2_HOSTFN_MSIX_VT_INDEX_MBOX_ERR	(CT2_PCI_APP_BASE + 0x38)
 
 /*
- * Brocade 1860 adapter CPQ block registers
+ * QLogic BR-series 1860 adapter CPQ block registers
  */
 #define CT2_HOSTFN_LPU0_MBOX0		(CT2_PCI_CPQ_BASE + 0x00)
 #define CT2_HOSTFN_LPU1_MBOX0		(CT2_PCI_CPQ_BASE + 0x20)
diff --git a/drivers/net/ethernet/brocade/bna/bna.h b/drivers/net/ethernet/brocade/bna/bna.h
index 1f512190d696..8ba72b1f36d9 100644
--- a/drivers/net/ethernet/brocade/bna/bna.h
+++ b/drivers/net/ethernet/brocade/bna/bna.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BNA_H__
 #define __BNA_H__
diff --git a/drivers/net/ethernet/brocade/bna/bna_enet.c b/drivers/net/ethernet/brocade/bna/bna_enet.c
index 903466ef41c0..deb8da6ab9cc 100644
--- a/drivers/net/ethernet/brocade/bna/bna_enet.c
+++ b/drivers/net/ethernet/brocade/bna/bna_enet.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #include "bna.h"
 
diff --git a/drivers/net/ethernet/brocade/bna/bna_hw_defs.h b/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
index 2702d02e98d9..174af0e9d056 100644
--- a/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
+++ b/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 /* File for interrupt macros and functions */
@@ -362,7 +363,7 @@ struct bna_txq_wi_vector {
 
 /*  TxQ Entry Structure
  *
- *  BEWARE:  Load values into this structure with correct endianess.
+ *  BEWARE:  Load values into this structure with correct endianness.
  */
 struct bna_txq_entry {
 	union {
diff --git a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
index 5fac411c52f4..8ab3a5f62706 100644
--- a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
+++ b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
   */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #include "bna.h"
 #include "bfi.h"
diff --git a/drivers/net/ethernet/brocade/bna/bna_types.h b/drivers/net/ethernet/brocade/bna/bna_types.h
index 621547cd3504..d0a7a566f5d6 100644
--- a/drivers/net/ethernet/brocade/bna/bna_types.h
+++ b/drivers/net/ethernet/brocade/bna/bna_types.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BNA_TYPES_H__
 #define __BNA_TYPES_H__
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index 7714d7790089..37072a83f9d6 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #include <linux/bitops.h>
 #include <linux/netdevice.h>
@@ -3867,7 +3868,7 @@ bnad_module_init(void)
 {
 	int err;
 
-	pr_info("Brocade 10G Ethernet driver - version: %s\n",
+	pr_info("QLogic BR-series 10G Ethernet driver - version: %s\n",
 			BNAD_VERSION);
 
 	bfa_nw_ioc_auto_recover(bnad_ioc_auto_recover);
@@ -3894,7 +3895,7 @@ module_exit(bnad_module_exit);
 
 MODULE_AUTHOR("Brocade");
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Brocade 10G PCIe Ethernet driver");
+MODULE_DESCRIPTION("QLogic BR-series 10G PCIe Ethernet driver");
 MODULE_VERSION(BNAD_VERSION);
 MODULE_FIRMWARE(CNA_FW_FILE_CT);
 MODULE_FIRMWARE(CNA_FW_FILE_CT2);
diff --git a/drivers/net/ethernet/brocade/bna/bnad.h b/drivers/net/ethernet/brocade/bna/bnad.h
index 2842c188e0da..7ead6c23edb6 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.h
+++ b/drivers/net/ethernet/brocade/bna/bnad.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #ifndef __BNAD_H__
 #define __BNAD_H__
@@ -71,7 +72,7 @@ struct bnad_rx_ctrl {
 #define BNAD_NAME			"bna"
 #define BNAD_NAME_LEN			64
 
-#define BNAD_VERSION			"3.2.23.0"
+#define BNAD_VERSION			"3.2.25.1"
 
 #define BNAD_MAILBOX_MSIX_INDEX		0
 #define BNAD_MAILBOX_MSIX_VECTORS	1
diff --git a/drivers/net/ethernet/brocade/bna/bnad_debugfs.c b/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
index 619083a860a4..72c89550417c 100644
--- a/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
+++ b/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2011 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #include <linux/debugfs.h>
diff --git a/drivers/net/ethernet/brocade/bna/bnad_ethtool.c b/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
index d26adac6ab99..12f344debd1c 100644
--- a/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
+++ b/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #include "cna.h"
diff --git a/drivers/net/ethernet/brocade/bna/cna.h b/drivers/net/ethernet/brocade/bna/cna.h
index b3ff6d507951..28e7d0ffeab1 100644
--- a/drivers/net/ethernet/brocade/bna/cna.h
+++ b/drivers/net/ethernet/brocade/bna/cna.h
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2006-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2006-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 
 #ifndef __CNA_H__
@@ -37,8 +38,8 @@
 
 extern char bfa_version[];
 
-#define CNA_FW_FILE_CT	"ctfw-3.2.3.0.bin"
-#define CNA_FW_FILE_CT2	"ct2fw-3.2.3.0.bin"
+#define CNA_FW_FILE_CT	"ctfw-3.2.5.1.bin"
+#define CNA_FW_FILE_CT2	"ct2fw-3.2.5.1.bin"
 #define FC_SYMNAME_MAX	256	/*!< max name server symbolic name size */
 
 #pragma pack(1)
diff --git a/drivers/net/ethernet/brocade/bna/cna_fwimg.c b/drivers/net/ethernet/brocade/bna/cna_fwimg.c
index 6f72771caea6..ebf462d8082f 100644
--- a/drivers/net/ethernet/brocade/bna/cna_fwimg.c
+++ b/drivers/net/ethernet/brocade/bna/cna_fwimg.c
@@ -1,5 +1,5 @@
 /*
- * Linux network driver for Brocade Converged Network Adapter.
+ * Linux network driver for QLogic BR-series Converged Network Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -11,9 +11,10 @@
  * General Public License for more details.
  */
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014-2015 QLogic Corporation
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  */
 #include <linux/firmware.h>
 #include "bnad.h"
diff --git a/drivers/net/ethernet/cadence/Kconfig b/drivers/net/ethernet/cadence/Kconfig
index 321d2ad235d9..1ba3e3a67389 100644
--- a/drivers/net/ethernet/cadence/Kconfig
+++ b/drivers/net/ethernet/cadence/Kconfig
@@ -4,7 +4,7 @@
 
 config NET_CADENCE
 	bool "Cadence devices"
-	depends on HAS_IOMEM && (ARM || AVR32 || MICROBLAZE || COMPILE_TEST)
+	depends on HAS_IOMEM
 	default y
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y.
@@ -20,17 +20,9 @@ config NET_CADENCE
 
 if NET_CADENCE
 
-config ARM_AT91_ETHER
-	tristate "AT91RM9200 Ethernet support"
-	depends on HAS_DMA && (ARCH_AT91 || COMPILE_TEST)
-	select MACB
-	---help---
-	  If you wish to compile a kernel for the AT91RM9200 and enable
-	  ethernet support, then you should always answer Y to this.
-
 config MACB
 	tristate "Cadence MACB/GEM support"
-	depends on HAS_DMA && (PLATFORM_AT32AP || ARCH_AT91 || ARCH_PICOXCELL || ARCH_ZYNQ || MICROBLAZE || COMPILE_TEST)
+	depends on HAS_DMA
 	select PHYLIB
 	---help---
 	  The Cadence MACB ethernet interface is found on many Atmel AT32 and
diff --git a/drivers/net/ethernet/cadence/Makefile b/drivers/net/ethernet/cadence/Makefile
index 9068b8331ed1..91f79b1f0505 100644
--- a/drivers/net/ethernet/cadence/Makefile
+++ b/drivers/net/ethernet/cadence/Makefile
@@ -2,5 +2,4 @@
 # Makefile for the Atmel network device drivers.
 #
 
-obj-$(CONFIG_ARM_AT91_ETHER) += at91_ether.o
 obj-$(CONFIG_MACB) += macb.o
diff --git a/drivers/net/ethernet/cadence/at91_ether.c b/drivers/net/ethernet/cadence/at91_ether.c
deleted file mode 100644
index 7ef55f5fa664..000000000000
--- a/drivers/net/ethernet/cadence/at91_ether.c
+++ /dev/null
@@ -1,481 +0,0 @@
-/*
- * Ethernet driver for the Atmel AT91RM9200 (Thunder)
- *
- *  Copyright (C) 2003 SAN People (Pty) Ltd
- *
- * Based on an earlier Atmel EMAC macrocell driver by Atmel and Lineo Inc.
- * Initial version by Rick Bronson 01/11/2003
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * as published by the Free Software Foundation; either version
- * 2 of the License, or (at your option) any later version.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-#include <linux/dma-mapping.h>
-#include <linux/ethtool.h>
-#include <linux/platform_data/macb.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/gfp.h>
-#include <linux/phy.h>
-#include <linux/io.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/of_net.h>
-
-#include "macb.h"
-
-/* 1518 rounded up */
-#define MAX_RBUFF_SZ	0x600
-/* max number of receive buffers */
-#define MAX_RX_DESCR	9
-
-/* Initialize and start the Receiver and Transmit subsystems */
-static int at91ether_start(struct net_device *dev)
-{
-	struct macb *lp = netdev_priv(dev);
-	dma_addr_t addr;
-	u32 ctl;
-	int i;
-
-	lp->rx_ring = dma_alloc_coherent(&lp->pdev->dev,
-					 (MAX_RX_DESCR *
-					  sizeof(struct macb_dma_desc)),
-					 &lp->rx_ring_dma, GFP_KERNEL);
-	if (!lp->rx_ring)
-		return -ENOMEM;
-
-	lp->rx_buffers = dma_alloc_coherent(&lp->pdev->dev,
-					    MAX_RX_DESCR * MAX_RBUFF_SZ,
-					    &lp->rx_buffers_dma, GFP_KERNEL);
-	if (!lp->rx_buffers) {
-		dma_free_coherent(&lp->pdev->dev,
-				  MAX_RX_DESCR * sizeof(struct macb_dma_desc),
-				  lp->rx_ring, lp->rx_ring_dma);
-		lp->rx_ring = NULL;
-		return -ENOMEM;
-	}
-
-	addr = lp->rx_buffers_dma;
-	for (i = 0; i < MAX_RX_DESCR; i++) {
-		lp->rx_ring[i].addr = addr;
-		lp->rx_ring[i].ctrl = 0;
-		addr += MAX_RBUFF_SZ;
-	}
-
-	/* Set the Wrap bit on the last descriptor */
-	lp->rx_ring[MAX_RX_DESCR - 1].addr |= MACB_BIT(RX_WRAP);
-
-	/* Reset buffer index */
-	lp->rx_tail = 0;
-
-	/* Program address of descriptor list in Rx Buffer Queue register */
-	macb_writel(lp, RBQP, lp->rx_ring_dma);
-
-	/* Enable Receive and Transmit */
-	ctl = macb_readl(lp, NCR);
-	macb_writel(lp, NCR, ctl | MACB_BIT(RE) | MACB_BIT(TE));
-
-	return 0;
-}
-
-/* Open the ethernet interface */
-static int at91ether_open(struct net_device *dev)
-{
-	struct macb *lp = netdev_priv(dev);
-	u32 ctl;
-	int ret;
-
-	/* Clear internal statistics */
-	ctl = macb_readl(lp, NCR);
-	macb_writel(lp, NCR, ctl | MACB_BIT(CLRSTAT));
-
-	macb_set_hwaddr(lp);
-
-	ret = at91ether_start(dev);
-	if (ret)
-		return ret;
-
-	/* Enable MAC interrupts */
-	macb_writel(lp, IER, MACB_BIT(RCOMP)	|
-			     MACB_BIT(RXUBR)	|
-			     MACB_BIT(ISR_TUND)	|
-			     MACB_BIT(ISR_RLE)	|
-			     MACB_BIT(TCOMP)	|
-			     MACB_BIT(ISR_ROVR)	|
-			     MACB_BIT(HRESP));
-
-	/* schedule a link state check */
-	phy_start(lp->phy_dev);
-
-	netif_start_queue(dev);
-
-	return 0;
-}
-
-/* Close the interface */
-static int at91ether_close(struct net_device *dev)
-{
-	struct macb *lp = netdev_priv(dev);
-	u32 ctl;
-
-	/* Disable Receiver and Transmitter */
-	ctl = macb_readl(lp, NCR);
-	macb_writel(lp, NCR, ctl & ~(MACB_BIT(TE) | MACB_BIT(RE)));
-
-	/* Disable MAC interrupts */
-	macb_writel(lp, IDR, MACB_BIT(RCOMP)	|
-			     MACB_BIT(RXUBR)	|
-			     MACB_BIT(ISR_TUND)	|
-			     MACB_BIT(ISR_RLE)	|
-			     MACB_BIT(TCOMP)	|
-			     MACB_BIT(ISR_ROVR) |
-			     MACB_BIT(HRESP));
-
-	netif_stop_queue(dev);
-
-	dma_free_coherent(&lp->pdev->dev,
-				MAX_RX_DESCR * sizeof(struct macb_dma_desc),
-				lp->rx_ring, lp->rx_ring_dma);
-	lp->rx_ring = NULL;
-
-	dma_free_coherent(&lp->pdev->dev,
-				MAX_RX_DESCR * MAX_RBUFF_SZ,
-				lp->rx_buffers, lp->rx_buffers_dma);
-	lp->rx_buffers = NULL;
-
-	return 0;
-}
-
-/* Transmit packet */
-static int at91ether_start_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct macb *lp = netdev_priv(dev);
-
-	if (macb_readl(lp, TSR) & MACB_BIT(RM9200_BNQ)) {
-		netif_stop_queue(dev);
-
-		/* Store packet information (to free when Tx completed) */
-		lp->skb = skb;
-		lp->skb_length = skb->len;
-		lp->skb_physaddr = dma_map_single(NULL, skb->data, skb->len,
-							DMA_TO_DEVICE);
-
-		/* Set address of the data in the Transmit Address register */
-		macb_writel(lp, TAR, lp->skb_physaddr);
-		/* Set length of the packet in the Transmit Control register */
-		macb_writel(lp, TCR, skb->len);
-
-	} else {
-		netdev_err(dev, "%s called, but device is busy!\n", __func__);
-		return NETDEV_TX_BUSY;
-	}
-
-	return NETDEV_TX_OK;
-}
-
-/* Extract received frame from buffer descriptors and sent to upper layers.
- * (Called from interrupt context)
- */
-static void at91ether_rx(struct net_device *dev)
-{
-	struct macb *lp = netdev_priv(dev);
-	unsigned char *p_recv;
-	struct sk_buff *skb;
-	unsigned int pktlen;
-
-	while (lp->rx_ring[lp->rx_tail].addr & MACB_BIT(RX_USED)) {
-		p_recv = lp->rx_buffers + lp->rx_tail * MAX_RBUFF_SZ;
-		pktlen = MACB_BF(RX_FRMLEN, lp->rx_ring[lp->rx_tail].ctrl);
-		skb = netdev_alloc_skb(dev, pktlen + 2);
-		if (skb) {
-			skb_reserve(skb, 2);
-			memcpy(skb_put(skb, pktlen), p_recv, pktlen);
-
-			skb->protocol = eth_type_trans(skb, dev);
-			lp->stats.rx_packets++;
-			lp->stats.rx_bytes += pktlen;
-			netif_rx(skb);
-		} else {
-			lp->stats.rx_dropped++;
-		}
-
-		if (lp->rx_ring[lp->rx_tail].ctrl & MACB_BIT(RX_MHASH_MATCH))
-			lp->stats.multicast++;
-
-		/* reset ownership bit */
-		lp->rx_ring[lp->rx_tail].addr &= ~MACB_BIT(RX_USED);
-
-		/* wrap after last buffer */
-		if (lp->rx_tail == MAX_RX_DESCR - 1)
-			lp->rx_tail = 0;
-		else
-			lp->rx_tail++;
-	}
-}
-
-/* MAC interrupt handler */
-static irqreturn_t at91ether_interrupt(int irq, void *dev_id)
-{
-	struct net_device *dev = dev_id;
-	struct macb *lp = netdev_priv(dev);
-	u32 intstatus, ctl;
-
-	/* MAC Interrupt Status register indicates what interrupts are pending.
-	 * It is automatically cleared once read.
-	 */
-	intstatus = macb_readl(lp, ISR);
-
-	/* Receive complete */
-	if (intstatus & MACB_BIT(RCOMP))
-		at91ether_rx(dev);
-
-	/* Transmit complete */
-	if (intstatus & MACB_BIT(TCOMP)) {
-		/* The TCOM bit is set even if the transmission failed */
-		if (intstatus & (MACB_BIT(ISR_TUND) | MACB_BIT(ISR_RLE)))
-			lp->stats.tx_errors++;
-
-		if (lp->skb) {
-			dev_kfree_skb_irq(lp->skb);
-			lp->skb = NULL;
-			dma_unmap_single(NULL, lp->skb_physaddr, lp->skb_length, DMA_TO_DEVICE);
-			lp->stats.tx_packets++;
-			lp->stats.tx_bytes += lp->skb_length;
-		}
-		netif_wake_queue(dev);
-	}
-
-	/* Work-around for EMAC Errata section 41.3.1 */
-	if (intstatus & MACB_BIT(RXUBR)) {
-		ctl = macb_readl(lp, NCR);
-		macb_writel(lp, NCR, ctl & ~MACB_BIT(RE));
-		macb_writel(lp, NCR, ctl | MACB_BIT(RE));
-	}
-
-	if (intstatus & MACB_BIT(ISR_ROVR))
-		netdev_err(dev, "ROVR error\n");
-
-	return IRQ_HANDLED;
-}
-
-#ifdef CONFIG_NET_POLL_CONTROLLER
-static void at91ether_poll_controller(struct net_device *dev)
-{
-	unsigned long flags;
-
-	local_irq_save(flags);
-	at91ether_interrupt(dev->irq, dev);
-	local_irq_restore(flags);
-}
-#endif
-
-static const struct net_device_ops at91ether_netdev_ops = {
-	.ndo_open		= at91ether_open,
-	.ndo_stop		= at91ether_close,
-	.ndo_start_xmit		= at91ether_start_xmit,
-	.ndo_get_stats		= macb_get_stats,
-	.ndo_set_rx_mode	= macb_set_rx_mode,
-	.ndo_set_mac_address	= eth_mac_addr,
-	.ndo_do_ioctl		= macb_ioctl,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_change_mtu		= eth_change_mtu,
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	.ndo_poll_controller	= at91ether_poll_controller,
-#endif
-};
-
-#if defined(CONFIG_OF)
-static const struct of_device_id at91ether_dt_ids[] = {
-	{ .compatible = "cdns,at91rm9200-emac" },
-	{ .compatible = "cdns,emac" },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, at91ether_dt_ids);
-#endif
-
-/* Detect MAC & PHY and perform ethernet interface initialization */
-static int __init at91ether_probe(struct platform_device *pdev)
-{
-	struct macb_platform_data *board_data = dev_get_platdata(&pdev->dev);
-	struct resource *regs;
-	struct net_device *dev;
-	struct phy_device *phydev;
-	struct macb *lp;
-	int res;
-	u32 reg;
-	const char *mac;
-
-	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!regs)
-		return -ENOENT;
-
-	dev = alloc_etherdev(sizeof(struct macb));
-	if (!dev)
-		return -ENOMEM;
-
-	lp = netdev_priv(dev);
-	lp->pdev = pdev;
-	lp->dev = dev;
-	spin_lock_init(&lp->lock);
-
-	/* physical base address */
-	dev->base_addr = regs->start;
-	lp->regs = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
-	if (!lp->regs) {
-		res = -ENOMEM;
-		goto err_free_dev;
-	}
-
-	/* Clock */
-	lp->pclk = devm_clk_get(&pdev->dev, "ether_clk");
-	if (IS_ERR(lp->pclk)) {
-		res = PTR_ERR(lp->pclk);
-		goto err_free_dev;
-	}
-	clk_prepare_enable(lp->pclk);
-
-	lp->hclk = ERR_PTR(-ENOENT);
-	lp->tx_clk = ERR_PTR(-ENOENT);
-
-	/* Install the interrupt handler */
-	dev->irq = platform_get_irq(pdev, 0);
-	res = devm_request_irq(&pdev->dev, dev->irq, at91ether_interrupt, 0, dev->name, dev);
-	if (res)
-		goto err_disable_clock;
-
-	dev->netdev_ops = &at91ether_netdev_ops;
-	dev->ethtool_ops = &macb_ethtool_ops;
-	platform_set_drvdata(pdev, dev);
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	mac = of_get_mac_address(pdev->dev.of_node);
-	if (mac)
-		memcpy(lp->dev->dev_addr, mac, ETH_ALEN);
-	else
-		macb_get_hwaddr(lp);
-
-	res = of_get_phy_mode(pdev->dev.of_node);
-	if (res < 0) {
-		if (board_data && board_data->is_rmii)
-			lp->phy_interface = PHY_INTERFACE_MODE_RMII;
-		else
-			lp->phy_interface = PHY_INTERFACE_MODE_MII;
-	} else {
-		lp->phy_interface = res;
-	}
-
-	macb_writel(lp, NCR, 0);
-
-	reg = MACB_BF(CLK, MACB_CLK_DIV32) | MACB_BIT(BIG);
-	if (lp->phy_interface == PHY_INTERFACE_MODE_RMII)
-		reg |= MACB_BIT(RM9200_RMII);
-
-	macb_writel(lp, NCFGR, reg);
-
-	/* Register the network interface */
-	res = register_netdev(dev);
-	if (res)
-		goto err_disable_clock;
-
-	res = macb_mii_init(lp);
-	if (res)
-		goto err_out_unregister_netdev;
-
-	/* will be enabled in open() */
-	netif_carrier_off(dev);
-
-	phydev = lp->phy_dev;
-	netdev_info(dev, "attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-				phydev->drv->name, dev_name(&phydev->dev),
-				phydev->irq);
-
-	/* Display ethernet banner */
-	netdev_info(dev, "AT91 ethernet at 0x%08lx int=%d (%pM)\n",
-				dev->base_addr, dev->irq, dev->dev_addr);
-
-	return 0;
-
-err_out_unregister_netdev:
-	unregister_netdev(dev);
-err_disable_clock:
-	clk_disable_unprepare(lp->pclk);
-err_free_dev:
-	free_netdev(dev);
-	return res;
-}
-
-static int at91ether_remove(struct platform_device *pdev)
-{
-	struct net_device *dev = platform_get_drvdata(pdev);
-	struct macb *lp = netdev_priv(dev);
-
-	if (lp->phy_dev)
-		phy_disconnect(lp->phy_dev);
-
-	mdiobus_unregister(lp->mii_bus);
-	kfree(lp->mii_bus->irq);
-	mdiobus_free(lp->mii_bus);
-	unregister_netdev(dev);
-	clk_disable_unprepare(lp->pclk);
-	free_netdev(dev);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int at91ether_suspend(struct platform_device *pdev, pm_message_t mesg)
-{
-	struct net_device *net_dev = platform_get_drvdata(pdev);
-	struct macb *lp = netdev_priv(net_dev);
-
-	if (netif_running(net_dev)) {
-		netif_stop_queue(net_dev);
-		netif_device_detach(net_dev);
-
-		clk_disable_unprepare(lp->pclk);
-	}
-	return 0;
-}
-
-static int at91ether_resume(struct platform_device *pdev)
-{
-	struct net_device *net_dev = platform_get_drvdata(pdev);
-	struct macb *lp = netdev_priv(net_dev);
-
-	if (netif_running(net_dev)) {
-		clk_prepare_enable(lp->pclk);
-
-		netif_device_attach(net_dev);
-		netif_start_queue(net_dev);
-	}
-	return 0;
-}
-#else
-#define at91ether_suspend	NULL
-#define at91ether_resume	NULL
-#endif
-
-static struct platform_driver at91ether_driver = {
-	.remove		= at91ether_remove,
-	.suspend	= at91ether_suspend,
-	.resume		= at91ether_resume,
-	.driver		= {
-		.name	= "at91_ether",
-		.of_match_table	= of_match_ptr(at91ether_dt_ids),
-	},
-};
-
-module_platform_driver_probe(at91ether_driver, at91ether_probe);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("AT91RM9200 EMAC Ethernet driver");
-MODULE_AUTHOR("Andrew Victor");
-MODULE_ALIAS("platform:at91_ether");
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index 81d41539fcba..9f5387249f24 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -102,7 +102,7 @@ static void *macb_rx_buffer(struct macb *bp, unsigned int index)
 	return bp->rx_buffers + bp->rx_buffer_size * macb_rx_ring_wrap(index);
 }
 
-void macb_set_hwaddr(struct macb *bp)
+static void macb_set_hwaddr(struct macb *bp)
 {
 	u32 bottom;
 	u16 top;
@@ -120,9 +120,8 @@ void macb_set_hwaddr(struct macb *bp)
 	macb_or_gem_writel(bp, SA4B, 0);
 	macb_or_gem_writel(bp, SA4T, 0);
 }
-EXPORT_SYMBOL_GPL(macb_set_hwaddr);
 
-void macb_get_hwaddr(struct macb *bp)
+static void macb_get_hwaddr(struct macb *bp)
 {
 	struct macb_platform_data *pdata;
 	u32 bottom;
@@ -162,7 +161,6 @@ void macb_get_hwaddr(struct macb *bp)
 	netdev_info(bp->dev, "invalid hw address, using random\n");
 	eth_hw_addr_random(bp->dev);
 }
-EXPORT_SYMBOL_GPL(macb_get_hwaddr);
 
 static int macb_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
@@ -213,6 +211,9 @@ static void macb_set_tx_clk(struct clk *clk, int speed, struct net_device *dev)
 {
 	long ferr, rate, rate_rounded;
 
+	if (!clk)
+		return;
+
 	switch (speed) {
 	case SPEED_10:
 		rate = 2500000;
@@ -292,11 +293,13 @@ static void macb_handle_link_change(struct net_device *dev)
 
 	spin_unlock_irqrestore(&bp->lock, flags);
 
-	if (!IS_ERR(bp->tx_clk))
-		macb_set_tx_clk(bp->tx_clk, phydev->speed, dev);
-
 	if (status_change) {
 		if (phydev->link) {
+			/* Update the TX clock rate if and only if the link is
+			 * up and there has been a link change.
+			 */
+			macb_set_tx_clk(bp->tx_clk, phydev->speed, dev);
+
 			netif_carrier_on(dev);
 			netdev_info(dev, "link up (%d/%s)\n",
 				    phydev->speed,
@@ -357,7 +360,7 @@ static int macb_mii_probe(struct net_device *dev)
 	return 0;
 }
 
-int macb_mii_init(struct macb *bp)
+static int macb_mii_init(struct macb *bp)
 {
 	struct macb_platform_data *pdata;
 	struct device_node *np;
@@ -438,7 +441,6 @@ err_out_free_mdiobus:
 err_out:
 	return err;
 }
-EXPORT_SYMBOL_GPL(macb_mii_init);
 
 static void macb_update_stats(struct macb *bp)
 {
@@ -449,7 +451,7 @@ static void macb_update_stats(struct macb *bp)
 	WARN_ON((unsigned long)(end - p - 1) != (MACB_TPF - MACB_PFR) / 4);
 
 	for(; p < end; p++, reg++)
-		*p += __raw_readl(reg);
+		*p += readl_relaxed(reg);
 }
 
 static int macb_halt_tx(struct macb *bp)
@@ -1578,6 +1580,7 @@ static u32 macb_dbw(struct macb *bp)
 static void macb_configure_dma(struct macb *bp)
 {
 	u32 dmacfg;
+	u32 tmp, ncr;
 
 	if (macb_is_gem(bp)) {
 		dmacfg = gem_readl(bp, DMACFG) & ~GEM_BF(RXBS, -1L);
@@ -1585,7 +1588,24 @@ static void macb_configure_dma(struct macb *bp)
 		if (bp->dma_burst_length)
 			dmacfg = GEM_BFINS(FBLDO, bp->dma_burst_length, dmacfg);
 		dmacfg |= GEM_BIT(TXPBMS) | GEM_BF(RXBMS, -1L);
-		dmacfg &= ~GEM_BIT(ENDIA);
+		dmacfg &= ~GEM_BIT(ENDIA_PKT);
+
+		/* Find the CPU endianness by using the loopback bit of net_ctrl
+		 * register. save it first. When the CPU is in big endian we
+		 * need to program swaped mode for management descriptor access.
+		 */
+		ncr = macb_readl(bp, NCR);
+		__raw_writel(MACB_BIT(LLB), bp->regs + MACB_NCR);
+		tmp =  __raw_readl(bp->regs + MACB_NCR);
+
+		if (tmp == MACB_BIT(LLB))
+			dmacfg &= ~GEM_BIT(ENDIA_DESC);
+		else
+			dmacfg |= GEM_BIT(ENDIA_DESC); /* CPU in big endian */
+
+		/* Restore net_ctrl */
+		macb_writel(bp, NCR, ncr);
+
 		if (bp->dev->features & NETIF_F_HW_CSUM)
 			dmacfg |= GEM_BIT(TXCOEN);
 		else
@@ -1723,7 +1743,7 @@ static void macb_sethashtable(struct net_device *dev)
 /*
  * Enable/Disable promiscuous and multicast modes.
  */
-void macb_set_rx_mode(struct net_device *dev)
+static void macb_set_rx_mode(struct net_device *dev)
 {
 	unsigned long cfg;
 	struct macb *bp = netdev_priv(dev);
@@ -1764,7 +1784,6 @@ void macb_set_rx_mode(struct net_device *dev)
 
 	macb_writel(bp, NCFGR, cfg);
 }
-EXPORT_SYMBOL_GPL(macb_set_rx_mode);
 
 static int macb_open(struct net_device *dev)
 {
@@ -1832,14 +1851,14 @@ static void gem_update_stats(struct macb *bp)
 
 	for (i = 0; i < GEM_STATS_LEN; ++i, ++p) {
 		u32 offset = gem_statistics[i].offset;
-		u64 val = __raw_readl(bp->regs + offset);
+		u64 val = readl_relaxed(bp->regs + offset);
 
 		bp->ethtool_stats[i] += val;
 		*p += val;
 
 		if (offset == GEM_OCTTXL || offset == GEM_OCTRXL) {
 			/* Add GEM_OCTTXH, GEM_OCTRXH */
-			val = __raw_readl(bp->regs + offset + 4);
+			val = readl_relaxed(bp->regs + offset + 4);
 			bp->ethtool_stats[i] += ((u64)val) << 32;
 			*(++p) += val;
 		}
@@ -1917,7 +1936,7 @@ static void gem_get_ethtool_strings(struct net_device *dev, u32 sset, u8 *p)
 	}
 }
 
-struct net_device_stats *macb_get_stats(struct net_device *dev)
+static struct net_device_stats *macb_get_stats(struct net_device *dev)
 {
 	struct macb *bp = netdev_priv(dev);
 	struct net_device_stats *nstat = &bp->stats;
@@ -1937,12 +1956,12 @@ struct net_device_stats *macb_get_stats(struct net_device *dev)
 			    hwstat->rx_oversize_pkts +
 			    hwstat->rx_jabbers +
 			    hwstat->rx_undersize_pkts +
-			    hwstat->sqe_test_errors +
 			    hwstat->rx_length_mismatch);
 	nstat->tx_errors = (hwstat->tx_late_cols +
 			    hwstat->tx_excessive_cols +
 			    hwstat->tx_underruns +
-			    hwstat->tx_carrier_errors);
+			    hwstat->tx_carrier_errors +
+			    hwstat->sqe_test_errors);
 	nstat->collisions = (hwstat->tx_single_cols +
 			     hwstat->tx_multiple_cols +
 			     hwstat->tx_excessive_cols);
@@ -1963,7 +1982,6 @@ struct net_device_stats *macb_get_stats(struct net_device *dev)
 
 	return nstat;
 }
-EXPORT_SYMBOL_GPL(macb_get_stats);
 
 static int macb_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
@@ -2019,13 +2037,13 @@ static void macb_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 	regs_buff[10] = macb_tx_dma(&bp->queues[0], tail);
 	regs_buff[11] = macb_tx_dma(&bp->queues[0], head);
 
+	regs_buff[12] = macb_or_gem_readl(bp, USRIO);
 	if (macb_is_gem(bp)) {
-		regs_buff[12] = gem_readl(bp, USRIO);
 		regs_buff[13] = gem_readl(bp, DMACFG);
 	}
 }
 
-const struct ethtool_ops macb_ethtool_ops = {
+static const struct ethtool_ops macb_ethtool_ops = {
 	.get_settings		= macb_get_settings,
 	.set_settings		= macb_set_settings,
 	.get_regs_len		= macb_get_regs_len,
@@ -2033,7 +2051,6 @@ const struct ethtool_ops macb_ethtool_ops = {
 	.get_link		= ethtool_op_get_link,
 	.get_ts_info		= ethtool_op_get_ts_info,
 };
-EXPORT_SYMBOL_GPL(macb_ethtool_ops);
 
 static const struct ethtool_ops gem_ethtool_ops = {
 	.get_settings		= macb_get_settings,
@@ -2047,7 +2064,7 @@ static const struct ethtool_ops gem_ethtool_ops = {
 	.get_sset_count		= gem_get_sset_count,
 };
 
-int macb_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+static int macb_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
 	struct macb *bp = netdev_priv(dev);
 	struct phy_device *phydev = bp->phy_dev;
@@ -2060,7 +2077,6 @@ int macb_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 
 	return phy_mii_ioctl(phydev, rq, cmd);
 }
-EXPORT_SYMBOL_GPL(macb_ioctl);
 
 static int macb_set_features(struct net_device *netdev,
 			     netdev_features_t features)
@@ -2112,63 +2128,20 @@ static const struct net_device_ops macb_netdev_ops = {
 	.ndo_set_features	= macb_set_features,
 };
 
-#if defined(CONFIG_OF)
-static const struct macb_config pc302gem_config = {
-	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
-	.dma_burst_length = 16,
-};
-
-static const struct macb_config sama5d3_config = {
-	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
-	.dma_burst_length = 16,
-};
-
-static const struct macb_config sama5d4_config = {
-	.caps = 0,
-	.dma_burst_length = 4,
-};
-
-static const struct of_device_id macb_dt_ids[] = {
-	{ .compatible = "cdns,at32ap7000-macb" },
-	{ .compatible = "cdns,at91sam9260-macb" },
-	{ .compatible = "cdns,macb" },
-	{ .compatible = "cdns,pc302-gem", .data = &pc302gem_config },
-	{ .compatible = "cdns,gem", .data = &pc302gem_config },
-	{ .compatible = "atmel,sama5d3-gem", .data = &sama5d3_config },
-	{ .compatible = "atmel,sama5d4-gem", .data = &sama5d4_config },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, macb_dt_ids);
-#endif
-
 /*
- * Configure peripheral capacities according to device tree
+ * Configure peripheral capabilities according to device tree
  * and integration options used
  */
-static void macb_configure_caps(struct macb *bp)
+static void macb_configure_caps(struct macb *bp, const struct macb_config *dt_conf)
 {
 	u32 dcfg;
-	const struct of_device_id *match;
-	const struct macb_config *config;
 
-	if (bp->pdev->dev.of_node) {
-		match = of_match_node(macb_dt_ids, bp->pdev->dev.of_node);
-		if (match && match->data) {
-			config = match->data;
+	if (dt_conf)
+		bp->caps = dt_conf->caps;
 
-			bp->caps = config->caps;
-			/*
-			 * As we have access to the matching node, configure
-			 * DMA burst length as well
-			 */
-			bp->dma_burst_length = config->dma_burst_length;
-		}
-	}
-
-	if (MACB_BFEXT(IDNUM, macb_readl(bp, MID)) == 0x2)
+	if (macb_is_gem_hw(bp->regs)) {
 		bp->caps |= MACB_CAPS_MACB_IS_GEM;
 
-	if (macb_is_gem(bp)) {
 		dcfg = gem_readl(bp, DCFG1);
 		if (GEM_BFEXT(IRQCOR, dcfg) == 0)
 			bp->caps |= MACB_CAPS_ISR_CLEAR_ON_WRITE;
@@ -2185,18 +2158,22 @@ static void macb_probe_queues(void __iomem *mem,
 			      unsigned int *num_queues)
 {
 	unsigned int hw_q;
-	u32 mid;
 
 	*queue_mask = 0x1;
 	*num_queues = 1;
 
-	/* is it macb or gem ? */
-	mid = __raw_readl(mem + MACB_MID);
-	if (MACB_BFEXT(IDNUM, mid) != 0x2)
+	/* is it macb or gem ?
+	 *
+	 * We need to read directly from the hardware here because
+	 * we are early in the probe process and don't have the
+	 * MACB_CAPS_MACB_IS_GEM flag positioned
+	 */
+	if (!macb_is_gem_hw(mem))
 		return;
 
 	/* bit 0 is never set but queue 0 always exists */
-	*queue_mask = __raw_readl(mem + GEM_DCFG6) & 0xff;
+	*queue_mask = readl_relaxed(mem + GEM_DCFG6) & 0xff;
+
 	*queue_mask |= 0x1;
 
 	for (hw_q = 1; hw_q < MACB_MAX_QUEUES; ++hw_q)
@@ -2204,95 +2181,73 @@ static void macb_probe_queues(void __iomem *mem,
 			(*num_queues)++;
 }
 
-static int macb_probe(struct platform_device *pdev)
+static int macb_clk_init(struct platform_device *pdev, struct clk **pclk,
+			 struct clk **hclk, struct clk **tx_clk)
 {
-	struct macb_platform_data *pdata;
-	struct resource *regs;
-	struct net_device *dev;
-	struct macb *bp;
-	struct macb_queue *queue;
-	struct phy_device *phydev;
-	u32 config;
-	int err = -ENXIO;
-	const char *mac;
-	void __iomem *mem;
-	unsigned int hw_q, queue_mask, q, num_queues;
-	struct clk *pclk, *hclk, *tx_clk;
-
-	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!regs) {
-		dev_err(&pdev->dev, "no mmio resource defined\n");
-		goto err_out;
-	}
+	int err;
 
-	pclk = devm_clk_get(&pdev->dev, "pclk");
-	if (IS_ERR(pclk)) {
-		err = PTR_ERR(pclk);
+	*pclk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(*pclk)) {
+		err = PTR_ERR(*pclk);
 		dev_err(&pdev->dev, "failed to get macb_clk (%u)\n", err);
-		goto err_out;
+		return err;
 	}
 
-	hclk = devm_clk_get(&pdev->dev, "hclk");
-	if (IS_ERR(hclk)) {
-		err = PTR_ERR(hclk);
+	*hclk = devm_clk_get(&pdev->dev, "hclk");
+	if (IS_ERR(*hclk)) {
+		err = PTR_ERR(*hclk);
 		dev_err(&pdev->dev, "failed to get hclk (%u)\n", err);
-		goto err_out;
+		return err;
 	}
 
-	tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
+	*tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
+	if (IS_ERR(*tx_clk))
+		*tx_clk = NULL;
 
-	err = clk_prepare_enable(pclk);
+	err = clk_prepare_enable(*pclk);
 	if (err) {
 		dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
-		goto err_out;
+		return err;
 	}
 
-	err = clk_prepare_enable(hclk);
+	err = clk_prepare_enable(*hclk);
 	if (err) {
 		dev_err(&pdev->dev, "failed to enable hclk (%u)\n", err);
-		goto err_out_disable_pclk;
+		goto err_disable_pclk;
 	}
 
-	if (!IS_ERR(tx_clk)) {
-		err = clk_prepare_enable(tx_clk);
-		if (err) {
-			dev_err(&pdev->dev, "failed to enable tx_clk (%u)\n",
-				err);
-			goto err_out_disable_hclk;
-		}
+	err = clk_prepare_enable(*tx_clk);
+	if (err) {
+		dev_err(&pdev->dev, "failed to enable tx_clk (%u)\n", err);
+		goto err_disable_hclk;
 	}
 
-	err = -ENOMEM;
-	mem = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
-	if (!mem) {
-		dev_err(&pdev->dev, "failed to map registers, aborting.\n");
-		goto err_out_disable_clocks;
-	}
+	return 0;
 
-	macb_probe_queues(mem, &queue_mask, &num_queues);
-	dev = alloc_etherdev_mq(sizeof(*bp), num_queues);
-	if (!dev)
-		goto err_out_disable_clocks;
+err_disable_hclk:
+	clk_disable_unprepare(*hclk);
 
-	SET_NETDEV_DEV(dev, &pdev->dev);
+err_disable_pclk:
+	clk_disable_unprepare(*pclk);
 
-	bp = netdev_priv(dev);
-	bp->pdev = pdev;
-	bp->dev = dev;
-	bp->regs = mem;
-	bp->num_queues = num_queues;
-	bp->pclk = pclk;
-	bp->hclk = hclk;
-	bp->tx_clk = tx_clk;
+	return err;
+}
 
-	spin_lock_init(&bp->lock);
+static int macb_init(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	unsigned int hw_q, q;
+	struct macb *bp = netdev_priv(dev);
+	struct macb_queue *queue;
+	int err;
+	u32 val;
 
 	/* set the queue register mapping once for all: queue0 has a special
 	 * register mapping but we don't want to test the queue index then
 	 * compute the corresponding register offset at run time.
 	 */
 	for (hw_q = 0, q = 0; hw_q < MACB_MAX_QUEUES; ++hw_q) {
-		if (!(queue_mask & (1 << hw_q)))
+		if (!(bp->queue_mask & (1 << hw_q)))
 			continue;
 
 		queue = &bp->queues[q];
@@ -2319,27 +2274,21 @@ static int macb_probe(struct platform_device *pdev)
 		 */
 		queue->irq = platform_get_irq(pdev, q);
 		err = devm_request_irq(&pdev->dev, queue->irq, macb_interrupt,
-				       0, dev->name, queue);
+				       IRQF_SHARED, dev->name, queue);
 		if (err) {
 			dev_err(&pdev->dev,
 				"Unable to request IRQ %d (error %d)\n",
 				queue->irq, err);
-			goto err_out_free_netdev;
+			return err;
 		}
 
 		INIT_WORK(&queue->tx_error_task, macb_tx_error_task);
 		q++;
 	}
-	dev->irq = bp->queues[0].irq;
 
 	dev->netdev_ops = &macb_netdev_ops;
 	netif_napi_add(dev, &bp->napi, macb_poll, 64);
 
-	dev->base_addr = regs->start;
-
-	/* setup capacities */
-	macb_configure_caps(bp);
-
 	/* setup appropriated routines according to adapter type */
 	if (macb_is_gem(bp)) {
 		bp->max_tx_length = GEM_MAX_TX_LEN;
@@ -2366,18 +2315,470 @@ static int macb_probe(struct platform_device *pdev)
 		dev->hw_features &= ~NETIF_F_SG;
 	dev->features = dev->hw_features;
 
+	val = 0;
+	if (bp->phy_interface == PHY_INTERFACE_MODE_RGMII)
+		val = GEM_BIT(RGMII);
+	else if (bp->phy_interface == PHY_INTERFACE_MODE_RMII &&
+		 (bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII))
+		val = MACB_BIT(RMII);
+	else if (!(bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII))
+		val = MACB_BIT(MII);
+
+	if (bp->caps & MACB_CAPS_USRIO_HAS_CLKEN)
+		val |= MACB_BIT(CLKEN);
+
+	macb_or_gem_writel(bp, USRIO, val);
+
 	/* Set MII management clock divider */
-	config = macb_mdc_clk_div(bp);
-	config |= macb_dbw(bp);
-	macb_writel(bp, NCFGR, config);
+	val = macb_mdc_clk_div(bp);
+	val |= macb_dbw(bp);
+	macb_writel(bp, NCFGR, val);
+
+	return 0;
+}
+
+#if defined(CONFIG_OF)
+/* 1518 rounded up */
+#define AT91ETHER_MAX_RBUFF_SZ	0x600
+/* max number of receive buffers */
+#define AT91ETHER_MAX_RX_DESCR	9
+
+/* Initialize and start the Receiver and Transmit subsystems */
+static int at91ether_start(struct net_device *dev)
+{
+	struct macb *lp = netdev_priv(dev);
+	dma_addr_t addr;
+	u32 ctl;
+	int i;
+
+	lp->rx_ring = dma_alloc_coherent(&lp->pdev->dev,
+					 (AT91ETHER_MAX_RX_DESCR *
+					  sizeof(struct macb_dma_desc)),
+					 &lp->rx_ring_dma, GFP_KERNEL);
+	if (!lp->rx_ring)
+		return -ENOMEM;
+
+	lp->rx_buffers = dma_alloc_coherent(&lp->pdev->dev,
+					    AT91ETHER_MAX_RX_DESCR *
+					    AT91ETHER_MAX_RBUFF_SZ,
+					    &lp->rx_buffers_dma, GFP_KERNEL);
+	if (!lp->rx_buffers) {
+		dma_free_coherent(&lp->pdev->dev,
+				  AT91ETHER_MAX_RX_DESCR *
+				  sizeof(struct macb_dma_desc),
+				  lp->rx_ring, lp->rx_ring_dma);
+		lp->rx_ring = NULL;
+		return -ENOMEM;
+	}
+
+	addr = lp->rx_buffers_dma;
+	for (i = 0; i < AT91ETHER_MAX_RX_DESCR; i++) {
+		lp->rx_ring[i].addr = addr;
+		lp->rx_ring[i].ctrl = 0;
+		addr += AT91ETHER_MAX_RBUFF_SZ;
+	}
+
+	/* Set the Wrap bit on the last descriptor */
+	lp->rx_ring[AT91ETHER_MAX_RX_DESCR - 1].addr |= MACB_BIT(RX_WRAP);
+
+	/* Reset buffer index */
+	lp->rx_tail = 0;
+
+	/* Program address of descriptor list in Rx Buffer Queue register */
+	macb_writel(lp, RBQP, lp->rx_ring_dma);
+
+	/* Enable Receive and Transmit */
+	ctl = macb_readl(lp, NCR);
+	macb_writel(lp, NCR, ctl | MACB_BIT(RE) | MACB_BIT(TE));
+
+	return 0;
+}
+
+/* Open the ethernet interface */
+static int at91ether_open(struct net_device *dev)
+{
+	struct macb *lp = netdev_priv(dev);
+	u32 ctl;
+	int ret;
+
+	/* Clear internal statistics */
+	ctl = macb_readl(lp, NCR);
+	macb_writel(lp, NCR, ctl | MACB_BIT(CLRSTAT));
+
+	macb_set_hwaddr(lp);
+
+	ret = at91ether_start(dev);
+	if (ret)
+		return ret;
+
+	/* Enable MAC interrupts */
+	macb_writel(lp, IER, MACB_BIT(RCOMP)	|
+			     MACB_BIT(RXUBR)	|
+			     MACB_BIT(ISR_TUND)	|
+			     MACB_BIT(ISR_RLE)	|
+			     MACB_BIT(TCOMP)	|
+			     MACB_BIT(ISR_ROVR)	|
+			     MACB_BIT(HRESP));
+
+	/* schedule a link state check */
+	phy_start(lp->phy_dev);
+
+	netif_start_queue(dev);
+
+	return 0;
+}
+
+/* Close the interface */
+static int at91ether_close(struct net_device *dev)
+{
+	struct macb *lp = netdev_priv(dev);
+	u32 ctl;
+
+	/* Disable Receiver and Transmitter */
+	ctl = macb_readl(lp, NCR);
+	macb_writel(lp, NCR, ctl & ~(MACB_BIT(TE) | MACB_BIT(RE)));
+
+	/* Disable MAC interrupts */
+	macb_writel(lp, IDR, MACB_BIT(RCOMP)	|
+			     MACB_BIT(RXUBR)	|
+			     MACB_BIT(ISR_TUND)	|
+			     MACB_BIT(ISR_RLE)	|
+			     MACB_BIT(TCOMP)	|
+			     MACB_BIT(ISR_ROVR) |
+			     MACB_BIT(HRESP));
+
+	netif_stop_queue(dev);
+
+	dma_free_coherent(&lp->pdev->dev,
+			  AT91ETHER_MAX_RX_DESCR *
+			  sizeof(struct macb_dma_desc),
+			  lp->rx_ring, lp->rx_ring_dma);
+	lp->rx_ring = NULL;
+
+	dma_free_coherent(&lp->pdev->dev,
+			  AT91ETHER_MAX_RX_DESCR * AT91ETHER_MAX_RBUFF_SZ,
+			  lp->rx_buffers, lp->rx_buffers_dma);
+	lp->rx_buffers = NULL;
+
+	return 0;
+}
+
+/* Transmit packet */
+static int at91ether_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct macb *lp = netdev_priv(dev);
+
+	if (macb_readl(lp, TSR) & MACB_BIT(RM9200_BNQ)) {
+		netif_stop_queue(dev);
+
+		/* Store packet information (to free when Tx completed) */
+		lp->skb = skb;
+		lp->skb_length = skb->len;
+		lp->skb_physaddr = dma_map_single(NULL, skb->data, skb->len,
+							DMA_TO_DEVICE);
+
+		/* Set address of the data in the Transmit Address register */
+		macb_writel(lp, TAR, lp->skb_physaddr);
+		/* Set length of the packet in the Transmit Control register */
+		macb_writel(lp, TCR, skb->len);
+
+	} else {
+		netdev_err(dev, "%s called, but device is busy!\n", __func__);
+		return NETDEV_TX_BUSY;
+	}
+
+	return NETDEV_TX_OK;
+}
 
-	mac = of_get_mac_address(pdev->dev.of_node);
+/* Extract received frame from buffer descriptors and sent to upper layers.
+ * (Called from interrupt context)
+ */
+static void at91ether_rx(struct net_device *dev)
+{
+	struct macb *lp = netdev_priv(dev);
+	unsigned char *p_recv;
+	struct sk_buff *skb;
+	unsigned int pktlen;
+
+	while (lp->rx_ring[lp->rx_tail].addr & MACB_BIT(RX_USED)) {
+		p_recv = lp->rx_buffers + lp->rx_tail * AT91ETHER_MAX_RBUFF_SZ;
+		pktlen = MACB_BF(RX_FRMLEN, lp->rx_ring[lp->rx_tail].ctrl);
+		skb = netdev_alloc_skb(dev, pktlen + 2);
+		if (skb) {
+			skb_reserve(skb, 2);
+			memcpy(skb_put(skb, pktlen), p_recv, pktlen);
+
+			skb->protocol = eth_type_trans(skb, dev);
+			lp->stats.rx_packets++;
+			lp->stats.rx_bytes += pktlen;
+			netif_rx(skb);
+		} else {
+			lp->stats.rx_dropped++;
+		}
+
+		if (lp->rx_ring[lp->rx_tail].ctrl & MACB_BIT(RX_MHASH_MATCH))
+			lp->stats.multicast++;
+
+		/* reset ownership bit */
+		lp->rx_ring[lp->rx_tail].addr &= ~MACB_BIT(RX_USED);
+
+		/* wrap after last buffer */
+		if (lp->rx_tail == AT91ETHER_MAX_RX_DESCR - 1)
+			lp->rx_tail = 0;
+		else
+			lp->rx_tail++;
+	}
+}
+
+/* MAC interrupt handler */
+static irqreturn_t at91ether_interrupt(int irq, void *dev_id)
+{
+	struct net_device *dev = dev_id;
+	struct macb *lp = netdev_priv(dev);
+	u32 intstatus, ctl;
+
+	/* MAC Interrupt Status register indicates what interrupts are pending.
+	 * It is automatically cleared once read.
+	 */
+	intstatus = macb_readl(lp, ISR);
+
+	/* Receive complete */
+	if (intstatus & MACB_BIT(RCOMP))
+		at91ether_rx(dev);
+
+	/* Transmit complete */
+	if (intstatus & MACB_BIT(TCOMP)) {
+		/* The TCOM bit is set even if the transmission failed */
+		if (intstatus & (MACB_BIT(ISR_TUND) | MACB_BIT(ISR_RLE)))
+			lp->stats.tx_errors++;
+
+		if (lp->skb) {
+			dev_kfree_skb_irq(lp->skb);
+			lp->skb = NULL;
+			dma_unmap_single(NULL, lp->skb_physaddr,
+					 lp->skb_length, DMA_TO_DEVICE);
+			lp->stats.tx_packets++;
+			lp->stats.tx_bytes += lp->skb_length;
+		}
+		netif_wake_queue(dev);
+	}
+
+	/* Work-around for EMAC Errata section 41.3.1 */
+	if (intstatus & MACB_BIT(RXUBR)) {
+		ctl = macb_readl(lp, NCR);
+		macb_writel(lp, NCR, ctl & ~MACB_BIT(RE));
+		macb_writel(lp, NCR, ctl | MACB_BIT(RE));
+	}
+
+	if (intstatus & MACB_BIT(ISR_ROVR))
+		netdev_err(dev, "ROVR error\n");
+
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void at91ether_poll_controller(struct net_device *dev)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+	at91ether_interrupt(dev->irq, dev);
+	local_irq_restore(flags);
+}
+#endif
+
+static const struct net_device_ops at91ether_netdev_ops = {
+	.ndo_open		= at91ether_open,
+	.ndo_stop		= at91ether_close,
+	.ndo_start_xmit		= at91ether_start_xmit,
+	.ndo_get_stats		= macb_get_stats,
+	.ndo_set_rx_mode	= macb_set_rx_mode,
+	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_do_ioctl		= macb_ioctl,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_change_mtu		= eth_change_mtu,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= at91ether_poll_controller,
+#endif
+};
+
+static int at91ether_clk_init(struct platform_device *pdev, struct clk **pclk,
+			      struct clk **hclk, struct clk **tx_clk)
+{
+	int err;
+
+	*hclk = NULL;
+	*tx_clk = NULL;
+
+	*pclk = devm_clk_get(&pdev->dev, "ether_clk");
+	if (IS_ERR(*pclk))
+		return PTR_ERR(*pclk);
+
+	err = clk_prepare_enable(*pclk);
+	if (err) {
+		dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
+		return err;
+	}
+
+	return 0;
+}
+
+static int at91ether_init(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct macb *bp = netdev_priv(dev);
+	int err;
+	u32 reg;
+
+	dev->netdev_ops = &at91ether_netdev_ops;
+	dev->ethtool_ops = &macb_ethtool_ops;
+
+	err = devm_request_irq(&pdev->dev, dev->irq, at91ether_interrupt,
+			       0, dev->name, dev);
+	if (err)
+		return err;
+
+	macb_writel(bp, NCR, 0);
+
+	reg = MACB_BF(CLK, MACB_CLK_DIV32) | MACB_BIT(BIG);
+	if (bp->phy_interface == PHY_INTERFACE_MODE_RMII)
+		reg |= MACB_BIT(RM9200_RMII);
+
+	macb_writel(bp, NCFGR, reg);
+
+	return 0;
+}
+
+static const struct macb_config at91sam9260_config = {
+	.caps = MACB_CAPS_USRIO_HAS_CLKEN | MACB_CAPS_USRIO_DEFAULT_IS_MII,
+	.clk_init = macb_clk_init,
+	.init = macb_init,
+};
+
+static const struct macb_config pc302gem_config = {
+	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
+	.dma_burst_length = 16,
+	.clk_init = macb_clk_init,
+	.init = macb_init,
+};
+
+static const struct macb_config sama5d3_config = {
+	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
+	.dma_burst_length = 16,
+	.clk_init = macb_clk_init,
+	.init = macb_init,
+};
+
+static const struct macb_config sama5d4_config = {
+	.caps = 0,
+	.dma_burst_length = 4,
+	.clk_init = macb_clk_init,
+	.init = macb_init,
+};
+
+static const struct macb_config emac_config = {
+	.clk_init = at91ether_clk_init,
+	.init = at91ether_init,
+};
+
+static const struct of_device_id macb_dt_ids[] = {
+	{ .compatible = "cdns,at32ap7000-macb" },
+	{ .compatible = "cdns,at91sam9260-macb", .data = &at91sam9260_config },
+	{ .compatible = "cdns,macb" },
+	{ .compatible = "cdns,pc302-gem", .data = &pc302gem_config },
+	{ .compatible = "cdns,gem", .data = &pc302gem_config },
+	{ .compatible = "atmel,sama5d3-gem", .data = &sama5d3_config },
+	{ .compatible = "atmel,sama5d4-gem", .data = &sama5d4_config },
+	{ .compatible = "cdns,at91rm9200-emac", .data = &emac_config },
+	{ .compatible = "cdns,emac", .data = &emac_config },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, macb_dt_ids);
+#endif /* CONFIG_OF */
+
+static int macb_probe(struct platform_device *pdev)
+{
+	int (*clk_init)(struct platform_device *, struct clk **,
+			struct clk **, struct clk **)
+					      = macb_clk_init;
+	int (*init)(struct platform_device *) = macb_init;
+	struct device_node *np = pdev->dev.of_node;
+	const struct macb_config *macb_config = NULL;
+	struct clk *pclk, *hclk, *tx_clk;
+	unsigned int queue_mask, num_queues;
+	struct macb_platform_data *pdata;
+	struct phy_device *phydev;
+	struct net_device *dev;
+	struct resource *regs;
+	void __iomem *mem;
+	const char *mac;
+	struct macb *bp;
+	int err;
+
+	if (np) {
+		const struct of_device_id *match;
+
+		match = of_match_node(macb_dt_ids, np);
+		if (match && match->data) {
+			macb_config = match->data;
+			clk_init = macb_config->clk_init;
+			init = macb_config->init;
+		}
+	}
+
+	err = clk_init(pdev, &pclk, &hclk, &tx_clk);
+	if (err)
+		return err;
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mem = devm_ioremap_resource(&pdev->dev, regs);
+	if (IS_ERR(mem)) {
+		err = PTR_ERR(mem);
+		goto err_disable_clocks;
+	}
+
+	macb_probe_queues(mem, &queue_mask, &num_queues);
+	dev = alloc_etherdev_mq(sizeof(*bp), num_queues);
+	if (!dev) {
+		err = -ENOMEM;
+		goto err_disable_clocks;
+	}
+
+	dev->base_addr = regs->start;
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	bp = netdev_priv(dev);
+	bp->pdev = pdev;
+	bp->dev = dev;
+	bp->regs = mem;
+	bp->num_queues = num_queues;
+	bp->queue_mask = queue_mask;
+	if (macb_config)
+		bp->dma_burst_length = macb_config->dma_burst_length;
+	bp->pclk = pclk;
+	bp->hclk = hclk;
+	bp->tx_clk = tx_clk;
+	spin_lock_init(&bp->lock);
+
+	/* setup capabilities */
+	macb_configure_caps(bp, macb_config);
+
+	platform_set_drvdata(pdev, dev);
+
+	dev->irq = platform_get_irq(pdev, 0);
+	if (dev->irq < 0) {
+		err = dev->irq;
+		goto err_disable_clocks;
+	}
+
+	mac = of_get_mac_address(np);
 	if (mac)
 		memcpy(bp->dev->dev_addr, mac, ETH_ALEN);
 	else
 		macb_get_hwaddr(bp);
 
-	err = of_get_phy_mode(pdev->dev.of_node);
+	err = of_get_phy_mode(np);
 	if (err < 0) {
 		pdata = dev_get_platdata(&pdev->dev);
 		if (pdata && pdata->is_rmii)
@@ -2388,34 +2789,21 @@ static int macb_probe(struct platform_device *pdev)
 		bp->phy_interface = err;
 	}
 
-	if (bp->phy_interface == PHY_INTERFACE_MODE_RGMII)
-		macb_or_gem_writel(bp, USRIO, GEM_BIT(RGMII));
-	else if (bp->phy_interface == PHY_INTERFACE_MODE_RMII)
-#if defined(CONFIG_ARCH_AT91)
-		macb_or_gem_writel(bp, USRIO, (MACB_BIT(RMII) |
-					       MACB_BIT(CLKEN)));
-#else
-		macb_or_gem_writel(bp, USRIO, 0);
-#endif
-	else
-#if defined(CONFIG_ARCH_AT91)
-		macb_or_gem_writel(bp, USRIO, MACB_BIT(CLKEN));
-#else
-		macb_or_gem_writel(bp, USRIO, MACB_BIT(MII));
-#endif
+	/* IP specific init */
+	err = init(pdev);
+	if (err)
+		goto err_out_free_netdev;
 
 	err = register_netdev(dev);
 	if (err) {
 		dev_err(&pdev->dev, "Cannot register net device, aborting.\n");
-		goto err_out_free_netdev;
+		goto err_out_unregister_netdev;
 	}
 
 	err = macb_mii_init(bp);
 	if (err)
 		goto err_out_unregister_netdev;
 
-	platform_set_drvdata(pdev, dev);
-
 	netif_carrier_off(dev);
 
 	netdev_info(dev, "Cadence %s rev 0x%08x at 0x%08lx irq %d (%pM)\n",
@@ -2430,16 +2818,15 @@ static int macb_probe(struct platform_device *pdev)
 
 err_out_unregister_netdev:
 	unregister_netdev(dev);
+
 err_out_free_netdev:
 	free_netdev(dev);
-err_out_disable_clocks:
-	if (!IS_ERR(tx_clk))
-		clk_disable_unprepare(tx_clk);
-err_out_disable_hclk:
+
+err_disable_clocks:
+	clk_disable_unprepare(tx_clk);
 	clk_disable_unprepare(hclk);
-err_out_disable_pclk:
 	clk_disable_unprepare(pclk);
-err_out:
+
 	return err;
 }
 
@@ -2458,8 +2845,7 @@ static int macb_remove(struct platform_device *pdev)
 		kfree(bp->mii_bus->irq);
 		mdiobus_free(bp->mii_bus);
 		unregister_netdev(dev);
-		if (!IS_ERR(bp->tx_clk))
-			clk_disable_unprepare(bp->tx_clk);
+		clk_disable_unprepare(bp->tx_clk);
 		clk_disable_unprepare(bp->hclk);
 		clk_disable_unprepare(bp->pclk);
 		free_netdev(dev);
@@ -2477,8 +2863,7 @@ static int __maybe_unused macb_suspend(struct device *dev)
 	netif_carrier_off(netdev);
 	netif_device_detach(netdev);
 
-	if (!IS_ERR(bp->tx_clk))
-		clk_disable_unprepare(bp->tx_clk);
+	clk_disable_unprepare(bp->tx_clk);
 	clk_disable_unprepare(bp->hclk);
 	clk_disable_unprepare(bp->pclk);
 
@@ -2493,8 +2878,7 @@ static int __maybe_unused macb_resume(struct device *dev)
 
 	clk_prepare_enable(bp->pclk);
 	clk_prepare_enable(bp->hclk);
-	if (!IS_ERR(bp->tx_clk))
-		clk_prepare_enable(bp->tx_clk);
+	clk_prepare_enable(bp->tx_clk);
 
 	netif_device_attach(netdev);
 
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index ff85619a9732..eb7d76f7bf6a 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -11,7 +11,7 @@
 #define _MACB_H
 
 #define MACB_GREGS_NBR 16
-#define MACB_GREGS_VERSION 1
+#define MACB_GREGS_VERSION 2
 #define MACB_MAX_QUEUES 8
 
 /* MACB register offsets */
@@ -229,8 +229,10 @@
 /* Bitfields in DMACFG. */
 #define GEM_FBLDO_OFFSET	0 /* fixed burst length for DMA */
 #define GEM_FBLDO_SIZE		5
-#define GEM_ENDIA_OFFSET	7 /* endian swap mode for packet data access */
-#define GEM_ENDIA_SIZE		1
+#define GEM_ENDIA_DESC_OFFSET	6 /* endian swap mode for management descriptor access */
+#define GEM_ENDIA_DESC_SIZE	1
+#define GEM_ENDIA_PKT_OFFSET	7 /* endian swap mode for packet data access */
+#define GEM_ENDIA_PKT_SIZE	1
 #define GEM_RXBMS_OFFSET	8 /* RX packet buffer memory size select */
 #define GEM_RXBMS_SIZE		2
 #define GEM_TXPBMS_OFFSET	10 /* TX packet buffer memory size select */
@@ -389,6 +391,8 @@
 
 /* Capability mask bits */
 #define MACB_CAPS_ISR_CLEAR_ON_WRITE		0x00000001
+#define MACB_CAPS_USRIO_HAS_CLKEN		0x00000002
+#define MACB_CAPS_USRIO_DEFAULT_IS_MII		0x00000004
 #define MACB_CAPS_FIFO_MODE			0x10000000
 #define MACB_CAPS_GIGABIT_MODE_AVAILABLE	0x20000000
 #define MACB_CAPS_SG_DISABLED			0x40000000
@@ -423,17 +427,17 @@
 
 /* Register access macros */
 #define macb_readl(port,reg)				\
-	__raw_readl((port)->regs + MACB_##reg)
+	readl_relaxed((port)->regs + MACB_##reg)
 #define macb_writel(port,reg,value)			\
-	__raw_writel((value), (port)->regs + MACB_##reg)
+	writel_relaxed((value), (port)->regs + MACB_##reg)
 #define gem_readl(port, reg)				\
-	__raw_readl((port)->regs + GEM_##reg)
+	readl_relaxed((port)->regs + GEM_##reg)
 #define gem_writel(port, reg, value)			\
-	__raw_writel((value), (port)->regs + GEM_##reg)
+	writel_relaxed((value), (port)->regs + GEM_##reg)
 #define queue_readl(queue, reg)				\
-	__raw_readl((queue)->bp->regs + (queue)->reg)
+	readl_relaxed((queue)->bp->regs + (queue)->reg)
 #define queue_writel(queue, reg, value)			\
-	__raw_writel((value), (queue)->bp->regs + (queue)->reg)
+	writel_relaxed((value), (queue)->bp->regs + (queue)->reg)
 
 /* Conditional GEM/MACB macros.  These perform the operation to the correct
  * register dependent on whether the device is a GEM or a MACB.  For registers
@@ -750,6 +754,9 @@ struct macb_or_gem_ops {
 struct macb_config {
 	u32			caps;
 	unsigned int		dma_burst_length;
+	int	(*clk_init)(struct platform_device *pdev, struct clk **pclk,
+			    struct clk **hclk, struct clk **tx_clk);
+	int	(*init)(struct platform_device *pdev);
 };
 
 struct macb_queue {
@@ -780,6 +787,7 @@ struct macb {
 	size_t			rx_buffer_size;
 
 	unsigned int		num_queues;
+	unsigned int		queue_mask;
 	struct macb_queue	queues[MACB_MAX_QUEUES];
 
 	spinlock_t		lock;
@@ -820,18 +828,14 @@ struct macb {
 	u64			ethtool_stats[GEM_STATS_LEN];
 };
 
-extern const struct ethtool_ops macb_ethtool_ops;
-
-int macb_mii_init(struct macb *bp);
-int macb_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
-struct net_device_stats *macb_get_stats(struct net_device *dev);
-void macb_set_rx_mode(struct net_device *dev);
-void macb_set_hwaddr(struct macb *bp);
-void macb_get_hwaddr(struct macb *bp);
-
 static inline bool macb_is_gem(struct macb *bp)
 {
 	return !!(bp->caps & MACB_CAPS_MACB_IS_GEM);
 }
 
+static inline bool macb_is_gem_hw(void __iomem *addr)
+{
+	return !!(MACB_BFEXT(IDNUM, readl_relaxed(addr + MACB_MID)) >= 0x2);
+}
+
 #endif /* _MACB_H */
diff --git a/drivers/net/ethernet/calxeda/xgmac.c b/drivers/net/ethernet/calxeda/xgmac.c
index 47bfea24b9e1..63efa0dc45ba 100644
--- a/drivers/net/ethernet/calxeda/xgmac.c
+++ b/drivers/net/ethernet/calxeda/xgmac.c
@@ -47,9 +47,9 @@
 #define XGMAC_REMOTE_WAKE	0x00000700	/* Remote Wake-Up Frm Filter */
 #define XGMAC_PMT		0x00000704	/* PMT Control and Status */
 #define XGMAC_MMC_CTRL		0x00000800	/* XGMAC MMC Control */
-#define XGMAC_MMC_INTR_RX	0x00000804	/* Recieve Interrupt */
+#define XGMAC_MMC_INTR_RX	0x00000804	/* Receive Interrupt */
 #define XGMAC_MMC_INTR_TX	0x00000808	/* Transmit Interrupt */
-#define XGMAC_MMC_INTR_MASK_RX	0x0000080c	/* Recieve Interrupt Mask */
+#define XGMAC_MMC_INTR_MASK_RX	0x0000080c	/* Receive Interrupt Mask */
 #define XGMAC_MMC_INTR_MASK_TX	0x00000810	/* Transmit Interrupt Mask */
 
 /* Hardware TX Statistics Counters */
@@ -153,7 +153,7 @@
 #define XGMAC_FLOW_CTRL_PT_MASK	0xffff0000	/* Pause Time Mask */
 #define XGMAC_FLOW_CTRL_PT_SHIFT	16
 #define XGMAC_FLOW_CTRL_DZQP	0x00000080	/* Disable Zero-Quanta Phase */
-#define XGMAC_FLOW_CTRL_PLT	0x00000020	/* Pause Low Threshhold */
+#define XGMAC_FLOW_CTRL_PLT	0x00000020	/* Pause Low Threshold */
 #define XGMAC_FLOW_CTRL_PLT_MASK 0x00000030	/* PLT MASK */
 #define XGMAC_FLOW_CTRL_UP	0x00000008	/* Unicast Pause Frame Detect */
 #define XGMAC_FLOW_CTRL_RFE	0x00000004	/* Rx Flow Control Enable */
@@ -254,18 +254,18 @@
 /* XGMAC Operation Mode Register */
 #define XGMAC_OMR_TSF		0x00200000	/* TX FIFO Store and Forward */
 #define XGMAC_OMR_FTF		0x00100000	/* Flush Transmit FIFO */
-#define XGMAC_OMR_TTC		0x00020000	/* Transmit Threshhold Ctrl */
+#define XGMAC_OMR_TTC		0x00020000	/* Transmit Threshold Ctrl */
 #define XGMAC_OMR_TTC_MASK	0x00030000
-#define XGMAC_OMR_RFD		0x00006000	/* FC Deactivation Threshhold */
-#define XGMAC_OMR_RFD_MASK	0x00007000	/* FC Deact Threshhold MASK */
-#define XGMAC_OMR_RFA		0x00000600	/* FC Activation Threshhold */
-#define XGMAC_OMR_RFA_MASK	0x00000E00	/* FC Act Threshhold MASK */
+#define XGMAC_OMR_RFD		0x00006000	/* FC Deactivation Threshold */
+#define XGMAC_OMR_RFD_MASK	0x00007000	/* FC Deact Threshold MASK */
+#define XGMAC_OMR_RFA		0x00000600	/* FC Activation Threshold */
+#define XGMAC_OMR_RFA_MASK	0x00000E00	/* FC Act Threshold MASK */
 #define XGMAC_OMR_EFC		0x00000100	/* Enable Hardware FC */
 #define XGMAC_OMR_FEF		0x00000080	/* Forward Error Frames */
 #define XGMAC_OMR_DT		0x00000040	/* Drop TCP/IP csum Errors */
 #define XGMAC_OMR_RSF		0x00000020	/* RX FIFO Store and Forward */
-#define XGMAC_OMR_RTC_256	0x00000018	/* RX Threshhold Ctrl */
-#define XGMAC_OMR_RTC_MASK	0x00000018	/* RX Threshhold Ctrl MASK */
+#define XGMAC_OMR_RTC_256	0x00000018	/* RX Threshold Ctrl */
+#define XGMAC_OMR_RTC_MASK	0x00000018	/* RX Threshold Ctrl MASK */
 
 /* XGMAC HW Features Register */
 #define DMA_HW_FEAT_TXCOESEL	0x00010000	/* TX Checksum offload */
diff --git a/drivers/net/ethernet/chelsio/Kconfig b/drivers/net/ethernet/chelsio/Kconfig
index ac6473f75eb9..7daa088a9bb7 100644
--- a/drivers/net/ethernet/chelsio/Kconfig
+++ b/drivers/net/ethernet/chelsio/Kconfig
@@ -97,6 +97,17 @@ config CHELSIO_T4_DCB
 
 	  If unsure, say N.
 
+config CHELSIO_T4_FCOE
+	bool "Fibre Channel over Ethernet (FCoE) Support for Chelsio T5 cards"
+	default n
+	depends on CHELSIO_T4 && CHELSIO_T4_DCB && FCOE
+	---help---
+	  Enable FCoE offload features.
+	  Say Y here if you want to enable Fibre Channel over Ethernet (FCoE) support
+	  in the driver.
+
+	  If unsure, say N.
+
 config CHELSIO_T4VF
 	tristate "Chelsio Communications T4/T5 Virtual Function Ethernet support"
 	depends on PCI
diff --git a/drivers/net/ethernet/chelsio/cxgb/cxgb2.c b/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
index 186566bfdbc8..f5f1b0b51ebd 100644
--- a/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
+++ b/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
@@ -354,7 +354,7 @@ static void set_msglevel(struct net_device *dev, u32 val)
 	adapter->msg_enable = val;
 }
 
-static char stats_strings[][ETH_GSTRING_LEN] = {
+static const char stats_strings[][ETH_GSTRING_LEN] = {
 	"TxOctetsOK",
 	"TxOctetsBad",
 	"TxUnicastFramesOK",
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
index db76f7040455..b96e4bfcac41 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
@@ -1537,7 +1537,7 @@ static void set_msglevel(struct net_device *dev, u32 val)
 	adapter->msg_enable = val;
 }
 
-static char stats_strings[][ETH_GSTRING_LEN] = {
+static const char stats_strings[][ETH_GSTRING_LEN] = {
 	"TxOctetsOK         ",
 	"TxFramesOK         ",
 	"TxMulticastFramesOK",
diff --git a/drivers/net/ethernet/chelsio/cxgb3/sge.c b/drivers/net/ethernet/chelsio/cxgb3/sge.c
index d6aa602f168d..e4b5b057f417 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/sge.c
@@ -422,7 +422,7 @@ static inline int add_one_rx_buf(void *va, unsigned int len,
 
 	d->addr_lo = cpu_to_be32(mapping);
 	d->addr_hi = cpu_to_be32((u64) mapping >> 32);
-	wmb();
+	dma_wmb();
 	d->len_gen = cpu_to_be32(V_FLD_GEN1(gen));
 	d->gen2 = cpu_to_be32(V_FLD_GEN2(gen));
 	return 0;
@@ -433,7 +433,7 @@ static inline int add_one_rx_chunk(dma_addr_t mapping, struct rx_desc *d,
 {
 	d->addr_lo = cpu_to_be32(mapping);
 	d->addr_hi = cpu_to_be32((u64) mapping >> 32);
-	wmb();
+	dma_wmb();
 	d->len_gen = cpu_to_be32(V_FLD_GEN1(gen));
 	d->gen2 = cpu_to_be32(V_FLD_GEN2(gen));
 	return 0;
@@ -579,7 +579,7 @@ static void recycle_rx_buf(struct adapter *adap, struct sge_fl *q,
 	q->sdesc[q->pidx] = q->sdesc[idx];
 	to->addr_lo = from->addr_lo;	/* already big endian */
 	to->addr_hi = from->addr_hi;	/* likewise */
-	wmb();
+	dma_wmb();
 	to->len_gen = cpu_to_be32(V_FLD_GEN1(q->gen));
 	to->gen2 = cpu_to_be32(V_FLD_GEN2(q->gen));
 
@@ -1068,7 +1068,7 @@ static void write_wr_hdr_sgl(unsigned int ndesc, struct sk_buff *skb,
 		sd->eop = 1;
 		wrp->wr_hi = htonl(F_WR_SOP | F_WR_EOP | V_WR_DATATYPE(1) |
 				   V_WR_SGLSFLT(flits)) | wr_hi;
-		wmb();
+		dma_wmb();
 		wrp->wr_lo = htonl(V_WR_LEN(flits + sgl_flits) |
 				   V_WR_GEN(gen)) | wr_lo;
 		wr_gen2(d, gen);
@@ -1114,7 +1114,7 @@ static void write_wr_hdr_sgl(unsigned int ndesc, struct sk_buff *skb,
 		}
 		sd->eop = 1;
 		wrp->wr_hi |= htonl(F_WR_EOP);
-		wmb();
+		dma_wmb();
 		wp->wr_lo = htonl(V_WR_LEN(WR_FLITS) | V_WR_GEN(ogen)) | wr_lo;
 		wr_gen2((struct tx_desc *)wp, ogen);
 		WARN_ON(ndesc != 0);
@@ -1184,7 +1184,7 @@ static void write_tx_pkt_wr(struct adapter *adap, struct sk_buff *skb,
 			cpl->wr.wr_hi = htonl(V_WR_BCNTLFLT(skb->len & 7) |
 					      V_WR_OP(FW_WROPCODE_TUNNEL_TX_PKT)
 					      | F_WR_SOP | F_WR_EOP | compl);
-			wmb();
+			dma_wmb();
 			cpl->wr.wr_lo = htonl(V_WR_LEN(flits) | V_WR_GEN(gen) |
 					      V_WR_TID(q->token));
 			wr_gen2(d, gen);
@@ -1342,7 +1342,7 @@ static inline void write_imm(struct tx_desc *d, struct sk_buff *skb,
 
 	to->wr_hi = from->wr_hi | htonl(F_WR_SOP | F_WR_EOP |
 					V_WR_BCNTLFLT(len & 7));
-	wmb();
+	dma_wmb();
 	to->wr_lo = from->wr_lo | htonl(V_WR_GEN(gen) |
 					V_WR_LEN((len + 7) / 8));
 	wr_gen2(d, gen);
@@ -2271,7 +2271,7 @@ static int process_responses(struct adapter *adap, struct sge_qset *qs,
 		u32 len, flags;
 		__be32 rss_hi, rss_lo;
 
-		rmb();
+		dma_rmb();
 		eth = r->rss_hdr.opcode == CPL_RX_PKT;
 		rss_hi = *(const __be32 *)r;
 		rss_lo = r->rss_hdr.rss_hash_val;
@@ -2488,7 +2488,7 @@ static int process_pure_responses(struct adapter *adap, struct sge_qset *qs,
 		}
 		if (!is_new_response(r, q))
 			break;
-		rmb();
+		dma_rmb();
 	} while (is_pure_response(r));
 
 	if (sleeping)
@@ -2523,7 +2523,7 @@ static inline int handle_responses(struct adapter *adap, struct sge_rspq *q)
 
 	if (!is_new_response(r, q))
 		return -1;
-	rmb();
+	dma_rmb();
 	if (is_pure_response(r) && process_pure_responses(adap, qs, r) == 0) {
 		t3_write_reg(adap, A_SG_GTS, V_RSPQ(q->cntxt_id) |
 			     V_NEWTIMER(q->holdoff_tmr) | V_NEWINDEX(q->cidx));
diff --git a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
index 184a8d545ac4..a22768c94200 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
@@ -840,7 +840,7 @@ static int flash_wait_op(struct adapter *adapter, int attempts, int delay)
  *	Read the specified number of 32-bit words from the serial flash.
  *	If @byte_oriented is set the read data is stored as a byte array
  *	(i.e., big-endian), otherwise as 32-bit words in the platform's
- *	natural endianess.
+ *	natural endianness.
  */
 static int t3_read_flash(struct adapter *adapter, unsigned int addr,
 			 unsigned int nwords, u32 *data, int byte_oriented)
diff --git a/drivers/net/ethernet/chelsio/cxgb4/Makefile b/drivers/net/ethernet/chelsio/cxgb4/Makefile
index ae50cd72358c..ace0ab98d0f1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/Makefile
+++ b/drivers/net/ethernet/chelsio/cxgb4/Makefile
@@ -4,6 +4,7 @@
 
 obj-$(CONFIG_CHELSIO_T4) += cxgb4.o
 
-cxgb4-objs := cxgb4_main.o l2t.o t4_hw.o sge.o clip_tbl.o
+cxgb4-objs := cxgb4_main.o l2t.o t4_hw.o sge.o clip_tbl.o cxgb4_ethtool.o
 cxgb4-$(CONFIG_CHELSIO_T4_DCB) +=  cxgb4_dcb.o
+cxgb4-$(CONFIG_CHELSIO_T4_FCOE) +=  cxgb4_fcoe.o
 cxgb4-$(CONFIG_DEBUG_FS) += cxgb4_debugfs.o
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 97842d03675b..524d11098c56 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -60,6 +60,11 @@ enum {
 };
 
 enum {
+	T4_REGMAP_SIZE = (160 * 1024),
+	T5_REGMAP_SIZE = (332 * 1024),
+};
+
+enum {
 	MEM_EDC0,
 	MEM_EDC1,
 	MEM_MC,
@@ -369,15 +374,24 @@ enum {
 	MAX_OFLD_QSETS = 16,          /* # of offload Tx/Rx queue sets */
 	MAX_CTRL_QUEUES = NCHAN,      /* # of control Tx queues */
 	MAX_RDMA_QUEUES = NCHAN,      /* # of streaming RDMA Rx queues */
-	MAX_RDMA_CIQS = NCHAN,        /* # of  RDMA concentrator IQs */
+	MAX_RDMA_CIQS = 32,        /* # of  RDMA concentrator IQs */
 	MAX_ISCSI_QUEUES = NCHAN,     /* # of streaming iSCSI Rx queues */
 };
 
 enum {
+	MAX_TXQ_ENTRIES      = 16384,
+	MAX_CTRL_TXQ_ENTRIES = 1024,
+	MAX_RSPQ_ENTRIES     = 16384,
+	MAX_RX_BUFFERS       = 16384,
+	MIN_TXQ_ENTRIES      = 32,
+	MIN_CTRL_TXQ_ENTRIES = 32,
+	MIN_RSPQ_ENTRIES     = 128,
+	MIN_FL_ENTRIES       = 16
+};
+
+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,
 };
@@ -387,6 +401,10 @@ struct sge_rspq;
 
 #include "cxgb4_dcb.h"
 
+#ifdef CONFIG_CHELSIO_T4_FCOE
+#include "cxgb4_fcoe.h"
+#endif /* CONFIG_CHELSIO_T4_FCOE */
+
 struct port_info {
 	struct adapter *adapter;
 	u16    viid;
@@ -406,6 +424,9 @@ struct port_info {
 #ifdef CONFIG_CHELSIO_T4_DCB
 	struct port_dcb_info dcb;     /* Data Center Bridging support */
 #endif
+#ifdef CONFIG_CHELSIO_T4_FCOE
+	struct cxgb_fcoe fcoe;
+#endif /* CONFIG_CHELSIO_T4_FCOE */
 };
 
 struct dentry;
@@ -599,8 +620,8 @@ struct sge {
 	u16 rdmaqs;                 /* # of available RDMA Rx queues */
 	u16 rdmaciqs;               /* # of available RDMA concentrator IQs */
 	u16 ofld_rxq[MAX_OFLD_QSETS];
-	u16 rdma_rxq[NCHAN];
-	u16 rdma_ciq[NCHAN];
+	u16 rdma_rxq[MAX_RDMA_QUEUES];
+	u16 rdma_ciq[MAX_RDMA_CIQS];
 	u16 timer_val[SGE_NTIMERS];
 	u8 counter_val[SGE_NCOUNTERS];
 	u32 fl_pg_order;            /* large page allocation size */
@@ -616,11 +637,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 */
 };
@@ -993,6 +1016,30 @@ static inline bool cxgb_poll_busy_polling(struct sge_rspq *q)
 }
 #endif /* CONFIG_NET_RX_BUSY_POLL */
 
+/* Return a version number to identify the type of adapter.  The scheme is:
+ * - bits 0..9: chip version
+ * - bits 10..15: chip revision
+ * - bits 16..23: register dump version
+ */
+static inline unsigned int mk_adap_vers(struct adapter *ap)
+{
+	return CHELSIO_CHIP_VERSION(ap->params.chip) |
+		(CHELSIO_CHIP_RELEASE(ap->params.chip) << 10) | (1 << 16);
+}
+
+/* Return a queue's interrupt hold-off time in us.  0 means no timer. */
+static inline unsigned int qtimer_val(const struct adapter *adap,
+				      const struct sge_rspq *q)
+{
+	unsigned int idx = q->intr_params >> 1;
+
+	return idx < SGE_NTIMERS ? adap->sge.timer_val[idx] : 0;
+}
+
+/* driver version & name used for ethtool_drvinfo */
+extern char cxgb4_driver_name[];
+extern const char cxgb4_driver_version[];
+
 void t4_os_portmod_changed(const struct adapter *adap, int port_id);
 void t4_os_link_changed(struct adapter *adap, int port_id, int link_stat);
 
@@ -1022,6 +1069,10 @@ int t4_sge_init(struct adapter *adap);
 void t4_sge_start(struct adapter *adap);
 void t4_sge_stop(struct adapter *adap);
 int cxgb_busy_poll(struct napi_struct *napi);
+int cxgb4_set_rspq_intr_params(struct sge_rspq *q, unsigned int us,
+			       unsigned int cnt);
+void cxgb4_set_ethtool_ops(struct net_device *netdev);
+int cxgb4_write_rss(const struct port_info *pi, const u16 *queues);
 extern int dbfifo_int_thresh;
 
 #define for_each_port(adapter, iter) \
@@ -1110,6 +1161,9 @@ static inline int t4_memory_write(struct adapter *adap, int mtype, u32 addr,
 	return t4_memory_rw(adap, 0, mtype, addr, len, buf, 0);
 }
 
+unsigned int t4_get_regs_len(struct adapter *adapter);
+void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size);
+
 int t4_seeprom_wp(struct adapter *adapter, bool enable);
 int get_vpd_params(struct adapter *adapter, struct vpd_params *p);
 int t4_read_flash(struct adapter *adapter, unsigned int addr,
@@ -1136,6 +1190,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..f0285bcbe598 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;
 }
 
@@ -1769,6 +1775,8 @@ do { \
 		int n = min(4, adap->sge.rdmaqs - 4 * rdma_idx);
 
 		S("QType:", "RDMA-CPL");
+		S("Interface:",
+		  rx[i].rspq.netdev ? rx[i].rspq.netdev->name : "N/A");
 		R("RspQ ID:", rspq.abs_id);
 		R("RspQ size:", rspq.size);
 		R("RspQE size:", rspq.iqe_len);
@@ -1788,6 +1796,8 @@ do { \
 		int n = min(4, adap->sge.rdmaciqs - 4 * ciq_idx);
 
 		S("QType:", "RDMA-CIQ");
+		S("Interface:",
+		  rx[i].rspq.netdev ? rx[i].rspq.netdev->name : "N/A");
 		R("RspQ ID:", rspq.abs_id);
 		R("RspQ size:", rspq.size);
 		R("RspQE size:", rspq.iqe_len);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
new file mode 100644
index 000000000000..10d82b51d7ef
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
@@ -0,0 +1,915 @@
+/*
+ *  Copyright (C) 2013-2015 Chelsio Communications.  All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under the terms and conditions of the GNU General Public License,
+ *  version 2, as published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope it will be useful, but WITHOUT
+ *  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ *  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ *  more details.
+ *
+ *  The full GNU General Public License is included in this distribution in
+ *  the file called "COPYING".
+ *
+ */
+
+#include <linux/firmware.h>
+#include <linux/mdio.h>
+
+#include "cxgb4.h"
+#include "t4_regs.h"
+#include "t4fw_api.h"
+
+#define EEPROM_MAGIC 0x38E2F10C
+
+static u32 get_msglevel(struct net_device *dev)
+{
+	return netdev2adap(dev)->msg_enable;
+}
+
+static void set_msglevel(struct net_device *dev, u32 val)
+{
+	netdev2adap(dev)->msg_enable = val;
+}
+
+static const char stats_strings[][ETH_GSTRING_LEN] = {
+	"TxOctetsOK         ",
+	"TxFramesOK         ",
+	"TxBroadcastFrames  ",
+	"TxMulticastFrames  ",
+	"TxUnicastFrames    ",
+	"TxErrorFrames      ",
+
+	"TxFrames64         ",
+	"TxFrames65To127    ",
+	"TxFrames128To255   ",
+	"TxFrames256To511   ",
+	"TxFrames512To1023  ",
+	"TxFrames1024To1518 ",
+	"TxFrames1519ToMax  ",
+
+	"TxFramesDropped    ",
+	"TxPauseFrames      ",
+	"TxPPP0Frames       ",
+	"TxPPP1Frames       ",
+	"TxPPP2Frames       ",
+	"TxPPP3Frames       ",
+	"TxPPP4Frames       ",
+	"TxPPP5Frames       ",
+	"TxPPP6Frames       ",
+	"TxPPP7Frames       ",
+
+	"RxOctetsOK         ",
+	"RxFramesOK         ",
+	"RxBroadcastFrames  ",
+	"RxMulticastFrames  ",
+	"RxUnicastFrames    ",
+
+	"RxFramesTooLong    ",
+	"RxJabberErrors     ",
+	"RxFCSErrors        ",
+	"RxLengthErrors     ",
+	"RxSymbolErrors     ",
+	"RxRuntFrames       ",
+
+	"RxFrames64         ",
+	"RxFrames65To127    ",
+	"RxFrames128To255   ",
+	"RxFrames256To511   ",
+	"RxFrames512To1023  ",
+	"RxFrames1024To1518 ",
+	"RxFrames1519ToMax  ",
+
+	"RxPauseFrames      ",
+	"RxPPP0Frames       ",
+	"RxPPP1Frames       ",
+	"RxPPP2Frames       ",
+	"RxPPP3Frames       ",
+	"RxPPP4Frames       ",
+	"RxPPP5Frames       ",
+	"RxPPP6Frames       ",
+	"RxPPP7Frames       ",
+
+	"RxBG0FramesDropped ",
+	"RxBG1FramesDropped ",
+	"RxBG2FramesDropped ",
+	"RxBG3FramesDropped ",
+	"RxBG0FramesTrunc   ",
+	"RxBG1FramesTrunc   ",
+	"RxBG2FramesTrunc   ",
+	"RxBG3FramesTrunc   ",
+
+	"TSO                ",
+	"TxCsumOffload      ",
+	"RxCsumGood         ",
+	"VLANextractions    ",
+	"VLANinsertions     ",
+	"GROpackets         ",
+	"GROmerged          ",
+	"WriteCoalSuccess   ",
+	"WriteCoalFail      ",
+};
+
+static int get_sset_count(struct net_device *dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS:
+		return ARRAY_SIZE(stats_strings);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int get_regs_len(struct net_device *dev)
+{
+	struct adapter *adap = netdev2adap(dev);
+
+	return t4_get_regs_len(adap);
+}
+
+static int get_eeprom_len(struct net_device *dev)
+{
+	return EEPROMSIZE;
+}
+
+static void get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *info)
+{
+	struct adapter *adapter = netdev2adap(dev);
+	u32 exprom_vers;
+
+	strlcpy(info->driver, cxgb4_driver_name, sizeof(info->driver));
+	strlcpy(info->version, cxgb4_driver_version,
+		sizeof(info->version));
+	strlcpy(info->bus_info, pci_name(adapter->pdev),
+		sizeof(info->bus_info));
+
+	if (adapter->params.fw_vers)
+		snprintf(info->fw_version, sizeof(info->fw_version),
+			 "%u.%u.%u.%u, TP %u.%u.%u.%u",
+			 FW_HDR_FW_VER_MAJOR_G(adapter->params.fw_vers),
+			 FW_HDR_FW_VER_MINOR_G(adapter->params.fw_vers),
+			 FW_HDR_FW_VER_MICRO_G(adapter->params.fw_vers),
+			 FW_HDR_FW_VER_BUILD_G(adapter->params.fw_vers),
+			 FW_HDR_FW_VER_MAJOR_G(adapter->params.tp_vers),
+			 FW_HDR_FW_VER_MINOR_G(adapter->params.tp_vers),
+			 FW_HDR_FW_VER_MICRO_G(adapter->params.tp_vers),
+			 FW_HDR_FW_VER_BUILD_G(adapter->params.tp_vers));
+
+	if (!t4_get_exprom_version(adapter, &exprom_vers))
+		snprintf(info->erom_version, sizeof(info->erom_version),
+			 "%u.%u.%u.%u",
+			 FW_HDR_FW_VER_MAJOR_G(exprom_vers),
+			 FW_HDR_FW_VER_MINOR_G(exprom_vers),
+			 FW_HDR_FW_VER_MICRO_G(exprom_vers),
+			 FW_HDR_FW_VER_BUILD_G(exprom_vers));
+}
+
+static void get_strings(struct net_device *dev, u32 stringset, u8 *data)
+{
+	if (stringset == ETH_SS_STATS)
+		memcpy(data, stats_strings, sizeof(stats_strings));
+}
+
+/* port stats maintained per queue of the port. They should be in the same
+ * order as in stats_strings above.
+ */
+struct queue_port_stats {
+	u64 tso;
+	u64 tx_csum;
+	u64 rx_csum;
+	u64 vlan_ex;
+	u64 vlan_ins;
+	u64 gro_pkts;
+	u64 gro_merged;
+};
+
+static void collect_sge_port_stats(const struct adapter *adap,
+				   const struct port_info *p,
+				   struct queue_port_stats *s)
+{
+	int i;
+	const struct sge_eth_txq *tx = &adap->sge.ethtxq[p->first_qset];
+	const struct sge_eth_rxq *rx = &adap->sge.ethrxq[p->first_qset];
+
+	memset(s, 0, sizeof(*s));
+	for (i = 0; i < p->nqsets; i++, rx++, tx++) {
+		s->tso += tx->tso;
+		s->tx_csum += tx->tx_cso;
+		s->rx_csum += rx->stats.rx_cso;
+		s->vlan_ex += rx->stats.vlan_ex;
+		s->vlan_ins += tx->vlan_ins;
+		s->gro_pkts += rx->stats.lro_pkts;
+		s->gro_merged += rx->stats.lro_merged;
+	}
+}
+
+static void get_stats(struct net_device *dev, struct ethtool_stats *stats,
+		      u64 *data)
+{
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adapter = pi->adapter;
+	u32 val1, val2;
+
+	t4_get_port_stats(adapter, pi->tx_chan, (struct port_stats *)data);
+
+	data += sizeof(struct port_stats) / sizeof(u64);
+	collect_sge_port_stats(adapter, pi, (struct queue_port_stats *)data);
+	data += sizeof(struct queue_port_stats) / sizeof(u64);
+	if (!is_t4(adapter->params.chip)) {
+		t4_write_reg(adapter, SGE_STAT_CFG_A, STATSOURCE_T5_V(7));
+		val1 = t4_read_reg(adapter, SGE_STAT_TOTAL_A);
+		val2 = t4_read_reg(adapter, SGE_STAT_MATCH_A);
+		*data = val1 - val2;
+		data++;
+		*data = val2;
+		data++;
+	} else {
+		memset(data, 0, 2 * sizeof(u64));
+		*data += 2;
+	}
+}
+
+static void get_regs(struct net_device *dev, struct ethtool_regs *regs,
+		     void *buf)
+{
+	struct adapter *adap = netdev2adap(dev);
+	size_t buf_size;
+
+	buf_size = t4_get_regs_len(adap);
+	regs->version = mk_adap_vers(adap);
+	t4_get_regs(adap, buf, buf_size);
+}
+
+static int restart_autoneg(struct net_device *dev)
+{
+	struct port_info *p = netdev_priv(dev);
+
+	if (!netif_running(dev))
+		return -EAGAIN;
+	if (p->link_cfg.autoneg != AUTONEG_ENABLE)
+		return -EINVAL;
+	t4_restart_aneg(p->adapter, p->adapter->fn, p->tx_chan);
+	return 0;
+}
+
+static int identify_port(struct net_device *dev,
+			 enum ethtool_phys_id_state state)
+{
+	unsigned int val;
+	struct adapter *adap = netdev2adap(dev);
+
+	if (state == ETHTOOL_ID_ACTIVE)
+		val = 0xffff;
+	else if (state == ETHTOOL_ID_INACTIVE)
+		val = 0;
+	else
+		return -EINVAL;
+
+	return t4_identify_port(adap, adap->fn, netdev2pinfo(dev)->viid, val);
+}
+
+static unsigned int from_fw_linkcaps(enum fw_port_type type, unsigned int caps)
+{
+	unsigned int v = 0;
+
+	if (type == FW_PORT_TYPE_BT_SGMII || type == FW_PORT_TYPE_BT_XFI ||
+	    type == FW_PORT_TYPE_BT_XAUI) {
+		v |= SUPPORTED_TP;
+		if (caps & FW_PORT_CAP_SPEED_100M)
+			v |= SUPPORTED_100baseT_Full;
+		if (caps & FW_PORT_CAP_SPEED_1G)
+			v |= SUPPORTED_1000baseT_Full;
+		if (caps & FW_PORT_CAP_SPEED_10G)
+			v |= SUPPORTED_10000baseT_Full;
+	} else if (type == FW_PORT_TYPE_KX4 || type == FW_PORT_TYPE_KX) {
+		v |= SUPPORTED_Backplane;
+		if (caps & FW_PORT_CAP_SPEED_1G)
+			v |= SUPPORTED_1000baseKX_Full;
+		if (caps & FW_PORT_CAP_SPEED_10G)
+			v |= SUPPORTED_10000baseKX4_Full;
+	} else if (type == FW_PORT_TYPE_KR) {
+		v |= SUPPORTED_Backplane | SUPPORTED_10000baseKR_Full;
+	} else if (type == FW_PORT_TYPE_BP_AP) {
+		v |= SUPPORTED_Backplane | SUPPORTED_10000baseR_FEC |
+		     SUPPORTED_10000baseKR_Full | SUPPORTED_1000baseKX_Full;
+	} else if (type == FW_PORT_TYPE_BP4_AP) {
+		v |= SUPPORTED_Backplane | SUPPORTED_10000baseR_FEC |
+		     SUPPORTED_10000baseKR_Full | SUPPORTED_1000baseKX_Full |
+		     SUPPORTED_10000baseKX4_Full;
+	} else if (type == FW_PORT_TYPE_FIBER_XFI ||
+		   type == FW_PORT_TYPE_FIBER_XAUI ||
+		   type == FW_PORT_TYPE_SFP ||
+		   type == FW_PORT_TYPE_QSFP_10G ||
+		   type == FW_PORT_TYPE_QSA) {
+		v |= SUPPORTED_FIBRE;
+		if (caps & FW_PORT_CAP_SPEED_1G)
+			v |= SUPPORTED_1000baseT_Full;
+		if (caps & FW_PORT_CAP_SPEED_10G)
+			v |= SUPPORTED_10000baseT_Full;
+	} else if (type == FW_PORT_TYPE_BP40_BA ||
+		   type == FW_PORT_TYPE_QSFP) {
+		v |= SUPPORTED_40000baseSR4_Full;
+		v |= SUPPORTED_FIBRE;
+	}
+
+	if (caps & FW_PORT_CAP_ANEG)
+		v |= SUPPORTED_Autoneg;
+	return v;
+}
+
+static unsigned int to_fw_linkcaps(unsigned int caps)
+{
+	unsigned int v = 0;
+
+	if (caps & ADVERTISED_100baseT_Full)
+		v |= FW_PORT_CAP_SPEED_100M;
+	if (caps & ADVERTISED_1000baseT_Full)
+		v |= FW_PORT_CAP_SPEED_1G;
+	if (caps & ADVERTISED_10000baseT_Full)
+		v |= FW_PORT_CAP_SPEED_10G;
+	if (caps & ADVERTISED_40000baseSR4_Full)
+		v |= FW_PORT_CAP_SPEED_40G;
+	return v;
+}
+
+static int get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	const struct port_info *p = netdev_priv(dev);
+
+	if (p->port_type == FW_PORT_TYPE_BT_SGMII ||
+	    p->port_type == FW_PORT_TYPE_BT_XFI ||
+	    p->port_type == FW_PORT_TYPE_BT_XAUI) {
+		cmd->port = PORT_TP;
+	} else if (p->port_type == FW_PORT_TYPE_FIBER_XFI ||
+		   p->port_type == FW_PORT_TYPE_FIBER_XAUI) {
+		cmd->port = PORT_FIBRE;
+	} else if (p->port_type == FW_PORT_TYPE_SFP ||
+		   p->port_type == FW_PORT_TYPE_QSFP_10G ||
+		   p->port_type == FW_PORT_TYPE_QSA ||
+		   p->port_type == FW_PORT_TYPE_QSFP) {
+		if (p->mod_type == FW_PORT_MOD_TYPE_LR ||
+		    p->mod_type == FW_PORT_MOD_TYPE_SR ||
+		    p->mod_type == FW_PORT_MOD_TYPE_ER ||
+		    p->mod_type == FW_PORT_MOD_TYPE_LRM)
+			cmd->port = PORT_FIBRE;
+		else if (p->mod_type == FW_PORT_MOD_TYPE_TWINAX_PASSIVE ||
+			 p->mod_type == FW_PORT_MOD_TYPE_TWINAX_ACTIVE)
+			cmd->port = PORT_DA;
+		else
+			cmd->port = PORT_OTHER;
+	} else {
+		cmd->port = PORT_OTHER;
+	}
+
+	if (p->mdio_addr >= 0) {
+		cmd->phy_address = p->mdio_addr;
+		cmd->transceiver = XCVR_EXTERNAL;
+		cmd->mdio_support = p->port_type == FW_PORT_TYPE_BT_SGMII ?
+			MDIO_SUPPORTS_C22 : MDIO_SUPPORTS_C45;
+	} else {
+		cmd->phy_address = 0;  /* not really, but no better option */
+		cmd->transceiver = XCVR_INTERNAL;
+		cmd->mdio_support = 0;
+	}
+
+	cmd->supported = from_fw_linkcaps(p->port_type, p->link_cfg.supported);
+	cmd->advertising = from_fw_linkcaps(p->port_type,
+					    p->link_cfg.advertising);
+	ethtool_cmd_speed_set(cmd,
+			      netif_carrier_ok(dev) ? p->link_cfg.speed : 0);
+	cmd->duplex = DUPLEX_FULL;
+	cmd->autoneg = p->link_cfg.autoneg;
+	cmd->maxtxpkt = 0;
+	cmd->maxrxpkt = 0;
+	return 0;
+}
+
+static unsigned int speed_to_caps(int speed)
+{
+	if (speed == 100)
+		return FW_PORT_CAP_SPEED_100M;
+	if (speed == 1000)
+		return FW_PORT_CAP_SPEED_1G;
+	if (speed == 10000)
+		return FW_PORT_CAP_SPEED_10G;
+	if (speed == 40000)
+		return FW_PORT_CAP_SPEED_40G;
+	return 0;
+}
+
+static int set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	unsigned int cap;
+	struct port_info *p = netdev_priv(dev);
+	struct link_config *lc = &p->link_cfg;
+	u32 speed = ethtool_cmd_speed(cmd);
+
+	if (cmd->duplex != DUPLEX_FULL)     /* only full-duplex supported */
+		return -EINVAL;
+
+	if (!(lc->supported & FW_PORT_CAP_ANEG)) {
+		/* PHY offers a single speed.  See if that's what's
+		 * being requested.
+		 */
+		if (cmd->autoneg == AUTONEG_DISABLE &&
+		    (lc->supported & speed_to_caps(speed)))
+			return 0;
+		return -EINVAL;
+	}
+
+	if (cmd->autoneg == AUTONEG_DISABLE) {
+		cap = speed_to_caps(speed);
+
+		if (!(lc->supported & cap) ||
+		    (speed == 1000) ||
+		    (speed == 10000) ||
+		    (speed == 40000))
+			return -EINVAL;
+		lc->requested_speed = cap;
+		lc->advertising = 0;
+	} else {
+		cap = to_fw_linkcaps(cmd->advertising);
+		if (!(lc->supported & cap))
+			return -EINVAL;
+		lc->requested_speed = 0;
+		lc->advertising = cap | FW_PORT_CAP_ANEG;
+	}
+	lc->autoneg = cmd->autoneg;
+
+	if (netif_running(dev))
+		return t4_link_start(p->adapter, p->adapter->fn, p->tx_chan,
+				     lc);
+	return 0;
+}
+
+static void get_pauseparam(struct net_device *dev,
+			   struct ethtool_pauseparam *epause)
+{
+	struct port_info *p = netdev_priv(dev);
+
+	epause->autoneg = (p->link_cfg.requested_fc & PAUSE_AUTONEG) != 0;
+	epause->rx_pause = (p->link_cfg.fc & PAUSE_RX) != 0;
+	epause->tx_pause = (p->link_cfg.fc & PAUSE_TX) != 0;
+}
+
+static int set_pauseparam(struct net_device *dev,
+			  struct ethtool_pauseparam *epause)
+{
+	struct port_info *p = netdev_priv(dev);
+	struct link_config *lc = &p->link_cfg;
+
+	if (epause->autoneg == AUTONEG_DISABLE)
+		lc->requested_fc = 0;
+	else if (lc->supported & FW_PORT_CAP_ANEG)
+		lc->requested_fc = PAUSE_AUTONEG;
+	else
+		return -EINVAL;
+
+	if (epause->rx_pause)
+		lc->requested_fc |= PAUSE_RX;
+	if (epause->tx_pause)
+		lc->requested_fc |= PAUSE_TX;
+	if (netif_running(dev))
+		return t4_link_start(p->adapter, p->adapter->fn, p->tx_chan,
+				     lc);
+	return 0;
+}
+
+static void get_sge_param(struct net_device *dev, struct ethtool_ringparam *e)
+{
+	const struct port_info *pi = netdev_priv(dev);
+	const struct sge *s = &pi->adapter->sge;
+
+	e->rx_max_pending = MAX_RX_BUFFERS;
+	e->rx_mini_max_pending = MAX_RSPQ_ENTRIES;
+	e->rx_jumbo_max_pending = 0;
+	e->tx_max_pending = MAX_TXQ_ENTRIES;
+
+	e->rx_pending = s->ethrxq[pi->first_qset].fl.size - 8;
+	e->rx_mini_pending = s->ethrxq[pi->first_qset].rspq.size;
+	e->rx_jumbo_pending = 0;
+	e->tx_pending = s->ethtxq[pi->first_qset].q.size;
+}
+
+static int set_sge_param(struct net_device *dev, struct ethtool_ringparam *e)
+{
+	int i;
+	const struct port_info *pi = netdev_priv(dev);
+	struct adapter *adapter = pi->adapter;
+	struct sge *s = &adapter->sge;
+
+	if (e->rx_pending > MAX_RX_BUFFERS || e->rx_jumbo_pending ||
+	    e->tx_pending > MAX_TXQ_ENTRIES ||
+	    e->rx_mini_pending > MAX_RSPQ_ENTRIES ||
+	    e->rx_mini_pending < MIN_RSPQ_ENTRIES ||
+	    e->rx_pending < MIN_FL_ENTRIES || e->tx_pending < MIN_TXQ_ENTRIES)
+		return -EINVAL;
+
+	if (adapter->flags & FULL_INIT_DONE)
+		return -EBUSY;
+
+	for (i = 0; i < pi->nqsets; ++i) {
+		s->ethtxq[pi->first_qset + i].q.size = e->tx_pending;
+		s->ethrxq[pi->first_qset + i].fl.size = e->rx_pending + 8;
+		s->ethrxq[pi->first_qset + i].rspq.size = e->rx_mini_pending;
+	}
+	return 0;
+}
+
+/**
+ * set_rx_intr_params - set a net devices's RX interrupt holdoff paramete!
+ * @dev: the network device
+ * @us: the hold-off time in us, or 0 to disable timer
+ * @cnt: the hold-off packet count, or 0 to disable counter
+ *
+ * Set the RX interrupt hold-off parameters for a network device.
+ */
+static int set_rx_intr_params(struct net_device *dev,
+			      unsigned int us, unsigned int cnt)
+{
+	int i, err;
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
+
+	for (i = 0; i < pi->nqsets; i++, q++) {
+		err = cxgb4_set_rspq_intr_params(&q->rspq, us, cnt);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static int set_adaptive_rx_setting(struct net_device *dev, int adaptive_rx)
+{
+	int i;
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
+
+	for (i = 0; i < pi->nqsets; i++, q++)
+		q->rspq.adaptive_rx = adaptive_rx;
+
+	return 0;
+}
+
+static int get_adaptive_rx_setting(struct net_device *dev)
+{
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
+
+	return q->rspq.adaptive_rx;
+}
+
+static int set_coalesce(struct net_device *dev, struct ethtool_coalesce *c)
+{
+	set_adaptive_rx_setting(dev, c->use_adaptive_rx_coalesce);
+	return set_rx_intr_params(dev, c->rx_coalesce_usecs,
+				  c->rx_max_coalesced_frames);
+}
+
+static int get_coalesce(struct net_device *dev, struct ethtool_coalesce *c)
+{
+	const struct port_info *pi = netdev_priv(dev);
+	const struct adapter *adap = pi->adapter;
+	const struct sge_rspq *rq = &adap->sge.ethrxq[pi->first_qset].rspq;
+
+	c->rx_coalesce_usecs = qtimer_val(adap, rq);
+	c->rx_max_coalesced_frames = (rq->intr_params & QINTR_CNT_EN) ?
+		adap->sge.counter_val[rq->pktcnt_idx] : 0;
+	c->use_adaptive_rx_coalesce = get_adaptive_rx_setting(dev);
+	return 0;
+}
+
+/**
+ *	eeprom_ptov - translate a physical EEPROM address to virtual
+ *	@phys_addr: the physical EEPROM address
+ *	@fn: the PCI function number
+ *	@sz: size of function-specific area
+ *
+ *	Translate a physical EEPROM address to virtual.  The first 1K is
+ *	accessed through virtual addresses starting at 31K, the rest is
+ *	accessed through virtual addresses starting at 0.
+ *
+ *	The mapping is as follows:
+ *	[0..1K) -> [31K..32K)
+ *	[1K..1K+A) -> [31K-A..31K)
+ *	[1K+A..ES) -> [0..ES-A-1K)
+ *
+ *	where A = @fn * @sz, and ES = EEPROM size.
+ */
+static int eeprom_ptov(unsigned int phys_addr, unsigned int fn, unsigned int sz)
+{
+	fn *= sz;
+	if (phys_addr < 1024)
+		return phys_addr + (31 << 10);
+	if (phys_addr < 1024 + fn)
+		return 31744 - fn + phys_addr - 1024;
+	if (phys_addr < EEPROMSIZE)
+		return phys_addr - 1024 - fn;
+	return -EINVAL;
+}
+
+/* The next two routines implement eeprom read/write from physical addresses.
+ */
+static int eeprom_rd_phys(struct adapter *adap, unsigned int phys_addr, u32 *v)
+{
+	int vaddr = eeprom_ptov(phys_addr, adap->fn, EEPROMPFSIZE);
+
+	if (vaddr >= 0)
+		vaddr = pci_read_vpd(adap->pdev, vaddr, sizeof(u32), v);
+	return vaddr < 0 ? vaddr : 0;
+}
+
+static int eeprom_wr_phys(struct adapter *adap, unsigned int phys_addr, u32 v)
+{
+	int vaddr = eeprom_ptov(phys_addr, adap->fn, EEPROMPFSIZE);
+
+	if (vaddr >= 0)
+		vaddr = pci_write_vpd(adap->pdev, vaddr, sizeof(u32), &v);
+	return vaddr < 0 ? vaddr : 0;
+}
+
+#define EEPROM_MAGIC 0x38E2F10C
+
+static int get_eeprom(struct net_device *dev, struct ethtool_eeprom *e,
+		      u8 *data)
+{
+	int i, err = 0;
+	struct adapter *adapter = netdev2adap(dev);
+	u8 *buf = kmalloc(EEPROMSIZE, GFP_KERNEL);
+
+	if (!buf)
+		return -ENOMEM;
+
+	e->magic = EEPROM_MAGIC;
+	for (i = e->offset & ~3; !err && i < e->offset + e->len; i += 4)
+		err = eeprom_rd_phys(adapter, i, (u32 *)&buf[i]);
+
+	if (!err)
+		memcpy(data, buf + e->offset, e->len);
+	kfree(buf);
+	return err;
+}
+
+static int set_eeprom(struct net_device *dev, struct ethtool_eeprom *eeprom,
+		      u8 *data)
+{
+	u8 *buf;
+	int err = 0;
+	u32 aligned_offset, aligned_len, *p;
+	struct adapter *adapter = netdev2adap(dev);
+
+	if (eeprom->magic != EEPROM_MAGIC)
+		return -EINVAL;
+
+	aligned_offset = eeprom->offset & ~3;
+	aligned_len = (eeprom->len + (eeprom->offset & 3) + 3) & ~3;
+
+	if (adapter->fn > 0) {
+		u32 start = 1024 + adapter->fn * EEPROMPFSIZE;
+
+		if (aligned_offset < start ||
+		    aligned_offset + aligned_len > start + EEPROMPFSIZE)
+			return -EPERM;
+	}
+
+	if (aligned_offset != eeprom->offset || aligned_len != eeprom->len) {
+		/* RMW possibly needed for first or last words.
+		 */
+		buf = kmalloc(aligned_len, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+		err = eeprom_rd_phys(adapter, aligned_offset, (u32 *)buf);
+		if (!err && aligned_len > 4)
+			err = eeprom_rd_phys(adapter,
+					     aligned_offset + aligned_len - 4,
+					     (u32 *)&buf[aligned_len - 4]);
+		if (err)
+			goto out;
+		memcpy(buf + (eeprom->offset & 3), data, eeprom->len);
+	} else {
+		buf = data;
+	}
+
+	err = t4_seeprom_wp(adapter, false);
+	if (err)
+		goto out;
+
+	for (p = (u32 *)buf; !err && aligned_len; aligned_len -= 4, p++) {
+		err = eeprom_wr_phys(adapter, aligned_offset, *p);
+		aligned_offset += 4;
+	}
+
+	if (!err)
+		err = t4_seeprom_wp(adapter, true);
+out:
+	if (buf != data)
+		kfree(buf);
+	return err;
+}
+
+static int set_flash(struct net_device *netdev, struct ethtool_flash *ef)
+{
+	int ret;
+	const struct firmware *fw;
+	struct adapter *adap = netdev2adap(netdev);
+	unsigned int mbox = PCIE_FW_MASTER_M + 1;
+
+	ef->data[sizeof(ef->data) - 1] = '\0';
+	ret = request_firmware(&fw, ef->data, adap->pdev_dev);
+	if (ret < 0)
+		return ret;
+
+	/* If the adapter has been fully initialized then we'll go ahead and
+	 * try to get the firmware's cooperation in upgrading to the new
+	 * firmware image otherwise we'll try to do the entire job from the
+	 * host ... and we always "force" the operation in this path.
+	 */
+	if (adap->flags & FULL_INIT_DONE)
+		mbox = adap->mbox;
+
+	ret = t4_fw_upgrade(adap, mbox, fw->data, fw->size, 1);
+	release_firmware(fw);
+	if (!ret)
+		dev_info(adap->pdev_dev,
+			 "loaded firmware %s, reload cxgb4 driver\n", ef->data);
+	return ret;
+}
+
+#define WOL_SUPPORTED (WAKE_BCAST | WAKE_MAGIC)
+#define BCAST_CRC 0xa0ccc1a6
+
+static void get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
+{
+	wol->supported = WAKE_BCAST | WAKE_MAGIC;
+	wol->wolopts = netdev2adap(dev)->wol;
+	memset(&wol->sopass, 0, sizeof(wol->sopass));
+}
+
+static int set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
+{
+	int err = 0;
+	struct port_info *pi = netdev_priv(dev);
+
+	if (wol->wolopts & ~WOL_SUPPORTED)
+		return -EINVAL;
+	t4_wol_magic_enable(pi->adapter, pi->tx_chan,
+			    (wol->wolopts & WAKE_MAGIC) ? dev->dev_addr : NULL);
+	if (wol->wolopts & WAKE_BCAST) {
+		err = t4_wol_pat_enable(pi->adapter, pi->tx_chan, 0xfe, ~0ULL,
+					~0ULL, 0, false);
+		if (!err)
+			err = t4_wol_pat_enable(pi->adapter, pi->tx_chan, 1,
+						~6ULL, ~0ULL, BCAST_CRC, true);
+	} else {
+		t4_wol_pat_enable(pi->adapter, pi->tx_chan, 0, 0, 0, 0, false);
+	}
+	return err;
+}
+
+static u32 get_rss_table_size(struct net_device *dev)
+{
+	const struct port_info *pi = netdev_priv(dev);
+
+	return pi->rss_size;
+}
+
+static int get_rss_table(struct net_device *dev, u32 *p, u8 *key, u8 *hfunc)
+{
+	const struct port_info *pi = netdev_priv(dev);
+	unsigned int n = pi->rss_size;
+
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+	if (!p)
+		return 0;
+	while (n--)
+		p[n] = pi->rss[n];
+	return 0;
+}
+
+static int set_rss_table(struct net_device *dev, const u32 *p, const u8 *key,
+			 const u8 hfunc)
+{
+	unsigned int i;
+	struct port_info *pi = netdev_priv(dev);
+
+	/* We require at least one supported parameter to be changed and no
+	 * change in any of the unsupported parameters
+	 */
+	if (key ||
+	    (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP))
+		return -EOPNOTSUPP;
+	if (!p)
+		return 0;
+
+	for (i = 0; i < pi->rss_size; i++)
+		pi->rss[i] = p[i];
+	if (pi->adapter->flags & FULL_INIT_DONE)
+		return cxgb4_write_rss(pi, pi->rss);
+	return 0;
+}
+
+static int get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
+		     u32 *rules)
+{
+	const struct port_info *pi = netdev_priv(dev);
+
+	switch (info->cmd) {
+	case ETHTOOL_GRXFH: {
+		unsigned int v = pi->rss_mode;
+
+		info->data = 0;
+		switch (info->flow_type) {
+		case TCP_V4_FLOW:
+			if (v & FW_RSS_VI_CONFIG_CMD_IP4FOURTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST |
+					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
+			else if (v & FW_RSS_VI_CONFIG_CMD_IP4TWOTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST;
+			break;
+		case UDP_V4_FLOW:
+			if ((v & FW_RSS_VI_CONFIG_CMD_IP4FOURTUPEN_F) &&
+			    (v & FW_RSS_VI_CONFIG_CMD_UDPEN_F))
+				info->data = RXH_IP_SRC | RXH_IP_DST |
+					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
+			else if (v & FW_RSS_VI_CONFIG_CMD_IP4TWOTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST;
+			break;
+		case SCTP_V4_FLOW:
+		case AH_ESP_V4_FLOW:
+		case IPV4_FLOW:
+			if (v & FW_RSS_VI_CONFIG_CMD_IP4TWOTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST;
+			break;
+		case TCP_V6_FLOW:
+			if (v & FW_RSS_VI_CONFIG_CMD_IP6FOURTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST |
+					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
+			else if (v & FW_RSS_VI_CONFIG_CMD_IP6TWOTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST;
+			break;
+		case UDP_V6_FLOW:
+			if ((v & FW_RSS_VI_CONFIG_CMD_IP6FOURTUPEN_F) &&
+			    (v & FW_RSS_VI_CONFIG_CMD_UDPEN_F))
+				info->data = RXH_IP_SRC | RXH_IP_DST |
+					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
+			else if (v & FW_RSS_VI_CONFIG_CMD_IP6TWOTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST;
+			break;
+		case SCTP_V6_FLOW:
+		case AH_ESP_V6_FLOW:
+		case IPV6_FLOW:
+			if (v & FW_RSS_VI_CONFIG_CMD_IP6TWOTUPEN_F)
+				info->data = RXH_IP_SRC | RXH_IP_DST;
+			break;
+		}
+		return 0;
+	}
+	case ETHTOOL_GRXRINGS:
+		info->data = pi->nqsets;
+		return 0;
+	}
+	return -EOPNOTSUPP;
+}
+
+static const struct ethtool_ops cxgb_ethtool_ops = {
+	.get_settings      = get_settings,
+	.set_settings      = set_settings,
+	.get_drvinfo       = get_drvinfo,
+	.get_msglevel      = get_msglevel,
+	.set_msglevel      = set_msglevel,
+	.get_ringparam     = get_sge_param,
+	.set_ringparam     = set_sge_param,
+	.get_coalesce      = get_coalesce,
+	.set_coalesce      = set_coalesce,
+	.get_eeprom_len    = get_eeprom_len,
+	.get_eeprom        = get_eeprom,
+	.set_eeprom        = set_eeprom,
+	.get_pauseparam    = get_pauseparam,
+	.set_pauseparam    = set_pauseparam,
+	.get_link          = ethtool_op_get_link,
+	.get_strings       = get_strings,
+	.set_phys_id       = identify_port,
+	.nway_reset        = restart_autoneg,
+	.get_sset_count    = get_sset_count,
+	.get_ethtool_stats = get_stats,
+	.get_regs_len      = get_regs_len,
+	.get_regs          = get_regs,
+	.get_wol           = get_wol,
+	.set_wol           = set_wol,
+	.get_rxnfc         = get_rxnfc,
+	.get_rxfh_indir_size = get_rss_table_size,
+	.get_rxfh	   = get_rss_table,
+	.set_rxfh	   = set_rss_table,
+	.flash_device      = set_flash,
+};
+
+void cxgb4_set_ethtool_ops(struct net_device *netdev)
+{
+	netdev->ethtool_ops = &cxgb_ethtool_ops;
+}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_fcoe.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_fcoe.c
new file mode 100644
index 000000000000..6c8a62eefe51
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_fcoe.c
@@ -0,0 +1,122 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2015 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifdef CONFIG_CHELSIO_T4_FCOE
+
+#include <scsi/fc/fc_fs.h>
+#include <scsi/libfcoe.h>
+#include "cxgb4.h"
+
+bool cxgb_fcoe_sof_eof_supported(struct adapter *adap, struct sk_buff *skb)
+{
+	struct fcoe_hdr *fcoeh = (struct fcoe_hdr *)skb_network_header(skb);
+	u8 sof = fcoeh->fcoe_sof;
+	u8 eof = 0;
+
+	if ((sof != FC_SOF_I3) && (sof != FC_SOF_N3)) {
+		dev_err(adap->pdev_dev, "Unsupported SOF 0x%x\n", sof);
+		return false;
+	}
+
+	skb_copy_bits(skb, skb->len - 4, &eof, 1);
+
+	if ((eof != FC_EOF_N) && (eof != FC_EOF_T)) {
+		dev_err(adap->pdev_dev, "Unsupported EOF 0x%x\n", eof);
+		return false;
+	}
+
+	return true;
+}
+
+/**
+ * cxgb_fcoe_enable - enable FCoE offload features
+ * @netdev: net device
+ *
+ * Returns 0 on success or -EINVAL on failure.
+ */
+int cxgb_fcoe_enable(struct net_device *netdev)
+{
+	struct port_info *pi = netdev_priv(netdev);
+	struct adapter *adap = pi->adapter;
+	struct cxgb_fcoe *fcoe = &pi->fcoe;
+
+	if (is_t4(adap->params.chip))
+		return -EINVAL;
+
+	if (!(adap->flags & FULL_INIT_DONE))
+		return -EINVAL;
+
+	dev_info(adap->pdev_dev, "Enabling FCoE offload features\n");
+
+	netdev->features |= NETIF_F_FCOE_CRC;
+	netdev->vlan_features |= NETIF_F_FCOE_CRC;
+	netdev->features |= NETIF_F_FCOE_MTU;
+	netdev->vlan_features |= NETIF_F_FCOE_MTU;
+
+	netdev_features_change(netdev);
+
+	fcoe->flags |= CXGB_FCOE_ENABLED;
+
+	return 0;
+}
+
+/**
+ * cxgb_fcoe_disable - disable FCoE offload
+ * @netdev: net device
+ *
+ * Returns 0 on success or -EINVAL on failure.
+ */
+int cxgb_fcoe_disable(struct net_device *netdev)
+{
+	struct port_info *pi = netdev_priv(netdev);
+	struct adapter *adap = pi->adapter;
+	struct cxgb_fcoe *fcoe = &pi->fcoe;
+
+	if (!(fcoe->flags & CXGB_FCOE_ENABLED))
+		return -EINVAL;
+
+	dev_info(adap->pdev_dev, "Disabling FCoE offload features\n");
+
+	fcoe->flags &= ~CXGB_FCOE_ENABLED;
+
+	netdev->features &= ~NETIF_F_FCOE_CRC;
+	netdev->vlan_features &= ~NETIF_F_FCOE_CRC;
+	netdev->features &= ~NETIF_F_FCOE_MTU;
+	netdev->vlan_features &= ~NETIF_F_FCOE_MTU;
+
+	netdev_features_change(netdev);
+
+	return 0;
+}
+#endif /* CONFIG_CHELSIO_T4_FCOE */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_fcoe.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_fcoe.h
new file mode 100644
index 000000000000..bf9258a56ac9
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_fcoe.h
@@ -0,0 +1,57 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2015 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __CXGB4_FCOE_H__
+#define __CXGB4_FCOE_H__
+
+#ifdef CONFIG_CHELSIO_T4_FCOE
+
+#define CXGB_FCOE_TXPKT_CSUM_START	28
+#define CXGB_FCOE_TXPKT_CSUM_END	8
+
+/* fcoe flags */
+enum {
+	CXGB_FCOE_ENABLED     = (1 << 0),
+};
+
+struct cxgb_fcoe {
+	u8	flags;
+};
+
+int cxgb_fcoe_enable(struct net_device *);
+int cxgb_fcoe_disable(struct net_device *);
+bool cxgb_fcoe_sof_eof_supported(struct adapter *, struct sk_buff *);
+
+#endif /* CONFIG_CHELSIO_T4_FCOE */
+#endif /* __CXGB4_FCOE_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index a22cf932ca35..6de054404156 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -76,23 +76,15 @@
 #include "clip_tbl.h"
 #include "l2t.h"
 
+char cxgb4_driver_name[] = KBUILD_MODNAME;
+
 #ifdef DRV_VERSION
 #undef DRV_VERSION
 #endif
 #define DRV_VERSION "2.0.0-ko"
+const char cxgb4_driver_version[] = DRV_VERSION;
 #define DRV_DESC "Chelsio T4/T5 Network Driver"
 
-enum {
-	MAX_TXQ_ENTRIES      = 16384,
-	MAX_CTRL_TXQ_ENTRIES = 1024,
-	MAX_RSPQ_ENTRIES     = 16384,
-	MAX_RX_BUFFERS       = 16384,
-	MIN_TXQ_ENTRIES      = 32,
-	MIN_CTRL_TXQ_ENTRIES = 32,
-	MIN_RSPQ_ENTRIES     = 128,
-	MIN_FL_ENTRIES       = 16
-};
-
 /* Host shadow copy of ingress filter entry.  This is in host native format
  * and doesn't match the ordering or bit order, etc. of the hardware of the
  * firmware command.  The use of bit-field structure elements is purely to
@@ -124,7 +116,7 @@ struct filter_entry {
 /* Macros needed to support the PCI Device ID Table ...
  */
 #define CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN \
-	static struct pci_device_id cxgb4_pci_tbl[] = {
+	static const struct pci_device_id cxgb4_pci_tbl[] = {
 #define CH_PCI_DEVICE_ID_FUNCTION 0x4
 
 /* Include PCI Device IDs for both PF4 and PF0-3 so our PCI probe() routine is
@@ -732,7 +724,8 @@ static irqreturn_t t4_nondata_intr(int irq, void *cookie)
 		adap->swintr = 1;
 		t4_write_reg(adap, MYPF_REG(PL_PF_INT_CAUSE_A), v);
 	}
-	t4_slow_intr_handler(adap);
+	if (adap->flags & MASTER_PF)
+		t4_slow_intr_handler(adap);
 	return IRQ_HANDLED;
 }
 
@@ -857,14 +850,14 @@ static void free_msix_queue_irqs(struct adapter *adap)
 }
 
 /**
- *	write_rss - write the RSS table for a given port
+ *	cxgb4_write_rss - write the RSS table for a given port
  *	@pi: the port
  *	@queues: array of queue indices for RSS
  *
  *	Sets up the portion of the HW RSS table for the port's VI to distribute
  *	packets to the Rx queues in @queues.
  */
-static int write_rss(const struct port_info *pi, const u16 *queues)
+int cxgb4_write_rss(const struct port_info *pi, const u16 *queues)
 {
 	u16 *rss;
 	int i, err;
@@ -897,7 +890,7 @@ static int setup_rss(struct adapter *adap)
 	for_each_port(adap, i) {
 		const struct port_info *pi = adap2pinfo(adap, i);
 
-		err = write_rss(pi, pi->rss);
+		err = cxgb4_write_rss(pi, pi->rss);
 		if (err)
 			return err;
 	}
@@ -920,7 +913,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 +927,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 +949,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)
@@ -957,6 +965,28 @@ static void enable_rx(struct adapter *adap)
 	}
 }
 
+static int alloc_ofld_rxqs(struct adapter *adap, struct sge_ofld_rxq *q,
+			   unsigned int nq, unsigned int per_chan, int msi_idx,
+			   u16 *ids)
+{
+	int i, err;
+
+	for (i = 0; i < nq; i++, q++) {
+		if (msi_idx > 0)
+			msi_idx++;
+		err = t4_sge_alloc_rxq(adap, &q->rspq, false,
+				       adap->port[i / per_chan],
+				       msi_idx, q->fl.size ? &q->fl : NULL,
+				       uldrx_handler);
+		if (err)
+			return err;
+		memset(&q->stats, 0, sizeof(q->stats));
+		if (ids)
+			ids[i] = q->rspq.abs_id;
+	}
+	return 0;
+}
+
 /**
  *	setup_sge_queues - configure SGE Tx/Rx/response queues
  *	@adap: the adapter
@@ -970,8 +1000,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 +1013,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) {
@@ -1018,51 +1061,27 @@ freeout:	t4_free_sge_resources(adap);
 
 	j = s->ofldqsets / adap->params.nports; /* ofld queues per channel */
 	for_each_ofldrxq(s, i) {
-		struct sge_ofld_rxq *q = &s->ofldrxq[i];
-		struct net_device *dev = adap->port[i / j];
-
-		if (msi_idx > 0)
-			msi_idx++;
-		err = t4_sge_alloc_rxq(adap, &q->rspq, false, dev, msi_idx,
-				       q->fl.size ? &q->fl : NULL,
-				       uldrx_handler);
-		if (err)
-			goto freeout;
-		memset(&q->stats, 0, sizeof(q->stats));
-		s->ofld_rxq[i] = q->rspq.abs_id;
-		err = t4_sge_alloc_ofld_txq(adap, &s->ofldtxq[i], dev,
+		err = t4_sge_alloc_ofld_txq(adap, &s->ofldtxq[i],
+					    adap->port[i / j],
 					    s->fw_evtq.cntxt_id);
 		if (err)
 			goto freeout;
 	}
 
-	for_each_rdmarxq(s, i) {
-		struct sge_ofld_rxq *q = &s->rdmarxq[i];
-
-		if (msi_idx > 0)
-			msi_idx++;
-		err = t4_sge_alloc_rxq(adap, &q->rspq, false, adap->port[i],
-				       msi_idx, q->fl.size ? &q->fl : NULL,
-				       uldrx_handler);
-		if (err)
-			goto freeout;
-		memset(&q->stats, 0, sizeof(q->stats));
-		s->rdma_rxq[i] = q->rspq.abs_id;
-	}
+#define ALLOC_OFLD_RXQS(firstq, nq, per_chan, ids) do { \
+	err = alloc_ofld_rxqs(adap, firstq, nq, per_chan, msi_idx, ids); \
+	if (err) \
+		goto freeout; \
+	if (msi_idx > 0) \
+		msi_idx += nq; \
+} while (0)
 
-	for_each_rdmaciq(s, i) {
-		struct sge_ofld_rxq *q = &s->rdmaciq[i];
+	ALLOC_OFLD_RXQS(s->ofldrxq, s->ofldqsets, j, s->ofld_rxq);
+	ALLOC_OFLD_RXQS(s->rdmarxq, s->rdmaqs, 1, s->rdma_rxq);
+	j = s->rdmaciqs / adap->params.nports; /* rdmaq queues per channel */
+	ALLOC_OFLD_RXQS(s->rdmaciq, s->rdmaciqs, j, s->rdma_ciq);
 
-		if (msi_idx > 0)
-			msi_idx++;
-		err = t4_sge_alloc_rxq(adap, &q->rspq, false, adap->port[i],
-				       msi_idx, q->fl.size ? &q->fl : NULL,
-				       uldrx_handler);
-		if (err)
-			goto freeout;
-		memset(&q->stats, 0, sizeof(q->stats));
-		s->rdma_ciq[i] = q->rspq.abs_id;
-	}
+#undef ALLOC_OFLD_RXQS
 
 	for_each_port(adap, i) {
 		/*
@@ -1273,6 +1292,10 @@ static u16 cxgb_select_queue(struct net_device *dev, struct sk_buff *skb,
 			txq = 0;
 		} else {
 			txq = (vlan_tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
+#ifdef CONFIG_CHELSIO_T4_FCOE
+			if (skb->protocol == htons(ETH_P_FCOE))
+				txq = skb->priority & 0x7;
+#endif /* CONFIG_CHELSIO_T4_FCOE */
 		}
 		return txq;
 	}
@@ -1297,1192 +1320,6 @@ static inline int is_offload(const struct adapter *adap)
 	return adap->params.offload;
 }
 
-/*
- * Implementation of ethtool operations.
- */
-
-static u32 get_msglevel(struct net_device *dev)
-{
-	return netdev2adap(dev)->msg_enable;
-}
-
-static void set_msglevel(struct net_device *dev, u32 val)
-{
-	netdev2adap(dev)->msg_enable = val;
-}
-
-static char stats_strings[][ETH_GSTRING_LEN] = {
-	"TxOctetsOK         ",
-	"TxFramesOK         ",
-	"TxBroadcastFrames  ",
-	"TxMulticastFrames  ",
-	"TxUnicastFrames    ",
-	"TxErrorFrames      ",
-
-	"TxFrames64         ",
-	"TxFrames65To127    ",
-	"TxFrames128To255   ",
-	"TxFrames256To511   ",
-	"TxFrames512To1023  ",
-	"TxFrames1024To1518 ",
-	"TxFrames1519ToMax  ",
-
-	"TxFramesDropped    ",
-	"TxPauseFrames      ",
-	"TxPPP0Frames       ",
-	"TxPPP1Frames       ",
-	"TxPPP2Frames       ",
-	"TxPPP3Frames       ",
-	"TxPPP4Frames       ",
-	"TxPPP5Frames       ",
-	"TxPPP6Frames       ",
-	"TxPPP7Frames       ",
-
-	"RxOctetsOK         ",
-	"RxFramesOK         ",
-	"RxBroadcastFrames  ",
-	"RxMulticastFrames  ",
-	"RxUnicastFrames    ",
-
-	"RxFramesTooLong    ",
-	"RxJabberErrors     ",
-	"RxFCSErrors        ",
-	"RxLengthErrors     ",
-	"RxSymbolErrors     ",
-	"RxRuntFrames       ",
-
-	"RxFrames64         ",
-	"RxFrames65To127    ",
-	"RxFrames128To255   ",
-	"RxFrames256To511   ",
-	"RxFrames512To1023  ",
-	"RxFrames1024To1518 ",
-	"RxFrames1519ToMax  ",
-
-	"RxPauseFrames      ",
-	"RxPPP0Frames       ",
-	"RxPPP1Frames       ",
-	"RxPPP2Frames       ",
-	"RxPPP3Frames       ",
-	"RxPPP4Frames       ",
-	"RxPPP5Frames       ",
-	"RxPPP6Frames       ",
-	"RxPPP7Frames       ",
-
-	"RxBG0FramesDropped ",
-	"RxBG1FramesDropped ",
-	"RxBG2FramesDropped ",
-	"RxBG3FramesDropped ",
-	"RxBG0FramesTrunc   ",
-	"RxBG1FramesTrunc   ",
-	"RxBG2FramesTrunc   ",
-	"RxBG3FramesTrunc   ",
-
-	"TSO                ",
-	"TxCsumOffload      ",
-	"RxCsumGood         ",
-	"VLANextractions    ",
-	"VLANinsertions     ",
-	"GROpackets         ",
-	"GROmerged          ",
-	"WriteCoalSuccess   ",
-	"WriteCoalFail      ",
-};
-
-static int get_sset_count(struct net_device *dev, int sset)
-{
-	switch (sset) {
-	case ETH_SS_STATS:
-		return ARRAY_SIZE(stats_strings);
-	default:
-		return -EOPNOTSUPP;
-	}
-}
-
-#define T4_REGMAP_SIZE (160 * 1024)
-#define T5_REGMAP_SIZE (332 * 1024)
-
-static int get_regs_len(struct net_device *dev)
-{
-	struct adapter *adap = netdev2adap(dev);
-	if (is_t4(adap->params.chip))
-		return T4_REGMAP_SIZE;
-	else
-		return T5_REGMAP_SIZE;
-}
-
-static int get_eeprom_len(struct net_device *dev)
-{
-	return EEPROMSIZE;
-}
-
-static void get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *info)
-{
-	struct adapter *adapter = netdev2adap(dev);
-	u32 exprom_vers;
-
-	strlcpy(info->driver, KBUILD_MODNAME, sizeof(info->driver));
-	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
-	strlcpy(info->bus_info, pci_name(adapter->pdev),
-		sizeof(info->bus_info));
-
-	if (adapter->params.fw_vers)
-		snprintf(info->fw_version, sizeof(info->fw_version),
-			"%u.%u.%u.%u, TP %u.%u.%u.%u",
-			FW_HDR_FW_VER_MAJOR_G(adapter->params.fw_vers),
-			FW_HDR_FW_VER_MINOR_G(adapter->params.fw_vers),
-			FW_HDR_FW_VER_MICRO_G(adapter->params.fw_vers),
-			FW_HDR_FW_VER_BUILD_G(adapter->params.fw_vers),
-			FW_HDR_FW_VER_MAJOR_G(adapter->params.tp_vers),
-			FW_HDR_FW_VER_MINOR_G(adapter->params.tp_vers),
-			FW_HDR_FW_VER_MICRO_G(adapter->params.tp_vers),
-			FW_HDR_FW_VER_BUILD_G(adapter->params.tp_vers));
-
-	if (!t4_get_exprom_version(adapter, &exprom_vers))
-		snprintf(info->erom_version, sizeof(info->erom_version),
-			 "%u.%u.%u.%u",
-			 FW_HDR_FW_VER_MAJOR_G(exprom_vers),
-			 FW_HDR_FW_VER_MINOR_G(exprom_vers),
-			 FW_HDR_FW_VER_MICRO_G(exprom_vers),
-			 FW_HDR_FW_VER_BUILD_G(exprom_vers));
-}
-
-static void get_strings(struct net_device *dev, u32 stringset, u8 *data)
-{
-	if (stringset == ETH_SS_STATS)
-		memcpy(data, stats_strings, sizeof(stats_strings));
-}
-
-/*
- * port stats maintained per queue of the port.  They should be in the same
- * order as in stats_strings above.
- */
-struct queue_port_stats {
-	u64 tso;
-	u64 tx_csum;
-	u64 rx_csum;
-	u64 vlan_ex;
-	u64 vlan_ins;
-	u64 gro_pkts;
-	u64 gro_merged;
-};
-
-static void collect_sge_port_stats(const struct adapter *adap,
-		const struct port_info *p, struct queue_port_stats *s)
-{
-	int i;
-	const struct sge_eth_txq *tx = &adap->sge.ethtxq[p->first_qset];
-	const struct sge_eth_rxq *rx = &adap->sge.ethrxq[p->first_qset];
-
-	memset(s, 0, sizeof(*s));
-	for (i = 0; i < p->nqsets; i++, rx++, tx++) {
-		s->tso += tx->tso;
-		s->tx_csum += tx->tx_cso;
-		s->rx_csum += rx->stats.rx_cso;
-		s->vlan_ex += rx->stats.vlan_ex;
-		s->vlan_ins += tx->vlan_ins;
-		s->gro_pkts += rx->stats.lro_pkts;
-		s->gro_merged += rx->stats.lro_merged;
-	}
-}
-
-static void get_stats(struct net_device *dev, struct ethtool_stats *stats,
-		      u64 *data)
-{
-	struct port_info *pi = netdev_priv(dev);
-	struct adapter *adapter = pi->adapter;
-	u32 val1, val2;
-
-	t4_get_port_stats(adapter, pi->tx_chan, (struct port_stats *)data);
-
-	data += sizeof(struct port_stats) / sizeof(u64);
-	collect_sge_port_stats(adapter, pi, (struct queue_port_stats *)data);
-	data += sizeof(struct queue_port_stats) / sizeof(u64);
-	if (!is_t4(adapter->params.chip)) {
-		t4_write_reg(adapter, SGE_STAT_CFG_A, STATSOURCE_T5_V(7));
-		val1 = t4_read_reg(adapter, SGE_STAT_TOTAL_A);
-		val2 = t4_read_reg(adapter, SGE_STAT_MATCH_A);
-		*data = val1 - val2;
-		data++;
-		*data = val2;
-		data++;
-	} else {
-		memset(data, 0, 2 * sizeof(u64));
-		*data += 2;
-	}
-}
-
-/*
- * Return a version number to identify the type of adapter.  The scheme is:
- * - bits 0..9: chip version
- * - bits 10..15: chip revision
- * - bits 16..23: register dump version
- */
-static inline unsigned int mk_adap_vers(const struct adapter *ap)
-{
-	return CHELSIO_CHIP_VERSION(ap->params.chip) |
-		(CHELSIO_CHIP_RELEASE(ap->params.chip) << 10) | (1 << 16);
-}
-
-static void reg_block_dump(struct adapter *ap, void *buf, unsigned int start,
-			   unsigned int end)
-{
-	u32 *p = buf + start;
-
-	for ( ; start <= end; start += sizeof(u32))
-		*p++ = t4_read_reg(ap, start);
-}
-
-static void get_regs(struct net_device *dev, struct ethtool_regs *regs,
-		     void *buf)
-{
-	static const unsigned int t4_reg_ranges[] = {
-		0x1008, 0x1108,
-		0x1180, 0x11b4,
-		0x11fc, 0x123c,
-		0x1300, 0x173c,
-		0x1800, 0x18fc,
-		0x3000, 0x30d8,
-		0x30e0, 0x5924,
-		0x5960, 0x59d4,
-		0x5a00, 0x5af8,
-		0x6000, 0x6098,
-		0x6100, 0x6150,
-		0x6200, 0x6208,
-		0x6240, 0x6248,
-		0x6280, 0x6338,
-		0x6370, 0x638c,
-		0x6400, 0x643c,
-		0x6500, 0x6524,
-		0x6a00, 0x6a38,
-		0x6a60, 0x6a78,
-		0x6b00, 0x6b84,
-		0x6bf0, 0x6c84,
-		0x6cf0, 0x6d84,
-		0x6df0, 0x6e84,
-		0x6ef0, 0x6f84,
-		0x6ff0, 0x7084,
-		0x70f0, 0x7184,
-		0x71f0, 0x7284,
-		0x72f0, 0x7384,
-		0x73f0, 0x7450,
-		0x7500, 0x7530,
-		0x7600, 0x761c,
-		0x7680, 0x76cc,
-		0x7700, 0x7798,
-		0x77c0, 0x77fc,
-		0x7900, 0x79fc,
-		0x7b00, 0x7c38,
-		0x7d00, 0x7efc,
-		0x8dc0, 0x8e1c,
-		0x8e30, 0x8e78,
-		0x8ea0, 0x8f6c,
-		0x8fc0, 0x9074,
-		0x90fc, 0x90fc,
-		0x9400, 0x9458,
-		0x9600, 0x96bc,
-		0x9800, 0x9808,
-		0x9820, 0x983c,
-		0x9850, 0x9864,
-		0x9c00, 0x9c6c,
-		0x9c80, 0x9cec,
-		0x9d00, 0x9d6c,
-		0x9d80, 0x9dec,
-		0x9e00, 0x9e6c,
-		0x9e80, 0x9eec,
-		0x9f00, 0x9f6c,
-		0x9f80, 0x9fec,
-		0xd004, 0xd03c,
-		0xdfc0, 0xdfe0,
-		0xe000, 0xea7c,
-		0xf000, 0x11110,
-		0x11118, 0x11190,
-		0x19040, 0x1906c,
-		0x19078, 0x19080,
-		0x1908c, 0x19124,
-		0x19150, 0x191b0,
-		0x191d0, 0x191e8,
-		0x19238, 0x1924c,
-		0x193f8, 0x19474,
-		0x19490, 0x194f8,
-		0x19800, 0x19f30,
-		0x1a000, 0x1a06c,
-		0x1a0b0, 0x1a120,
-		0x1a128, 0x1a138,
-		0x1a190, 0x1a1c4,
-		0x1a1fc, 0x1a1fc,
-		0x1e040, 0x1e04c,
-		0x1e284, 0x1e28c,
-		0x1e2c0, 0x1e2c0,
-		0x1e2e0, 0x1e2e0,
-		0x1e300, 0x1e384,
-		0x1e3c0, 0x1e3c8,
-		0x1e440, 0x1e44c,
-		0x1e684, 0x1e68c,
-		0x1e6c0, 0x1e6c0,
-		0x1e6e0, 0x1e6e0,
-		0x1e700, 0x1e784,
-		0x1e7c0, 0x1e7c8,
-		0x1e840, 0x1e84c,
-		0x1ea84, 0x1ea8c,
-		0x1eac0, 0x1eac0,
-		0x1eae0, 0x1eae0,
-		0x1eb00, 0x1eb84,
-		0x1ebc0, 0x1ebc8,
-		0x1ec40, 0x1ec4c,
-		0x1ee84, 0x1ee8c,
-		0x1eec0, 0x1eec0,
-		0x1eee0, 0x1eee0,
-		0x1ef00, 0x1ef84,
-		0x1efc0, 0x1efc8,
-		0x1f040, 0x1f04c,
-		0x1f284, 0x1f28c,
-		0x1f2c0, 0x1f2c0,
-		0x1f2e0, 0x1f2e0,
-		0x1f300, 0x1f384,
-		0x1f3c0, 0x1f3c8,
-		0x1f440, 0x1f44c,
-		0x1f684, 0x1f68c,
-		0x1f6c0, 0x1f6c0,
-		0x1f6e0, 0x1f6e0,
-		0x1f700, 0x1f784,
-		0x1f7c0, 0x1f7c8,
-		0x1f840, 0x1f84c,
-		0x1fa84, 0x1fa8c,
-		0x1fac0, 0x1fac0,
-		0x1fae0, 0x1fae0,
-		0x1fb00, 0x1fb84,
-		0x1fbc0, 0x1fbc8,
-		0x1fc40, 0x1fc4c,
-		0x1fe84, 0x1fe8c,
-		0x1fec0, 0x1fec0,
-		0x1fee0, 0x1fee0,
-		0x1ff00, 0x1ff84,
-		0x1ffc0, 0x1ffc8,
-		0x20000, 0x2002c,
-		0x20100, 0x2013c,
-		0x20190, 0x201c8,
-		0x20200, 0x20318,
-		0x20400, 0x20528,
-		0x20540, 0x20614,
-		0x21000, 0x21040,
-		0x2104c, 0x21060,
-		0x210c0, 0x210ec,
-		0x21200, 0x21268,
-		0x21270, 0x21284,
-		0x212fc, 0x21388,
-		0x21400, 0x21404,
-		0x21500, 0x21518,
-		0x2152c, 0x2153c,
-		0x21550, 0x21554,
-		0x21600, 0x21600,
-		0x21608, 0x21628,
-		0x21630, 0x2163c,
-		0x21700, 0x2171c,
-		0x21780, 0x2178c,
-		0x21800, 0x21c38,
-		0x21c80, 0x21d7c,
-		0x21e00, 0x21e04,
-		0x22000, 0x2202c,
-		0x22100, 0x2213c,
-		0x22190, 0x221c8,
-		0x22200, 0x22318,
-		0x22400, 0x22528,
-		0x22540, 0x22614,
-		0x23000, 0x23040,
-		0x2304c, 0x23060,
-		0x230c0, 0x230ec,
-		0x23200, 0x23268,
-		0x23270, 0x23284,
-		0x232fc, 0x23388,
-		0x23400, 0x23404,
-		0x23500, 0x23518,
-		0x2352c, 0x2353c,
-		0x23550, 0x23554,
-		0x23600, 0x23600,
-		0x23608, 0x23628,
-		0x23630, 0x2363c,
-		0x23700, 0x2371c,
-		0x23780, 0x2378c,
-		0x23800, 0x23c38,
-		0x23c80, 0x23d7c,
-		0x23e00, 0x23e04,
-		0x24000, 0x2402c,
-		0x24100, 0x2413c,
-		0x24190, 0x241c8,
-		0x24200, 0x24318,
-		0x24400, 0x24528,
-		0x24540, 0x24614,
-		0x25000, 0x25040,
-		0x2504c, 0x25060,
-		0x250c0, 0x250ec,
-		0x25200, 0x25268,
-		0x25270, 0x25284,
-		0x252fc, 0x25388,
-		0x25400, 0x25404,
-		0x25500, 0x25518,
-		0x2552c, 0x2553c,
-		0x25550, 0x25554,
-		0x25600, 0x25600,
-		0x25608, 0x25628,
-		0x25630, 0x2563c,
-		0x25700, 0x2571c,
-		0x25780, 0x2578c,
-		0x25800, 0x25c38,
-		0x25c80, 0x25d7c,
-		0x25e00, 0x25e04,
-		0x26000, 0x2602c,
-		0x26100, 0x2613c,
-		0x26190, 0x261c8,
-		0x26200, 0x26318,
-		0x26400, 0x26528,
-		0x26540, 0x26614,
-		0x27000, 0x27040,
-		0x2704c, 0x27060,
-		0x270c0, 0x270ec,
-		0x27200, 0x27268,
-		0x27270, 0x27284,
-		0x272fc, 0x27388,
-		0x27400, 0x27404,
-		0x27500, 0x27518,
-		0x2752c, 0x2753c,
-		0x27550, 0x27554,
-		0x27600, 0x27600,
-		0x27608, 0x27628,
-		0x27630, 0x2763c,
-		0x27700, 0x2771c,
-		0x27780, 0x2778c,
-		0x27800, 0x27c38,
-		0x27c80, 0x27d7c,
-		0x27e00, 0x27e04
-	};
-
-	static const unsigned int t5_reg_ranges[] = {
-		0x1008, 0x1148,
-		0x1180, 0x11b4,
-		0x11fc, 0x123c,
-		0x1280, 0x173c,
-		0x1800, 0x18fc,
-		0x3000, 0x3028,
-		0x3060, 0x30d8,
-		0x30e0, 0x30fc,
-		0x3140, 0x357c,
-		0x35a8, 0x35cc,
-		0x35ec, 0x35ec,
-		0x3600, 0x5624,
-		0x56cc, 0x575c,
-		0x580c, 0x5814,
-		0x5890, 0x58bc,
-		0x5940, 0x59dc,
-		0x59fc, 0x5a18,
-		0x5a60, 0x5a9c,
-		0x5b9c, 0x5bfc,
-		0x6000, 0x6040,
-		0x6058, 0x614c,
-		0x7700, 0x7798,
-		0x77c0, 0x78fc,
-		0x7b00, 0x7c54,
-		0x7d00, 0x7efc,
-		0x8dc0, 0x8de0,
-		0x8df8, 0x8e84,
-		0x8ea0, 0x8f84,
-		0x8fc0, 0x90f8,
-		0x9400, 0x9470,
-		0x9600, 0x96f4,
-		0x9800, 0x9808,
-		0x9820, 0x983c,
-		0x9850, 0x9864,
-		0x9c00, 0x9c6c,
-		0x9c80, 0x9cec,
-		0x9d00, 0x9d6c,
-		0x9d80, 0x9dec,
-		0x9e00, 0x9e6c,
-		0x9e80, 0x9eec,
-		0x9f00, 0x9f6c,
-		0x9f80, 0xa020,
-		0xd004, 0xd03c,
-		0xdfc0, 0xdfe0,
-		0xe000, 0x11088,
-		0x1109c, 0x11110,
-		0x11118, 0x1117c,
-		0x11190, 0x11204,
-		0x19040, 0x1906c,
-		0x19078, 0x19080,
-		0x1908c, 0x19124,
-		0x19150, 0x191b0,
-		0x191d0, 0x191e8,
-		0x19238, 0x19290,
-		0x193f8, 0x19474,
-		0x19490, 0x194cc,
-		0x194f0, 0x194f8,
-		0x19c00, 0x19c60,
-		0x19c94, 0x19e10,
-		0x19e50, 0x19f34,
-		0x19f40, 0x19f50,
-		0x19f90, 0x19fe4,
-		0x1a000, 0x1a06c,
-		0x1a0b0, 0x1a120,
-		0x1a128, 0x1a138,
-		0x1a190, 0x1a1c4,
-		0x1a1fc, 0x1a1fc,
-		0x1e008, 0x1e00c,
-		0x1e040, 0x1e04c,
-		0x1e284, 0x1e290,
-		0x1e2c0, 0x1e2c0,
-		0x1e2e0, 0x1e2e0,
-		0x1e300, 0x1e384,
-		0x1e3c0, 0x1e3c8,
-		0x1e408, 0x1e40c,
-		0x1e440, 0x1e44c,
-		0x1e684, 0x1e690,
-		0x1e6c0, 0x1e6c0,
-		0x1e6e0, 0x1e6e0,
-		0x1e700, 0x1e784,
-		0x1e7c0, 0x1e7c8,
-		0x1e808, 0x1e80c,
-		0x1e840, 0x1e84c,
-		0x1ea84, 0x1ea90,
-		0x1eac0, 0x1eac0,
-		0x1eae0, 0x1eae0,
-		0x1eb00, 0x1eb84,
-		0x1ebc0, 0x1ebc8,
-		0x1ec08, 0x1ec0c,
-		0x1ec40, 0x1ec4c,
-		0x1ee84, 0x1ee90,
-		0x1eec0, 0x1eec0,
-		0x1eee0, 0x1eee0,
-		0x1ef00, 0x1ef84,
-		0x1efc0, 0x1efc8,
-		0x1f008, 0x1f00c,
-		0x1f040, 0x1f04c,
-		0x1f284, 0x1f290,
-		0x1f2c0, 0x1f2c0,
-		0x1f2e0, 0x1f2e0,
-		0x1f300, 0x1f384,
-		0x1f3c0, 0x1f3c8,
-		0x1f408, 0x1f40c,
-		0x1f440, 0x1f44c,
-		0x1f684, 0x1f690,
-		0x1f6c0, 0x1f6c0,
-		0x1f6e0, 0x1f6e0,
-		0x1f700, 0x1f784,
-		0x1f7c0, 0x1f7c8,
-		0x1f808, 0x1f80c,
-		0x1f840, 0x1f84c,
-		0x1fa84, 0x1fa90,
-		0x1fac0, 0x1fac0,
-		0x1fae0, 0x1fae0,
-		0x1fb00, 0x1fb84,
-		0x1fbc0, 0x1fbc8,
-		0x1fc08, 0x1fc0c,
-		0x1fc40, 0x1fc4c,
-		0x1fe84, 0x1fe90,
-		0x1fec0, 0x1fec0,
-		0x1fee0, 0x1fee0,
-		0x1ff00, 0x1ff84,
-		0x1ffc0, 0x1ffc8,
-		0x30000, 0x30030,
-		0x30100, 0x30144,
-		0x30190, 0x301d0,
-		0x30200, 0x30318,
-		0x30400, 0x3052c,
-		0x30540, 0x3061c,
-		0x30800, 0x30834,
-		0x308c0, 0x30908,
-		0x30910, 0x309ac,
-		0x30a00, 0x30a04,
-		0x30a0c, 0x30a2c,
-		0x30a44, 0x30a50,
-		0x30a74, 0x30c24,
-		0x30d08, 0x30d14,
-		0x30d1c, 0x30d20,
-		0x30d3c, 0x30d50,
-		0x31200, 0x3120c,
-		0x31220, 0x31220,
-		0x31240, 0x31240,
-		0x31600, 0x31600,
-		0x31608, 0x3160c,
-		0x31a00, 0x31a1c,
-		0x31e04, 0x31e20,
-		0x31e38, 0x31e3c,
-		0x31e80, 0x31e80,
-		0x31e88, 0x31ea8,
-		0x31eb0, 0x31eb4,
-		0x31ec8, 0x31ed4,
-		0x31fb8, 0x32004,
-		0x32208, 0x3223c,
-		0x32600, 0x32630,
-		0x32a00, 0x32abc,
-		0x32b00, 0x32b70,
-		0x33000, 0x33048,
-		0x33060, 0x3309c,
-		0x330f0, 0x33148,
-		0x33160, 0x3319c,
-		0x331f0, 0x332e4,
-		0x332f8, 0x333e4,
-		0x333f8, 0x33448,
-		0x33460, 0x3349c,
-		0x334f0, 0x33548,
-		0x33560, 0x3359c,
-		0x335f0, 0x336e4,
-		0x336f8, 0x337e4,
-		0x337f8, 0x337fc,
-		0x33814, 0x33814,
-		0x3382c, 0x3382c,
-		0x33880, 0x3388c,
-		0x338e8, 0x338ec,
-		0x33900, 0x33948,
-		0x33960, 0x3399c,
-		0x339f0, 0x33ae4,
-		0x33af8, 0x33b10,
-		0x33b28, 0x33b28,
-		0x33b3c, 0x33b50,
-		0x33bf0, 0x33c10,
-		0x33c28, 0x33c28,
-		0x33c3c, 0x33c50,
-		0x33cf0, 0x33cfc,
-		0x34000, 0x34030,
-		0x34100, 0x34144,
-		0x34190, 0x341d0,
-		0x34200, 0x34318,
-		0x34400, 0x3452c,
-		0x34540, 0x3461c,
-		0x34800, 0x34834,
-		0x348c0, 0x34908,
-		0x34910, 0x349ac,
-		0x34a00, 0x34a04,
-		0x34a0c, 0x34a2c,
-		0x34a44, 0x34a50,
-		0x34a74, 0x34c24,
-		0x34d08, 0x34d14,
-		0x34d1c, 0x34d20,
-		0x34d3c, 0x34d50,
-		0x35200, 0x3520c,
-		0x35220, 0x35220,
-		0x35240, 0x35240,
-		0x35600, 0x35600,
-		0x35608, 0x3560c,
-		0x35a00, 0x35a1c,
-		0x35e04, 0x35e20,
-		0x35e38, 0x35e3c,
-		0x35e80, 0x35e80,
-		0x35e88, 0x35ea8,
-		0x35eb0, 0x35eb4,
-		0x35ec8, 0x35ed4,
-		0x35fb8, 0x36004,
-		0x36208, 0x3623c,
-		0x36600, 0x36630,
-		0x36a00, 0x36abc,
-		0x36b00, 0x36b70,
-		0x37000, 0x37048,
-		0x37060, 0x3709c,
-		0x370f0, 0x37148,
-		0x37160, 0x3719c,
-		0x371f0, 0x372e4,
-		0x372f8, 0x373e4,
-		0x373f8, 0x37448,
-		0x37460, 0x3749c,
-		0x374f0, 0x37548,
-		0x37560, 0x3759c,
-		0x375f0, 0x376e4,
-		0x376f8, 0x377e4,
-		0x377f8, 0x377fc,
-		0x37814, 0x37814,
-		0x3782c, 0x3782c,
-		0x37880, 0x3788c,
-		0x378e8, 0x378ec,
-		0x37900, 0x37948,
-		0x37960, 0x3799c,
-		0x379f0, 0x37ae4,
-		0x37af8, 0x37b10,
-		0x37b28, 0x37b28,
-		0x37b3c, 0x37b50,
-		0x37bf0, 0x37c10,
-		0x37c28, 0x37c28,
-		0x37c3c, 0x37c50,
-		0x37cf0, 0x37cfc,
-		0x38000, 0x38030,
-		0x38100, 0x38144,
-		0x38190, 0x381d0,
-		0x38200, 0x38318,
-		0x38400, 0x3852c,
-		0x38540, 0x3861c,
-		0x38800, 0x38834,
-		0x388c0, 0x38908,
-		0x38910, 0x389ac,
-		0x38a00, 0x38a04,
-		0x38a0c, 0x38a2c,
-		0x38a44, 0x38a50,
-		0x38a74, 0x38c24,
-		0x38d08, 0x38d14,
-		0x38d1c, 0x38d20,
-		0x38d3c, 0x38d50,
-		0x39200, 0x3920c,
-		0x39220, 0x39220,
-		0x39240, 0x39240,
-		0x39600, 0x39600,
-		0x39608, 0x3960c,
-		0x39a00, 0x39a1c,
-		0x39e04, 0x39e20,
-		0x39e38, 0x39e3c,
-		0x39e80, 0x39e80,
-		0x39e88, 0x39ea8,
-		0x39eb0, 0x39eb4,
-		0x39ec8, 0x39ed4,
-		0x39fb8, 0x3a004,
-		0x3a208, 0x3a23c,
-		0x3a600, 0x3a630,
-		0x3aa00, 0x3aabc,
-		0x3ab00, 0x3ab70,
-		0x3b000, 0x3b048,
-		0x3b060, 0x3b09c,
-		0x3b0f0, 0x3b148,
-		0x3b160, 0x3b19c,
-		0x3b1f0, 0x3b2e4,
-		0x3b2f8, 0x3b3e4,
-		0x3b3f8, 0x3b448,
-		0x3b460, 0x3b49c,
-		0x3b4f0, 0x3b548,
-		0x3b560, 0x3b59c,
-		0x3b5f0, 0x3b6e4,
-		0x3b6f8, 0x3b7e4,
-		0x3b7f8, 0x3b7fc,
-		0x3b814, 0x3b814,
-		0x3b82c, 0x3b82c,
-		0x3b880, 0x3b88c,
-		0x3b8e8, 0x3b8ec,
-		0x3b900, 0x3b948,
-		0x3b960, 0x3b99c,
-		0x3b9f0, 0x3bae4,
-		0x3baf8, 0x3bb10,
-		0x3bb28, 0x3bb28,
-		0x3bb3c, 0x3bb50,
-		0x3bbf0, 0x3bc10,
-		0x3bc28, 0x3bc28,
-		0x3bc3c, 0x3bc50,
-		0x3bcf0, 0x3bcfc,
-		0x3c000, 0x3c030,
-		0x3c100, 0x3c144,
-		0x3c190, 0x3c1d0,
-		0x3c200, 0x3c318,
-		0x3c400, 0x3c52c,
-		0x3c540, 0x3c61c,
-		0x3c800, 0x3c834,
-		0x3c8c0, 0x3c908,
-		0x3c910, 0x3c9ac,
-		0x3ca00, 0x3ca04,
-		0x3ca0c, 0x3ca2c,
-		0x3ca44, 0x3ca50,
-		0x3ca74, 0x3cc24,
-		0x3cd08, 0x3cd14,
-		0x3cd1c, 0x3cd20,
-		0x3cd3c, 0x3cd50,
-		0x3d200, 0x3d20c,
-		0x3d220, 0x3d220,
-		0x3d240, 0x3d240,
-		0x3d600, 0x3d600,
-		0x3d608, 0x3d60c,
-		0x3da00, 0x3da1c,
-		0x3de04, 0x3de20,
-		0x3de38, 0x3de3c,
-		0x3de80, 0x3de80,
-		0x3de88, 0x3dea8,
-		0x3deb0, 0x3deb4,
-		0x3dec8, 0x3ded4,
-		0x3dfb8, 0x3e004,
-		0x3e208, 0x3e23c,
-		0x3e600, 0x3e630,
-		0x3ea00, 0x3eabc,
-		0x3eb00, 0x3eb70,
-		0x3f000, 0x3f048,
-		0x3f060, 0x3f09c,
-		0x3f0f0, 0x3f148,
-		0x3f160, 0x3f19c,
-		0x3f1f0, 0x3f2e4,
-		0x3f2f8, 0x3f3e4,
-		0x3f3f8, 0x3f448,
-		0x3f460, 0x3f49c,
-		0x3f4f0, 0x3f548,
-		0x3f560, 0x3f59c,
-		0x3f5f0, 0x3f6e4,
-		0x3f6f8, 0x3f7e4,
-		0x3f7f8, 0x3f7fc,
-		0x3f814, 0x3f814,
-		0x3f82c, 0x3f82c,
-		0x3f880, 0x3f88c,
-		0x3f8e8, 0x3f8ec,
-		0x3f900, 0x3f948,
-		0x3f960, 0x3f99c,
-		0x3f9f0, 0x3fae4,
-		0x3faf8, 0x3fb10,
-		0x3fb28, 0x3fb28,
-		0x3fb3c, 0x3fb50,
-		0x3fbf0, 0x3fc10,
-		0x3fc28, 0x3fc28,
-		0x3fc3c, 0x3fc50,
-		0x3fcf0, 0x3fcfc,
-		0x40000, 0x4000c,
-		0x40040, 0x40068,
-		0x40080, 0x40144,
-		0x40180, 0x4018c,
-		0x40200, 0x40298,
-		0x402ac, 0x4033c,
-		0x403f8, 0x403fc,
-		0x41304, 0x413c4,
-		0x41400, 0x4141c,
-		0x41480, 0x414d0,
-		0x44000, 0x44078,
-		0x440c0, 0x44278,
-		0x442c0, 0x44478,
-		0x444c0, 0x44678,
-		0x446c0, 0x44878,
-		0x448c0, 0x449fc,
-		0x45000, 0x45068,
-		0x45080, 0x45084,
-		0x450a0, 0x450b0,
-		0x45200, 0x45268,
-		0x45280, 0x45284,
-		0x452a0, 0x452b0,
-		0x460c0, 0x460e4,
-		0x47000, 0x4708c,
-		0x47200, 0x47250,
-		0x47400, 0x47420,
-		0x47600, 0x47618,
-		0x47800, 0x47814,
-		0x48000, 0x4800c,
-		0x48040, 0x48068,
-		0x48080, 0x48144,
-		0x48180, 0x4818c,
-		0x48200, 0x48298,
-		0x482ac, 0x4833c,
-		0x483f8, 0x483fc,
-		0x49304, 0x493c4,
-		0x49400, 0x4941c,
-		0x49480, 0x494d0,
-		0x4c000, 0x4c078,
-		0x4c0c0, 0x4c278,
-		0x4c2c0, 0x4c478,
-		0x4c4c0, 0x4c678,
-		0x4c6c0, 0x4c878,
-		0x4c8c0, 0x4c9fc,
-		0x4d000, 0x4d068,
-		0x4d080, 0x4d084,
-		0x4d0a0, 0x4d0b0,
-		0x4d200, 0x4d268,
-		0x4d280, 0x4d284,
-		0x4d2a0, 0x4d2b0,
-		0x4e0c0, 0x4e0e4,
-		0x4f000, 0x4f08c,
-		0x4f200, 0x4f250,
-		0x4f400, 0x4f420,
-		0x4f600, 0x4f618,
-		0x4f800, 0x4f814,
-		0x50000, 0x500cc,
-		0x50400, 0x50400,
-		0x50800, 0x508cc,
-		0x50c00, 0x50c00,
-		0x51000, 0x5101c,
-		0x51300, 0x51308,
-	};
-
-	int i;
-	struct adapter *ap = netdev2adap(dev);
-	static const unsigned int *reg_ranges;
-	int arr_size = 0, buf_size = 0;
-
-	if (is_t4(ap->params.chip)) {
-		reg_ranges = &t4_reg_ranges[0];
-		arr_size = ARRAY_SIZE(t4_reg_ranges);
-		buf_size = T4_REGMAP_SIZE;
-	} else {
-		reg_ranges = &t5_reg_ranges[0];
-		arr_size = ARRAY_SIZE(t5_reg_ranges);
-		buf_size = T5_REGMAP_SIZE;
-	}
-
-	regs->version = mk_adap_vers(ap);
-
-	memset(buf, 0, buf_size);
-	for (i = 0; i < arr_size; i += 2)
-		reg_block_dump(ap, buf, reg_ranges[i], reg_ranges[i + 1]);
-}
-
-static int restart_autoneg(struct net_device *dev)
-{
-	struct port_info *p = netdev_priv(dev);
-
-	if (!netif_running(dev))
-		return -EAGAIN;
-	if (p->link_cfg.autoneg != AUTONEG_ENABLE)
-		return -EINVAL;
-	t4_restart_aneg(p->adapter, p->adapter->fn, p->tx_chan);
-	return 0;
-}
-
-static int identify_port(struct net_device *dev,
-			 enum ethtool_phys_id_state state)
-{
-	unsigned int val;
-	struct adapter *adap = netdev2adap(dev);
-
-	if (state == ETHTOOL_ID_ACTIVE)
-		val = 0xffff;
-	else if (state == ETHTOOL_ID_INACTIVE)
-		val = 0;
-	else
-		return -EINVAL;
-
-	return t4_identify_port(adap, adap->fn, netdev2pinfo(dev)->viid, val);
-}
-
-static unsigned int from_fw_linkcaps(enum fw_port_type type, unsigned int caps)
-{
-	unsigned int v = 0;
-
-	if (type == FW_PORT_TYPE_BT_SGMII || type == FW_PORT_TYPE_BT_XFI ||
-	    type == FW_PORT_TYPE_BT_XAUI) {
-		v |= SUPPORTED_TP;
-		if (caps & FW_PORT_CAP_SPEED_100M)
-			v |= SUPPORTED_100baseT_Full;
-		if (caps & FW_PORT_CAP_SPEED_1G)
-			v |= SUPPORTED_1000baseT_Full;
-		if (caps & FW_PORT_CAP_SPEED_10G)
-			v |= SUPPORTED_10000baseT_Full;
-	} else if (type == FW_PORT_TYPE_KX4 || type == FW_PORT_TYPE_KX) {
-		v |= SUPPORTED_Backplane;
-		if (caps & FW_PORT_CAP_SPEED_1G)
-			v |= SUPPORTED_1000baseKX_Full;
-		if (caps & FW_PORT_CAP_SPEED_10G)
-			v |= SUPPORTED_10000baseKX4_Full;
-	} else if (type == FW_PORT_TYPE_KR)
-		v |= SUPPORTED_Backplane | SUPPORTED_10000baseKR_Full;
-	else if (type == FW_PORT_TYPE_BP_AP)
-		v |= SUPPORTED_Backplane | SUPPORTED_10000baseR_FEC |
-		     SUPPORTED_10000baseKR_Full | SUPPORTED_1000baseKX_Full;
-	else if (type == FW_PORT_TYPE_BP4_AP)
-		v |= SUPPORTED_Backplane | SUPPORTED_10000baseR_FEC |
-		     SUPPORTED_10000baseKR_Full | SUPPORTED_1000baseKX_Full |
-		     SUPPORTED_10000baseKX4_Full;
-	else if (type == FW_PORT_TYPE_FIBER_XFI ||
-		 type == FW_PORT_TYPE_FIBER_XAUI ||
-		 type == FW_PORT_TYPE_SFP ||
-		 type == FW_PORT_TYPE_QSFP_10G ||
-		 type == FW_PORT_TYPE_QSA) {
-		v |= SUPPORTED_FIBRE;
-		if (caps & FW_PORT_CAP_SPEED_1G)
-			v |= SUPPORTED_1000baseT_Full;
-		if (caps & FW_PORT_CAP_SPEED_10G)
-			v |= SUPPORTED_10000baseT_Full;
-	} else if (type == FW_PORT_TYPE_BP40_BA ||
-		   type == FW_PORT_TYPE_QSFP) {
-		v |= SUPPORTED_40000baseSR4_Full;
-		v |= SUPPORTED_FIBRE;
-	}
-
-	if (caps & FW_PORT_CAP_ANEG)
-		v |= SUPPORTED_Autoneg;
-	return v;
-}
-
-static unsigned int to_fw_linkcaps(unsigned int caps)
-{
-	unsigned int v = 0;
-
-	if (caps & ADVERTISED_100baseT_Full)
-		v |= FW_PORT_CAP_SPEED_100M;
-	if (caps & ADVERTISED_1000baseT_Full)
-		v |= FW_PORT_CAP_SPEED_1G;
-	if (caps & ADVERTISED_10000baseT_Full)
-		v |= FW_PORT_CAP_SPEED_10G;
-	if (caps & ADVERTISED_40000baseSR4_Full)
-		v |= FW_PORT_CAP_SPEED_40G;
-	return v;
-}
-
-static int get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
-{
-	const struct port_info *p = netdev_priv(dev);
-
-	if (p->port_type == FW_PORT_TYPE_BT_SGMII ||
-	    p->port_type == FW_PORT_TYPE_BT_XFI ||
-	    p->port_type == FW_PORT_TYPE_BT_XAUI)
-		cmd->port = PORT_TP;
-	else if (p->port_type == FW_PORT_TYPE_FIBER_XFI ||
-		 p->port_type == FW_PORT_TYPE_FIBER_XAUI)
-		cmd->port = PORT_FIBRE;
-	else if (p->port_type == FW_PORT_TYPE_SFP ||
-		 p->port_type == FW_PORT_TYPE_QSFP_10G ||
-		 p->port_type == FW_PORT_TYPE_QSA ||
-		 p->port_type == FW_PORT_TYPE_QSFP) {
-		if (p->mod_type == FW_PORT_MOD_TYPE_LR ||
-		    p->mod_type == FW_PORT_MOD_TYPE_SR ||
-		    p->mod_type == FW_PORT_MOD_TYPE_ER ||
-		    p->mod_type == FW_PORT_MOD_TYPE_LRM)
-			cmd->port = PORT_FIBRE;
-		else if (p->mod_type == FW_PORT_MOD_TYPE_TWINAX_PASSIVE ||
-			 p->mod_type == FW_PORT_MOD_TYPE_TWINAX_ACTIVE)
-			cmd->port = PORT_DA;
-		else
-			cmd->port = PORT_OTHER;
-	} else
-		cmd->port = PORT_OTHER;
-
-	if (p->mdio_addr >= 0) {
-		cmd->phy_address = p->mdio_addr;
-		cmd->transceiver = XCVR_EXTERNAL;
-		cmd->mdio_support = p->port_type == FW_PORT_TYPE_BT_SGMII ?
-			MDIO_SUPPORTS_C22 : MDIO_SUPPORTS_C45;
-	} else {
-		cmd->phy_address = 0;  /* not really, but no better option */
-		cmd->transceiver = XCVR_INTERNAL;
-		cmd->mdio_support = 0;
-	}
-
-	cmd->supported = from_fw_linkcaps(p->port_type, p->link_cfg.supported);
-	cmd->advertising = from_fw_linkcaps(p->port_type,
-					    p->link_cfg.advertising);
-	ethtool_cmd_speed_set(cmd,
-			      netif_carrier_ok(dev) ? p->link_cfg.speed : 0);
-	cmd->duplex = DUPLEX_FULL;
-	cmd->autoneg = p->link_cfg.autoneg;
-	cmd->maxtxpkt = 0;
-	cmd->maxrxpkt = 0;
-	return 0;
-}
-
-static unsigned int speed_to_caps(int speed)
-{
-	if (speed == 100)
-		return FW_PORT_CAP_SPEED_100M;
-	if (speed == 1000)
-		return FW_PORT_CAP_SPEED_1G;
-	if (speed == 10000)
-		return FW_PORT_CAP_SPEED_10G;
-	if (speed == 40000)
-		return FW_PORT_CAP_SPEED_40G;
-	return 0;
-}
-
-static int set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
-{
-	unsigned int cap;
-	struct port_info *p = netdev_priv(dev);
-	struct link_config *lc = &p->link_cfg;
-	u32 speed = ethtool_cmd_speed(cmd);
-
-	if (cmd->duplex != DUPLEX_FULL)     /* only full-duplex supported */
-		return -EINVAL;
-
-	if (!(lc->supported & FW_PORT_CAP_ANEG)) {
-		/*
-		 * PHY offers a single speed.  See if that's what's
-		 * being requested.
-		 */
-		if (cmd->autoneg == AUTONEG_DISABLE &&
-		    (lc->supported & speed_to_caps(speed)))
-			return 0;
-		return -EINVAL;
-	}
-
-	if (cmd->autoneg == AUTONEG_DISABLE) {
-		cap = speed_to_caps(speed);
-
-		if (!(lc->supported & cap) ||
-		    (speed == 1000) ||
-		    (speed == 10000) ||
-		    (speed == 40000))
-			return -EINVAL;
-		lc->requested_speed = cap;
-		lc->advertising = 0;
-	} else {
-		cap = to_fw_linkcaps(cmd->advertising);
-		if (!(lc->supported & cap))
-			return -EINVAL;
-		lc->requested_speed = 0;
-		lc->advertising = cap | FW_PORT_CAP_ANEG;
-	}
-	lc->autoneg = cmd->autoneg;
-
-	if (netif_running(dev))
-		return t4_link_start(p->adapter, p->adapter->fn, p->tx_chan,
-				     lc);
-	return 0;
-}
-
-static void get_pauseparam(struct net_device *dev,
-			   struct ethtool_pauseparam *epause)
-{
-	struct port_info *p = netdev_priv(dev);
-
-	epause->autoneg = (p->link_cfg.requested_fc & PAUSE_AUTONEG) != 0;
-	epause->rx_pause = (p->link_cfg.fc & PAUSE_RX) != 0;
-	epause->tx_pause = (p->link_cfg.fc & PAUSE_TX) != 0;
-}
-
-static int set_pauseparam(struct net_device *dev,
-			  struct ethtool_pauseparam *epause)
-{
-	struct port_info *p = netdev_priv(dev);
-	struct link_config *lc = &p->link_cfg;
-
-	if (epause->autoneg == AUTONEG_DISABLE)
-		lc->requested_fc = 0;
-	else if (lc->supported & FW_PORT_CAP_ANEG)
-		lc->requested_fc = PAUSE_AUTONEG;
-	else
-		return -EINVAL;
-
-	if (epause->rx_pause)
-		lc->requested_fc |= PAUSE_RX;
-	if (epause->tx_pause)
-		lc->requested_fc |= PAUSE_TX;
-	if (netif_running(dev))
-		return t4_link_start(p->adapter, p->adapter->fn, p->tx_chan,
-				     lc);
-	return 0;
-}
-
-static void get_sge_param(struct net_device *dev, struct ethtool_ringparam *e)
-{
-	const struct port_info *pi = netdev_priv(dev);
-	const struct sge *s = &pi->adapter->sge;
-
-	e->rx_max_pending = MAX_RX_BUFFERS;
-	e->rx_mini_max_pending = MAX_RSPQ_ENTRIES;
-	e->rx_jumbo_max_pending = 0;
-	e->tx_max_pending = MAX_TXQ_ENTRIES;
-
-	e->rx_pending = s->ethrxq[pi->first_qset].fl.size - 8;
-	e->rx_mini_pending = s->ethrxq[pi->first_qset].rspq.size;
-	e->rx_jumbo_pending = 0;
-	e->tx_pending = s->ethtxq[pi->first_qset].q.size;
-}
-
-static int set_sge_param(struct net_device *dev, struct ethtool_ringparam *e)
-{
-	int i;
-	const struct port_info *pi = netdev_priv(dev);
-	struct adapter *adapter = pi->adapter;
-	struct sge *s = &adapter->sge;
-
-	if (e->rx_pending > MAX_RX_BUFFERS || e->rx_jumbo_pending ||
-	    e->tx_pending > MAX_TXQ_ENTRIES ||
-	    e->rx_mini_pending > MAX_RSPQ_ENTRIES ||
-	    e->rx_mini_pending < MIN_RSPQ_ENTRIES ||
-	    e->rx_pending < MIN_FL_ENTRIES || e->tx_pending < MIN_TXQ_ENTRIES)
-		return -EINVAL;
-
-	if (adapter->flags & FULL_INIT_DONE)
-		return -EBUSY;
-
-	for (i = 0; i < pi->nqsets; ++i) {
-		s->ethtxq[pi->first_qset + i].q.size = e->tx_pending;
-		s->ethrxq[pi->first_qset + i].fl.size = e->rx_pending + 8;
-		s->ethrxq[pi->first_qset + i].rspq.size = e->rx_mini_pending;
-	}
-	return 0;
-}
-
 static int closest_timer(const struct sge *s, int time)
 {
 	int i, delta, match = 0, min_delta = INT_MAX;
@@ -2515,19 +1352,8 @@ static int closest_thres(const struct sge *s, int thres)
 	return match;
 }
 
-/*
- * Return a queue's interrupt hold-off time in us.  0 means no timer.
- */
-unsigned int qtimer_val(const struct adapter *adap,
-			const struct sge_rspq *q)
-{
-	unsigned int idx = q->intr_params >> 1;
-
-	return idx < SGE_NTIMERS ? adap->sge.timer_val[idx] : 0;
-}
-
 /**
- *	set_rspq_intr_params - set a queue's interrupt holdoff parameters
+ *	cxgb4_set_rspq_intr_params - set a queue's interrupt holdoff parameters
  *	@q: the Rx queue
  *	@us: the hold-off time in us, or 0 to disable timer
  *	@cnt: the hold-off packet count, or 0 to disable counter
@@ -2535,8 +1361,8 @@ unsigned int qtimer_val(const struct adapter *adap,
  *	Sets an Rx queue's interrupt hold-off time and packet count.  At least
  *	one of the two needs to be enabled for the queue to generate interrupts.
  */
-static int set_rspq_intr_params(struct sge_rspq *q,
-				unsigned int us, unsigned int cnt)
+int cxgb4_set_rspq_intr_params(struct sge_rspq *q,
+			       unsigned int us, unsigned int cnt)
 {
 	struct adapter *adap = q->adap;
 
@@ -2567,259 +1393,6 @@ static int set_rspq_intr_params(struct sge_rspq *q,
 	return 0;
 }
 
-/**
- * set_rx_intr_params - set a net devices's RX interrupt holdoff paramete!
- * @dev: the network device
- * @us: the hold-off time in us, or 0 to disable timer
- * @cnt: the hold-off packet count, or 0 to disable counter
- *
- * Set the RX interrupt hold-off parameters for a network device.
- */
-static int set_rx_intr_params(struct net_device *dev,
-			      unsigned int us, unsigned int cnt)
-{
-	int i, err;
-	struct port_info *pi = netdev_priv(dev);
-	struct adapter *adap = pi->adapter;
-	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
-
-	for (i = 0; i < pi->nqsets; i++, q++) {
-		err = set_rspq_intr_params(&q->rspq, us, cnt);
-		if (err)
-			return err;
-	}
-	return 0;
-}
-
-static int set_adaptive_rx_setting(struct net_device *dev, int adaptive_rx)
-{
-	int i;
-	struct port_info *pi = netdev_priv(dev);
-	struct adapter *adap = pi->adapter;
-	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
-
-	for (i = 0; i < pi->nqsets; i++, q++)
-		q->rspq.adaptive_rx = adaptive_rx;
-
-	return 0;
-}
-
-static int get_adaptive_rx_setting(struct net_device *dev)
-{
-	struct port_info *pi = netdev_priv(dev);
-	struct adapter *adap = pi->adapter;
-	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
-
-	return q->rspq.adaptive_rx;
-}
-
-static int set_coalesce(struct net_device *dev, struct ethtool_coalesce *c)
-{
-	set_adaptive_rx_setting(dev, c->use_adaptive_rx_coalesce);
-	return set_rx_intr_params(dev, c->rx_coalesce_usecs,
-				  c->rx_max_coalesced_frames);
-}
-
-static int get_coalesce(struct net_device *dev, struct ethtool_coalesce *c)
-{
-	const struct port_info *pi = netdev_priv(dev);
-	const struct adapter *adap = pi->adapter;
-	const struct sge_rspq *rq = &adap->sge.ethrxq[pi->first_qset].rspq;
-
-	c->rx_coalesce_usecs = qtimer_val(adap, rq);
-	c->rx_max_coalesced_frames = (rq->intr_params & QINTR_CNT_EN) ?
-		adap->sge.counter_val[rq->pktcnt_idx] : 0;
-	c->use_adaptive_rx_coalesce = get_adaptive_rx_setting(dev);
-	return 0;
-}
-
-/**
- *	eeprom_ptov - translate a physical EEPROM address to virtual
- *	@phys_addr: the physical EEPROM address
- *	@fn: the PCI function number
- *	@sz: size of function-specific area
- *
- *	Translate a physical EEPROM address to virtual.  The first 1K is
- *	accessed through virtual addresses starting at 31K, the rest is
- *	accessed through virtual addresses starting at 0.
- *
- *	The mapping is as follows:
- *	[0..1K) -> [31K..32K)
- *	[1K..1K+A) -> [31K-A..31K)
- *	[1K+A..ES) -> [0..ES-A-1K)
- *
- *	where A = @fn * @sz, and ES = EEPROM size.
- */
-static int eeprom_ptov(unsigned int phys_addr, unsigned int fn, unsigned int sz)
-{
-	fn *= sz;
-	if (phys_addr < 1024)
-		return phys_addr + (31 << 10);
-	if (phys_addr < 1024 + fn)
-		return 31744 - fn + phys_addr - 1024;
-	if (phys_addr < EEPROMSIZE)
-		return phys_addr - 1024 - fn;
-	return -EINVAL;
-}
-
-/*
- * The next two routines implement eeprom read/write from physical addresses.
- */
-static int eeprom_rd_phys(struct adapter *adap, unsigned int phys_addr, u32 *v)
-{
-	int vaddr = eeprom_ptov(phys_addr, adap->fn, EEPROMPFSIZE);
-
-	if (vaddr >= 0)
-		vaddr = pci_read_vpd(adap->pdev, vaddr, sizeof(u32), v);
-	return vaddr < 0 ? vaddr : 0;
-}
-
-static int eeprom_wr_phys(struct adapter *adap, unsigned int phys_addr, u32 v)
-{
-	int vaddr = eeprom_ptov(phys_addr, adap->fn, EEPROMPFSIZE);
-
-	if (vaddr >= 0)
-		vaddr = pci_write_vpd(adap->pdev, vaddr, sizeof(u32), &v);
-	return vaddr < 0 ? vaddr : 0;
-}
-
-#define EEPROM_MAGIC 0x38E2F10C
-
-static int get_eeprom(struct net_device *dev, struct ethtool_eeprom *e,
-		      u8 *data)
-{
-	int i, err = 0;
-	struct adapter *adapter = netdev2adap(dev);
-
-	u8 *buf = kmalloc(EEPROMSIZE, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	e->magic = EEPROM_MAGIC;
-	for (i = e->offset & ~3; !err && i < e->offset + e->len; i += 4)
-		err = eeprom_rd_phys(adapter, i, (u32 *)&buf[i]);
-
-	if (!err)
-		memcpy(data, buf + e->offset, e->len);
-	kfree(buf);
-	return err;
-}
-
-static int set_eeprom(struct net_device *dev, struct ethtool_eeprom *eeprom,
-		      u8 *data)
-{
-	u8 *buf;
-	int err = 0;
-	u32 aligned_offset, aligned_len, *p;
-	struct adapter *adapter = netdev2adap(dev);
-
-	if (eeprom->magic != EEPROM_MAGIC)
-		return -EINVAL;
-
-	aligned_offset = eeprom->offset & ~3;
-	aligned_len = (eeprom->len + (eeprom->offset & 3) + 3) & ~3;
-
-	if (adapter->fn > 0) {
-		u32 start = 1024 + adapter->fn * EEPROMPFSIZE;
-
-		if (aligned_offset < start ||
-		    aligned_offset + aligned_len > start + EEPROMPFSIZE)
-			return -EPERM;
-	}
-
-	if (aligned_offset != eeprom->offset || aligned_len != eeprom->len) {
-		/*
-		 * RMW possibly needed for first or last words.
-		 */
-		buf = kmalloc(aligned_len, GFP_KERNEL);
-		if (!buf)
-			return -ENOMEM;
-		err = eeprom_rd_phys(adapter, aligned_offset, (u32 *)buf);
-		if (!err && aligned_len > 4)
-			err = eeprom_rd_phys(adapter,
-					     aligned_offset + aligned_len - 4,
-					     (u32 *)&buf[aligned_len - 4]);
-		if (err)
-			goto out;
-		memcpy(buf + (eeprom->offset & 3), data, eeprom->len);
-	} else
-		buf = data;
-
-	err = t4_seeprom_wp(adapter, false);
-	if (err)
-		goto out;
-
-	for (p = (u32 *)buf; !err && aligned_len; aligned_len -= 4, p++) {
-		err = eeprom_wr_phys(adapter, aligned_offset, *p);
-		aligned_offset += 4;
-	}
-
-	if (!err)
-		err = t4_seeprom_wp(adapter, true);
-out:
-	if (buf != data)
-		kfree(buf);
-	return err;
-}
-
-static int set_flash(struct net_device *netdev, struct ethtool_flash *ef)
-{
-	int ret;
-	const struct firmware *fw;
-	struct adapter *adap = netdev2adap(netdev);
-	unsigned int mbox = PCIE_FW_MASTER_M + 1;
-
-	ef->data[sizeof(ef->data) - 1] = '\0';
-	ret = request_firmware(&fw, ef->data, adap->pdev_dev);
-	if (ret < 0)
-		return ret;
-
-	/* If the adapter has been fully initialized then we'll go ahead and
-	 * try to get the firmware's cooperation in upgrading to the new
-	 * firmware image otherwise we'll try to do the entire job from the
-	 * host ... and we always "force" the operation in this path.
-	 */
-	if (adap->flags & FULL_INIT_DONE)
-		mbox = adap->mbox;
-
-	ret = t4_fw_upgrade(adap, mbox, fw->data, fw->size, 1);
-	release_firmware(fw);
-	if (!ret)
-		dev_info(adap->pdev_dev, "loaded firmware %s,"
-			 " reload cxgb4 driver\n", ef->data);
-	return ret;
-}
-
-#define WOL_SUPPORTED (WAKE_BCAST | WAKE_MAGIC)
-#define BCAST_CRC 0xa0ccc1a6
-
-static void get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
-{
-	wol->supported = WAKE_BCAST | WAKE_MAGIC;
-	wol->wolopts = netdev2adap(dev)->wol;
-	memset(&wol->sopass, 0, sizeof(wol->sopass));
-}
-
-static int set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
-{
-	int err = 0;
-	struct port_info *pi = netdev_priv(dev);
-
-	if (wol->wolopts & ~WOL_SUPPORTED)
-		return -EINVAL;
-	t4_wol_magic_enable(pi->adapter, pi->tx_chan,
-			    (wol->wolopts & WAKE_MAGIC) ? dev->dev_addr : NULL);
-	if (wol->wolopts & WAKE_BCAST) {
-		err = t4_wol_pat_enable(pi->adapter, pi->tx_chan, 0xfe, ~0ULL,
-					~0ULL, 0, false);
-		if (!err)
-			err = t4_wol_pat_enable(pi->adapter, pi->tx_chan, 1,
-						~6ULL, ~0ULL, BCAST_CRC, true);
-	} else
-		t4_wol_pat_enable(pi->adapter, pi->tx_chan, 0, 0, 0, 0, false);
-	return err;
-}
-
 static int cxgb_set_features(struct net_device *dev, netdev_features_t features)
 {
 	const struct port_info *pi = netdev_priv(dev);
@@ -2837,144 +1410,6 @@ static int cxgb_set_features(struct net_device *dev, netdev_features_t features)
 	return err;
 }
 
-static u32 get_rss_table_size(struct net_device *dev)
-{
-	const struct port_info *pi = netdev_priv(dev);
-
-	return pi->rss_size;
-}
-
-static int get_rss_table(struct net_device *dev, u32 *p, u8 *key, u8 *hfunc)
-{
-	const struct port_info *pi = netdev_priv(dev);
-	unsigned int n = pi->rss_size;
-
-	if (hfunc)
-		*hfunc = ETH_RSS_HASH_TOP;
-	if (!p)
-		return 0;
-	while (n--)
-		p[n] = pi->rss[n];
-	return 0;
-}
-
-static int set_rss_table(struct net_device *dev, const u32 *p, const u8 *key,
-			 const u8 hfunc)
-{
-	unsigned int i;
-	struct port_info *pi = netdev_priv(dev);
-
-	/* We require at least one supported parameter to be changed and no
-	 * change in any of the unsupported parameters
-	 */
-	if (key ||
-	    (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP))
-		return -EOPNOTSUPP;
-	if (!p)
-		return 0;
-
-	for (i = 0; i < pi->rss_size; i++)
-		pi->rss[i] = p[i];
-	if (pi->adapter->flags & FULL_INIT_DONE)
-		return write_rss(pi, pi->rss);
-	return 0;
-}
-
-static int get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
-		     u32 *rules)
-{
-	const struct port_info *pi = netdev_priv(dev);
-
-	switch (info->cmd) {
-	case ETHTOOL_GRXFH: {
-		unsigned int v = pi->rss_mode;
-
-		info->data = 0;
-		switch (info->flow_type) {
-		case TCP_V4_FLOW:
-			if (v & FW_RSS_VI_CONFIG_CMD_IP4FOURTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST |
-					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
-			else if (v & FW_RSS_VI_CONFIG_CMD_IP4TWOTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST;
-			break;
-		case UDP_V4_FLOW:
-			if ((v & FW_RSS_VI_CONFIG_CMD_IP4FOURTUPEN_F) &&
-			    (v & FW_RSS_VI_CONFIG_CMD_UDPEN_F))
-				info->data = RXH_IP_SRC | RXH_IP_DST |
-					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
-			else if (v & FW_RSS_VI_CONFIG_CMD_IP4TWOTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST;
-			break;
-		case SCTP_V4_FLOW:
-		case AH_ESP_V4_FLOW:
-		case IPV4_FLOW:
-			if (v & FW_RSS_VI_CONFIG_CMD_IP4TWOTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST;
-			break;
-		case TCP_V6_FLOW:
-			if (v & FW_RSS_VI_CONFIG_CMD_IP6FOURTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST |
-					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
-			else if (v & FW_RSS_VI_CONFIG_CMD_IP6TWOTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST;
-			break;
-		case UDP_V6_FLOW:
-			if ((v & FW_RSS_VI_CONFIG_CMD_IP6FOURTUPEN_F) &&
-			    (v & FW_RSS_VI_CONFIG_CMD_UDPEN_F))
-				info->data = RXH_IP_SRC | RXH_IP_DST |
-					     RXH_L4_B_0_1 | RXH_L4_B_2_3;
-			else if (v & FW_RSS_VI_CONFIG_CMD_IP6TWOTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST;
-			break;
-		case SCTP_V6_FLOW:
-		case AH_ESP_V6_FLOW:
-		case IPV6_FLOW:
-			if (v & FW_RSS_VI_CONFIG_CMD_IP6TWOTUPEN_F)
-				info->data = RXH_IP_SRC | RXH_IP_DST;
-			break;
-		}
-		return 0;
-	}
-	case ETHTOOL_GRXRINGS:
-		info->data = pi->nqsets;
-		return 0;
-	}
-	return -EOPNOTSUPP;
-}
-
-static const struct ethtool_ops cxgb_ethtool_ops = {
-	.get_settings      = get_settings,
-	.set_settings      = set_settings,
-	.get_drvinfo       = get_drvinfo,
-	.get_msglevel      = get_msglevel,
-	.set_msglevel      = set_msglevel,
-	.get_ringparam     = get_sge_param,
-	.set_ringparam     = set_sge_param,
-	.get_coalesce      = get_coalesce,
-	.set_coalesce      = set_coalesce,
-	.get_eeprom_len    = get_eeprom_len,
-	.get_eeprom        = get_eeprom,
-	.set_eeprom        = set_eeprom,
-	.get_pauseparam    = get_pauseparam,
-	.set_pauseparam    = set_pauseparam,
-	.get_link          = ethtool_op_get_link,
-	.get_strings       = get_strings,
-	.set_phys_id       = identify_port,
-	.nway_reset        = restart_autoneg,
-	.get_sset_count    = get_sset_count,
-	.get_ethtool_stats = get_stats,
-	.get_regs_len      = get_regs_len,
-	.get_regs          = get_regs,
-	.get_wol           = get_wol,
-	.set_wol           = set_wol,
-	.get_rxnfc         = get_rxnfc,
-	.get_rxfh_indir_size = get_rss_table_size,
-	.get_rxfh	   = get_rss_table,
-	.set_rxfh	   = set_rss_table,
-	.flash_device      = set_flash,
-};
-
 static int setup_debugfs(struct adapter *adap)
 {
 	if (IS_ERR_OR_NULL(adap->debugfs_root))
@@ -4244,19 +2679,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;
@@ -4580,6 +3008,10 @@ static const struct net_device_ops cxgb4_netdev_ops = {
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller  = cxgb_netpoll,
 #endif
+#ifdef CONFIG_CHELSIO_T4_FCOE
+	.ndo_fcoe_enable      = cxgb_fcoe_enable,
+	.ndo_fcoe_disable     = cxgb_fcoe_disable,
+#endif /* CONFIG_CHELSIO_T4_FCOE */
 #ifdef CONFIG_NET_RX_BUSY_POLL
 	.ndo_busy_poll        = cxgb_busy_poll,
 #endif
@@ -4733,8 +3165,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 +3521,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 +3607,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 +3707,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);
@@ -5368,7 +3827,7 @@ static int adap_init0(struct adapter *adap)
 		adap->tids.stid_base = val[1];
 		adap->tids.nstids = val[2] - val[1] + 1;
 		/*
-		 * Setup server filter region. Divide the availble filter
+		 * Setup server filter region. Divide the available filter
 		 * region into two parts. Regular filters get 1/3rd and server
 		 * filters get 2/3rd part. This is only enabled if workarond
 		 * path is enabled.
@@ -5501,6 +3960,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 +3991,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();
@@ -5630,7 +4094,7 @@ static inline void init_rspq(struct adapter *adap, struct sge_rspq *q,
 			     unsigned int size, unsigned int iqe_size)
 {
 	q->adap = adap;
-	set_rspq_intr_params(q, us, cnt);
+	cxgb4_set_rspq_intr_params(q, us, cnt);
 	q->iqe_len = iqe_size;
 	q->size = size;
 }
@@ -5705,7 +4169,16 @@ static void cfg_queues(struct adapter *adap)
 			s->ofldqsets = adap->params.nports;
 		/* For RDMA one Rx queue per channel suffices */
 		s->rdmaqs = adap->params.nports;
-		s->rdmaciqs = adap->params.nports;
+		/* Try and allow at least 1 CIQ per cpu rounding down
+		 * to the number of ports, with a minimum of 1 per port.
+		 * A 2 port card in a 6 cpu system: 6 CIQs, 3 / port.
+		 * A 4 port card in a 6 cpu system: 4 CIQs, 1 / port.
+		 * A 4 port card in a 2 cpu system: 4 CIQs, 1 / port.
+		 */
+		s->rdmaciqs = min_t(int, MAX_RDMA_CIQS, num_online_cpus());
+		s->rdmaciqs = (s->rdmaciqs / adap->params.nports) *
+				adap->params.nports;
+		s->rdmaciqs = max_t(int, s->rdmaciqs, adap->params.nports);
 	}
 
 	for (i = 0; i < ARRAY_SIZE(s->ethrxq); i++) {
@@ -5791,12 +4264,17 @@ static void reduce_ethqs(struct adapter *adap, int n)
 static int enable_msix(struct adapter *adap)
 {
 	int ofld_need = 0;
-	int i, want, need;
+	int i, want, need, allocated;
 	struct sge *s = &adap->sge;
 	unsigned int nchan = adap->params.nports;
-	struct msix_entry entries[MAX_INGQ + 1];
+	struct msix_entry *entries;
+
+	entries = kmalloc(sizeof(*entries) * (MAX_INGQ + 1),
+			  GFP_KERNEL);
+	if (!entries)
+		return -ENOMEM;
 
-	for (i = 0; i < ARRAY_SIZE(entries); ++i)
+	for (i = 0; i < MAX_INGQ + 1; ++i)
 		entries[i].entry = i;
 
 	want = s->max_ethqsets + EXTRA_VECS;
@@ -5813,29 +4291,39 @@ static int enable_msix(struct adapter *adap)
 #else
 	need = adap->params.nports + EXTRA_VECS + ofld_need;
 #endif
-	want = pci_enable_msix_range(adap->pdev, entries, need, want);
-	if (want < 0)
-		return want;
+	allocated = pci_enable_msix_range(adap->pdev, entries, need, want);
+	if (allocated < 0) {
+		dev_info(adap->pdev_dev, "not enough MSI-X vectors left,"
+			 " not using MSI-X\n");
+		kfree(entries);
+		return allocated;
+	}
 
-	/*
-	 * Distribute available vectors to the various queue groups.
+	/* Distribute available vectors to the various queue groups.
 	 * Every group gets its minimum requirement and NIC gets top
 	 * priority for leftovers.
 	 */
-	i = want - EXTRA_VECS - ofld_need;
+	i = allocated - EXTRA_VECS - ofld_need;
 	if (i < s->max_ethqsets) {
 		s->max_ethqsets = i;
 		if (i < s->ethqsets)
 			reduce_ethqs(adap, i);
 	}
 	if (is_offload(adap)) {
-		i = want - EXTRA_VECS - s->max_ethqsets;
-		i -= ofld_need - nchan;
+		if (allocated < want) {
+			s->rdmaqs = nchan;
+			s->rdmaciqs = nchan;
+		}
+
+		/* leftovers go to OFLD */
+		i = allocated - EXTRA_VECS - s->max_ethqsets -
+		    s->rdmaqs - s->rdmaciqs;
 		s->ofldqsets = (i / nchan) * nchan;  /* round down */
 	}
-	for (i = 0; i < want; ++i)
+	for (i = 0; i < allocated; ++i)
 		adap->msix_info[i].vec = entries[i].vector;
 
+	kfree(entries);
 	return 0;
 }
 
@@ -5912,6 +4400,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)
@@ -6097,7 +4589,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		netdev->dcbnl_ops = &cxgb4_dcb_ops;
 		cxgb4_dcb_state_init(netdev);
 #endif
-		netdev->ethtool_ops = &cxgb_ethtool_ops;
+		cxgb4_set_ethtool_ops(netdev);
 	}
 
 	pci_set_drvdata(pdev, adapter);
@@ -6237,6 +4729,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..0d2eddab04ef 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -46,6 +46,9 @@
 #ifdef CONFIG_NET_RX_BUSY_POLL
 #include <net/busy_poll.h>
 #endif /* CONFIG_NET_RX_BUSY_POLL */
+#ifdef CONFIG_CHELSIO_T4_FCOE
+#include <scsi/fc/fc_fcoe.h>
+#endif /* CONFIG_CHELSIO_T4_FCOE */
 #include "cxgb4.h"
 #include "t4_regs.h"
 #include "t4_values.h"
@@ -118,12 +121,6 @@
 #define NOMEM_TMR_IDX (SGE_NTIMERS - 1)
 
 /*
- * An FL with <= FL_STARVE_THRES buffers is starving and a periodic timer will
- * attempt to refill it.
- */
-#define FL_STARVE_THRES 4
-
-/*
  * Suspend an Ethernet Tx queue with fewer available descriptors than this.
  * This is the same as calc_tx_descs() for a TSO packet with
  * nr_frags == MAX_SKB_FRAGS.
@@ -141,7 +138,7 @@
  * Max Tx descriptor space we allow for an Ethernet packet to be inlined
  * into a WR.
  */
-#define MAX_IMM_TX_PKT_LEN 128
+#define MAX_IMM_TX_PKT_LEN 256
 
 /*
  * Max size of a WR sent through a control Tx queue.
@@ -245,9 +242,21 @@ static inline unsigned int fl_cap(const struct sge_fl *fl)
 	return fl->size - 8;   /* 1 descriptor = 8 buffers */
 }
 
-static inline bool fl_starving(const struct sge_fl *fl)
+/**
+ *	fl_starving - return whether a Free List is starving.
+ *	@adapter: pointer to the adapter
+ *	@fl: the Free List
+ *
+ *	Tests specified Free List to see whether the number of buffers
+ *	available to the hardware has falled below our "starvation"
+ *	threshold.
+ */
+static inline bool fl_starving(const struct adapter *adapter,
+			       const struct sge_fl *fl)
 {
-	return fl->avail - fl->pend_cred <= FL_STARVE_THRES;
+	const struct sge *s = &adapter->sge;
+
+	return fl->avail - fl->pend_cred <= s->fl_starve_thres;
 }
 
 static int map_skb(struct device *dev, const struct sk_buff *skb,
@@ -583,8 +592,10 @@ static unsigned int refill_fl(struct adapter *adap, struct sge_fl *q, int n,
 	unsigned int cred = q->avail;
 	__be64 *d = &q->desc[q->pidx];
 	struct rx_sw_desc *sd = &q->sdesc[q->pidx];
+	int node;
 
 	gfp |= __GFP_NOWARN;
+	node = dev_to_node(adap->pdev_dev);
 
 	if (s->fl_pg_order == 0)
 		goto alloc_small_pages;
@@ -593,7 +604,7 @@ static unsigned int refill_fl(struct adapter *adap, struct sge_fl *q, int n,
 	 * Prefer large buffers
 	 */
 	while (n) {
-		pg = __dev_alloc_pages(gfp, s->fl_pg_order);
+		pg = alloc_pages_node(node, gfp | __GFP_COMP, s->fl_pg_order);
 		if (unlikely(!pg)) {
 			q->large_alloc_failed++;
 			break;       /* fall back to single pages */
@@ -623,7 +634,7 @@ static unsigned int refill_fl(struct adapter *adap, struct sge_fl *q, int n,
 
 alloc_small_pages:
 	while (n--) {
-		pg = __dev_alloc_page(gfp);
+		pg = alloc_pages_node(node, gfp, 0);
 		if (unlikely(!pg)) {
 			q->alloc_failed++;
 			break;
@@ -652,7 +663,7 @@ out:	cred = q->avail - cred;
 	q->pend_cred += cred;
 	ring_fl_db(adap, q);
 
-	if (unlikely(fl_starving(q))) {
+	if (unlikely(fl_starving(adap, q))) {
 		smp_wmb();
 		set_bit(q->cntxt_id - adap->sge.egr_start,
 			adap->sge.starving_fl);
@@ -719,6 +730,22 @@ static void *alloc_ring(struct device *dev, size_t nelem, size_t elem_size,
  */
 static inline unsigned int sgl_len(unsigned int n)
 {
+	/* A Direct Scatter Gather List uses 32-bit lengths and 64-bit PCI DMA
+	 * addresses.  The DSGL Work Request starts off with a 32-bit DSGL
+	 * ULPTX header, then Length0, then Address0, then, for 1 <= i <= N,
+	 * repeated sequences of { Length[i], Length[i+1], Address[i],
+	 * Address[i+1] } (this ensures that all addresses are on 64-bit
+	 * boundaries).  If N is even, then Length[N+1] should be set to 0 and
+	 * Address[N+1] is omitted.
+	 *
+	 * The following calculation incorporates all of the above.  It's
+	 * somewhat hard to follow but, briefly: the "+2" accounts for the
+	 * first two flits which include the DSGL header, Length0 and
+	 * Address0; the "(3*(n-1))/2" covers the main body of list entries (3
+	 * flits for every pair of the remaining N) +1 if (n-1) is odd; and
+	 * finally the "+((n-1)&1)" adds the one remaining flit needed if
+	 * (n-1) is odd ...
+	 */
 	n--;
 	return (3 * n) / 2 + (n & 1) + 2;
 }
@@ -766,12 +793,30 @@ static inline unsigned int calc_tx_flits(const struct sk_buff *skb)
 	unsigned int flits;
 	int hdrlen = is_eth_imm(skb);
 
+	/* If the skb is small enough, we can pump it out as a work request
+	 * with only immediate data.  In that case we just have to have the
+	 * TX Packet header plus the skb data in the Work Request.
+	 */
+
 	if (hdrlen)
 		return DIV_ROUND_UP(skb->len + hdrlen, sizeof(__be64));
 
+	/* Otherwise, we're going to have to construct a Scatter gather list
+	 * of the skb body and fragments.  We also include the flits necessary
+	 * for the TX Packet Work Request and CPL.  We always have a firmware
+	 * Write Header (incorporated as part of the cpl_tx_pkt_lso and
+	 * cpl_tx_pkt structures), followed by either a TX Packet Write CPL
+	 * message or, if we're doing a Large Send Offload, an LSO CPL message
+	 * with an embedded TX Packet Write CPL message.
+	 */
 	flits = sgl_len(skb_shinfo(skb)->nr_frags + 1) + 4;
 	if (skb_shinfo(skb)->gso_size)
-		flits += 2;
+		flits += (sizeof(struct fw_eth_tx_pkt_wr) +
+			  sizeof(struct cpl_tx_pkt_lso_core) +
+			  sizeof(struct cpl_tx_pkt_core)) / sizeof(__be64);
+	else
+		flits += (sizeof(struct fw_eth_tx_pkt_wr) +
+			  sizeof(struct cpl_tx_pkt_core)) / sizeof(__be64);
 	return flits;
 }
 
@@ -1044,6 +1089,38 @@ static inline void txq_advance(struct sge_txq *q, unsigned int n)
 		q->pidx -= q->size;
 }
 
+#ifdef CONFIG_CHELSIO_T4_FCOE
+static inline int
+cxgb_fcoe_offload(struct sk_buff *skb, struct adapter *adap,
+		  const struct port_info *pi, u64 *cntrl)
+{
+	const struct cxgb_fcoe *fcoe = &pi->fcoe;
+
+	if (!(fcoe->flags & CXGB_FCOE_ENABLED))
+		return 0;
+
+	if (skb->protocol != htons(ETH_P_FCOE))
+		return 0;
+
+	skb_reset_mac_header(skb);
+	skb->mac_len = sizeof(struct ethhdr);
+
+	skb_set_network_header(skb, skb->mac_len);
+	skb_set_transport_header(skb, skb->mac_len + sizeof(struct fcoe_hdr));
+
+	if (!cxgb_fcoe_sof_eof_supported(adap, skb))
+		return -ENOTSUPP;
+
+	/* FC CRC offload */
+	*cntrl = TXPKT_CSUM_TYPE(TX_CSUM_FCOE) |
+		     TXPKT_L4CSUM_DIS | TXPKT_IPCSUM_DIS |
+		     TXPKT_CSUM_START(CXGB_FCOE_TXPKT_CSUM_START) |
+		     TXPKT_CSUM_END(CXGB_FCOE_TXPKT_CSUM_END) |
+		     TXPKT_CSUM_LOC(CXGB_FCOE_TXPKT_CSUM_END);
+	return 0;
+}
+#endif /* CONFIG_CHELSIO_T4_FCOE */
+
 /**
  *	t4_eth_xmit - add a packet to an Ethernet Tx queue
  *	@skb: the packet
@@ -1066,6 +1143,9 @@ netdev_tx_t t4_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 	const struct skb_shared_info *ssi;
 	dma_addr_t addr[MAX_SKB_FRAGS + 1];
 	bool immediate = false;
+#ifdef CONFIG_CHELSIO_T4_FCOE
+	int err;
+#endif /* CONFIG_CHELSIO_T4_FCOE */
 
 	/*
 	 * The chip min packet length is 10 octets but play safe and reject
@@ -1082,6 +1162,13 @@ out_free:	dev_kfree_skb_any(skb);
 	q = &adap->sge.ethtxq[qidx + pi->first_qset];
 
 	reclaim_completed_tx(adap, &q->q, true);
+	cntrl = TXPKT_L4CSUM_DIS | TXPKT_IPCSUM_DIS;
+
+#ifdef CONFIG_CHELSIO_T4_FCOE
+	err = cxgb_fcoe_offload(skb, adap, pi, &cntrl);
+	if (unlikely(err == -ENOTSUPP))
+		goto out_free;
+#endif /* CONFIG_CHELSIO_T4_FCOE */
 
 	flits = calc_tx_flits(skb);
 	ndesc = flits_to_desc(flits);
@@ -1153,13 +1240,17 @@ out_free:	dev_kfree_skb_any(skb);
 		if (skb->ip_summed == CHECKSUM_PARTIAL) {
 			cntrl = hwcsum(skb) | TXPKT_IPCSUM_DIS;
 			q->tx_cso++;
-		} else
-			cntrl = TXPKT_L4CSUM_DIS | TXPKT_IPCSUM_DIS;
+		}
 	}
 
 	if (skb_vlan_tag_present(skb)) {
 		q->vlan_ins++;
 		cntrl |= TXPKT_VLAN_VLD | TXPKT_VLAN(skb_vlan_tag_get(skb));
+#ifdef CONFIG_CHELSIO_T4_FCOE
+		if (skb->protocol == htons(ETH_P_FCOE))
+			cntrl |= TXPKT_VLAN(
+				 ((skb->priority & 0x7) << VLAN_PRIO_SHIFT));
+#endif /* CONFIG_CHELSIO_T4_FCOE */
 	}
 
 	cpl->ctrl0 = htonl(TXPKT_OPCODE(CPL_TX_PKT_XT) |
@@ -1759,6 +1850,9 @@ int t4_ethrx_handler(struct sge_rspq *q, const __be64 *rsp,
 	struct sge *s = &q->adap->sge;
 	int cpl_trace_pkt = is_t4(q->adap->params.chip) ?
 			    CPL_TRACE_PKT : CPL_TRACE_PKT_T5;
+#ifdef CONFIG_CHELSIO_T4_FCOE
+	struct port_info *pi;
+#endif
 
 	if (unlikely(*(u8 *)rsp == cpl_trace_pkt))
 		return handle_trace_pkt(q->adap, si);
@@ -1799,8 +1893,24 @@ int t4_ethrx_handler(struct sge_rspq *q, const __be64 *rsp,
 			skb->ip_summed = CHECKSUM_COMPLETE;
 			rxq->stats.rx_cso++;
 		}
-	} else
+	} else {
 		skb_checksum_none_assert(skb);
+#ifdef CONFIG_CHELSIO_T4_FCOE
+#define CPL_RX_PKT_FLAGS (RXF_PSH_F | RXF_SYN_F | RXF_UDP_F | \
+			  RXF_TCP_F | RXF_IP_F | RXF_IP6_F | RXF_LRO_F)
+
+		pi = netdev_priv(skb->dev);
+		if (!(pkt->l2info & cpu_to_be32(CPL_RX_PKT_FLAGS))) {
+			if ((pkt->l2info & cpu_to_be32(RXF_FCOE_F)) &&
+			    (pi->fcoe.flags & CXGB_FCOE_ENABLED)) {
+				if (!(pkt->err_vec & cpu_to_be16(RXERR_CSUM_F)))
+					skb->ip_summed = CHECKSUM_UNNECESSARY;
+			}
+		}
+
+#undef CPL_RX_PKT_FLAGS
+#endif /* CONFIG_CHELSIO_T4_FCOE */
+	}
 
 	if (unlikely(pkt->vlan_ex)) {
 		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(pkt->vlan));
@@ -1900,7 +2010,7 @@ static int process_responses(struct sge_rspq *q, int budget)
 		if (!is_new_response(rc, q))
 			break;
 
-		rmb();
+		dma_rmb();
 		rsp_type = RSPD_TYPE(rc->type_gen);
 		if (likely(rsp_type == RSP_TYPE_FLBUF)) {
 			struct page_frag *fp;
@@ -2092,7 +2202,7 @@ static unsigned int process_intrq(struct adapter *adap)
 		if (!is_new_response(rc, q))
 			break;
 
-		rmb();
+		dma_rmb();
 		if (RSPD_TYPE(rc->type_gen) == RSP_TYPE_INTR) {
 			unsigned int qid = ntohl(rc->pldbuflen_qid);
 
@@ -2128,7 +2238,8 @@ static irqreturn_t t4_intr_msi(int irq, void *cookie)
 {
 	struct adapter *adap = cookie;
 
-	t4_slow_intr_handler(adap);
+	if (adap->flags & MASTER_PF)
+		t4_slow_intr_handler(adap);
 	process_intrq(adap);
 	return IRQ_HANDLED;
 }
@@ -2143,7 +2254,8 @@ static irqreturn_t t4_intr_intx(int irq, void *cookie)
 	struct adapter *adap = cookie;
 
 	t4_write_reg(adap, MYPF_REG(PCIE_PF_CLI_A), 0);
-	if (t4_slow_intr_handler(adap) | process_intrq(adap))
+	if (((adap->flags & MASTER_PF) && t4_slow_intr_handler(adap)) |
+	    process_intrq(adap))
 		return IRQ_HANDLED;
 	return IRQ_NONE;             /* probably shared interrupt */
 }
@@ -2171,7 +2283,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;
@@ -2180,7 +2292,7 @@ static void sge_rx_timer_cb(unsigned long data)
 			clear_bit(id, s->starving_fl);
 			smp_mb__after_atomic();
 
-			if (fl_starving(fl)) {
+			if (fl_starving(adap, fl)) {
 				rxq = container_of(fl, struct sge_eth_rxq, fl);
 				if (napi_reschedule(&rxq->rspq.napi))
 					fl->starving++;
@@ -2259,7 +2371,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 +2853,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 853c38997c82..5959e3ae72da 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -625,6 +625,734 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 	return 0;
 }
 
+/**
+ *	t4_get_regs_len - return the size of the chips register set
+ *	@adapter: the adapter
+ *
+ *	Returns the size of the chip's BAR0 register space.
+ */
+unsigned int t4_get_regs_len(struct adapter *adapter)
+{
+	unsigned int chip_version = CHELSIO_CHIP_VERSION(adapter->params.chip);
+
+	switch (chip_version) {
+	case CHELSIO_T4:
+		return T4_REGMAP_SIZE;
+
+	case CHELSIO_T5:
+		return T5_REGMAP_SIZE;
+	}
+
+	dev_err(adapter->pdev_dev,
+		"Unsupported chip version %d\n", chip_version);
+	return 0;
+}
+
+/**
+ *	t4_get_regs - read chip registers into provided buffer
+ *	@adap: the adapter
+ *	@buf: register buffer
+ *	@buf_size: size (in bytes) of register buffer
+ *
+ *	If the provided register buffer isn't large enough for the chip's
+ *	full register range, the register dump will be truncated to the
+ *	register buffer's size.
+ */
+void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
+{
+	static const unsigned int t4_reg_ranges[] = {
+		0x1008, 0x1108,
+		0x1180, 0x11b4,
+		0x11fc, 0x123c,
+		0x1300, 0x173c,
+		0x1800, 0x18fc,
+		0x3000, 0x30d8,
+		0x30e0, 0x5924,
+		0x5960, 0x59d4,
+		0x5a00, 0x5af8,
+		0x6000, 0x6098,
+		0x6100, 0x6150,
+		0x6200, 0x6208,
+		0x6240, 0x6248,
+		0x6280, 0x6338,
+		0x6370, 0x638c,
+		0x6400, 0x643c,
+		0x6500, 0x6524,
+		0x6a00, 0x6a38,
+		0x6a60, 0x6a78,
+		0x6b00, 0x6b84,
+		0x6bf0, 0x6c84,
+		0x6cf0, 0x6d84,
+		0x6df0, 0x6e84,
+		0x6ef0, 0x6f84,
+		0x6ff0, 0x7084,
+		0x70f0, 0x7184,
+		0x71f0, 0x7284,
+		0x72f0, 0x7384,
+		0x73f0, 0x7450,
+		0x7500, 0x7530,
+		0x7600, 0x761c,
+		0x7680, 0x76cc,
+		0x7700, 0x7798,
+		0x77c0, 0x77fc,
+		0x7900, 0x79fc,
+		0x7b00, 0x7c38,
+		0x7d00, 0x7efc,
+		0x8dc0, 0x8e1c,
+		0x8e30, 0x8e78,
+		0x8ea0, 0x8f6c,
+		0x8fc0, 0x9074,
+		0x90fc, 0x90fc,
+		0x9400, 0x9458,
+		0x9600, 0x96bc,
+		0x9800, 0x9808,
+		0x9820, 0x983c,
+		0x9850, 0x9864,
+		0x9c00, 0x9c6c,
+		0x9c80, 0x9cec,
+		0x9d00, 0x9d6c,
+		0x9d80, 0x9dec,
+		0x9e00, 0x9e6c,
+		0x9e80, 0x9eec,
+		0x9f00, 0x9f6c,
+		0x9f80, 0x9fec,
+		0xd004, 0xd03c,
+		0xdfc0, 0xdfe0,
+		0xe000, 0xea7c,
+		0xf000, 0x11110,
+		0x11118, 0x11190,
+		0x19040, 0x1906c,
+		0x19078, 0x19080,
+		0x1908c, 0x19124,
+		0x19150, 0x191b0,
+		0x191d0, 0x191e8,
+		0x19238, 0x1924c,
+		0x193f8, 0x19474,
+		0x19490, 0x194f8,
+		0x19800, 0x19f30,
+		0x1a000, 0x1a06c,
+		0x1a0b0, 0x1a120,
+		0x1a128, 0x1a138,
+		0x1a190, 0x1a1c4,
+		0x1a1fc, 0x1a1fc,
+		0x1e040, 0x1e04c,
+		0x1e284, 0x1e28c,
+		0x1e2c0, 0x1e2c0,
+		0x1e2e0, 0x1e2e0,
+		0x1e300, 0x1e384,
+		0x1e3c0, 0x1e3c8,
+		0x1e440, 0x1e44c,
+		0x1e684, 0x1e68c,
+		0x1e6c0, 0x1e6c0,
+		0x1e6e0, 0x1e6e0,
+		0x1e700, 0x1e784,
+		0x1e7c0, 0x1e7c8,
+		0x1e840, 0x1e84c,
+		0x1ea84, 0x1ea8c,
+		0x1eac0, 0x1eac0,
+		0x1eae0, 0x1eae0,
+		0x1eb00, 0x1eb84,
+		0x1ebc0, 0x1ebc8,
+		0x1ec40, 0x1ec4c,
+		0x1ee84, 0x1ee8c,
+		0x1eec0, 0x1eec0,
+		0x1eee0, 0x1eee0,
+		0x1ef00, 0x1ef84,
+		0x1efc0, 0x1efc8,
+		0x1f040, 0x1f04c,
+		0x1f284, 0x1f28c,
+		0x1f2c0, 0x1f2c0,
+		0x1f2e0, 0x1f2e0,
+		0x1f300, 0x1f384,
+		0x1f3c0, 0x1f3c8,
+		0x1f440, 0x1f44c,
+		0x1f684, 0x1f68c,
+		0x1f6c0, 0x1f6c0,
+		0x1f6e0, 0x1f6e0,
+		0x1f700, 0x1f784,
+		0x1f7c0, 0x1f7c8,
+		0x1f840, 0x1f84c,
+		0x1fa84, 0x1fa8c,
+		0x1fac0, 0x1fac0,
+		0x1fae0, 0x1fae0,
+		0x1fb00, 0x1fb84,
+		0x1fbc0, 0x1fbc8,
+		0x1fc40, 0x1fc4c,
+		0x1fe84, 0x1fe8c,
+		0x1fec0, 0x1fec0,
+		0x1fee0, 0x1fee0,
+		0x1ff00, 0x1ff84,
+		0x1ffc0, 0x1ffc8,
+		0x20000, 0x2002c,
+		0x20100, 0x2013c,
+		0x20190, 0x201c8,
+		0x20200, 0x20318,
+		0x20400, 0x20528,
+		0x20540, 0x20614,
+		0x21000, 0x21040,
+		0x2104c, 0x21060,
+		0x210c0, 0x210ec,
+		0x21200, 0x21268,
+		0x21270, 0x21284,
+		0x212fc, 0x21388,
+		0x21400, 0x21404,
+		0x21500, 0x21518,
+		0x2152c, 0x2153c,
+		0x21550, 0x21554,
+		0x21600, 0x21600,
+		0x21608, 0x21628,
+		0x21630, 0x2163c,
+		0x21700, 0x2171c,
+		0x21780, 0x2178c,
+		0x21800, 0x21c38,
+		0x21c80, 0x21d7c,
+		0x21e00, 0x21e04,
+		0x22000, 0x2202c,
+		0x22100, 0x2213c,
+		0x22190, 0x221c8,
+		0x22200, 0x22318,
+		0x22400, 0x22528,
+		0x22540, 0x22614,
+		0x23000, 0x23040,
+		0x2304c, 0x23060,
+		0x230c0, 0x230ec,
+		0x23200, 0x23268,
+		0x23270, 0x23284,
+		0x232fc, 0x23388,
+		0x23400, 0x23404,
+		0x23500, 0x23518,
+		0x2352c, 0x2353c,
+		0x23550, 0x23554,
+		0x23600, 0x23600,
+		0x23608, 0x23628,
+		0x23630, 0x2363c,
+		0x23700, 0x2371c,
+		0x23780, 0x2378c,
+		0x23800, 0x23c38,
+		0x23c80, 0x23d7c,
+		0x23e00, 0x23e04,
+		0x24000, 0x2402c,
+		0x24100, 0x2413c,
+		0x24190, 0x241c8,
+		0x24200, 0x24318,
+		0x24400, 0x24528,
+		0x24540, 0x24614,
+		0x25000, 0x25040,
+		0x2504c, 0x25060,
+		0x250c0, 0x250ec,
+		0x25200, 0x25268,
+		0x25270, 0x25284,
+		0x252fc, 0x25388,
+		0x25400, 0x25404,
+		0x25500, 0x25518,
+		0x2552c, 0x2553c,
+		0x25550, 0x25554,
+		0x25600, 0x25600,
+		0x25608, 0x25628,
+		0x25630, 0x2563c,
+		0x25700, 0x2571c,
+		0x25780, 0x2578c,
+		0x25800, 0x25c38,
+		0x25c80, 0x25d7c,
+		0x25e00, 0x25e04,
+		0x26000, 0x2602c,
+		0x26100, 0x2613c,
+		0x26190, 0x261c8,
+		0x26200, 0x26318,
+		0x26400, 0x26528,
+		0x26540, 0x26614,
+		0x27000, 0x27040,
+		0x2704c, 0x27060,
+		0x270c0, 0x270ec,
+		0x27200, 0x27268,
+		0x27270, 0x27284,
+		0x272fc, 0x27388,
+		0x27400, 0x27404,
+		0x27500, 0x27518,
+		0x2752c, 0x2753c,
+		0x27550, 0x27554,
+		0x27600, 0x27600,
+		0x27608, 0x27628,
+		0x27630, 0x2763c,
+		0x27700, 0x2771c,
+		0x27780, 0x2778c,
+		0x27800, 0x27c38,
+		0x27c80, 0x27d7c,
+		0x27e00, 0x27e04
+	};
+
+	static const unsigned int t5_reg_ranges[] = {
+		0x1008, 0x1148,
+		0x1180, 0x11b4,
+		0x11fc, 0x123c,
+		0x1280, 0x173c,
+		0x1800, 0x18fc,
+		0x3000, 0x3028,
+		0x3060, 0x30d8,
+		0x30e0, 0x30fc,
+		0x3140, 0x357c,
+		0x35a8, 0x35cc,
+		0x35ec, 0x35ec,
+		0x3600, 0x5624,
+		0x56cc, 0x575c,
+		0x580c, 0x5814,
+		0x5890, 0x58bc,
+		0x5940, 0x59dc,
+		0x59fc, 0x5a18,
+		0x5a60, 0x5a9c,
+		0x5b9c, 0x5bfc,
+		0x6000, 0x6040,
+		0x6058, 0x614c,
+		0x7700, 0x7798,
+		0x77c0, 0x78fc,
+		0x7b00, 0x7c54,
+		0x7d00, 0x7efc,
+		0x8dc0, 0x8de0,
+		0x8df8, 0x8e84,
+		0x8ea0, 0x8f84,
+		0x8fc0, 0x90f8,
+		0x9400, 0x9470,
+		0x9600, 0x96f4,
+		0x9800, 0x9808,
+		0x9820, 0x983c,
+		0x9850, 0x9864,
+		0x9c00, 0x9c6c,
+		0x9c80, 0x9cec,
+		0x9d00, 0x9d6c,
+		0x9d80, 0x9dec,
+		0x9e00, 0x9e6c,
+		0x9e80, 0x9eec,
+		0x9f00, 0x9f6c,
+		0x9f80, 0xa020,
+		0xd004, 0xd03c,
+		0xdfc0, 0xdfe0,
+		0xe000, 0x11088,
+		0x1109c, 0x11110,
+		0x11118, 0x1117c,
+		0x11190, 0x11204,
+		0x19040, 0x1906c,
+		0x19078, 0x19080,
+		0x1908c, 0x19124,
+		0x19150, 0x191b0,
+		0x191d0, 0x191e8,
+		0x19238, 0x19290,
+		0x193f8, 0x19474,
+		0x19490, 0x194cc,
+		0x194f0, 0x194f8,
+		0x19c00, 0x19c60,
+		0x19c94, 0x19e10,
+		0x19e50, 0x19f34,
+		0x19f40, 0x19f50,
+		0x19f90, 0x19fe4,
+		0x1a000, 0x1a06c,
+		0x1a0b0, 0x1a120,
+		0x1a128, 0x1a138,
+		0x1a190, 0x1a1c4,
+		0x1a1fc, 0x1a1fc,
+		0x1e008, 0x1e00c,
+		0x1e040, 0x1e04c,
+		0x1e284, 0x1e290,
+		0x1e2c0, 0x1e2c0,
+		0x1e2e0, 0x1e2e0,
+		0x1e300, 0x1e384,
+		0x1e3c0, 0x1e3c8,
+		0x1e408, 0x1e40c,
+		0x1e440, 0x1e44c,
+		0x1e684, 0x1e690,
+		0x1e6c0, 0x1e6c0,
+		0x1e6e0, 0x1e6e0,
+		0x1e700, 0x1e784,
+		0x1e7c0, 0x1e7c8,
+		0x1e808, 0x1e80c,
+		0x1e840, 0x1e84c,
+		0x1ea84, 0x1ea90,
+		0x1eac0, 0x1eac0,
+		0x1eae0, 0x1eae0,
+		0x1eb00, 0x1eb84,
+		0x1ebc0, 0x1ebc8,
+		0x1ec08, 0x1ec0c,
+		0x1ec40, 0x1ec4c,
+		0x1ee84, 0x1ee90,
+		0x1eec0, 0x1eec0,
+		0x1eee0, 0x1eee0,
+		0x1ef00, 0x1ef84,
+		0x1efc0, 0x1efc8,
+		0x1f008, 0x1f00c,
+		0x1f040, 0x1f04c,
+		0x1f284, 0x1f290,
+		0x1f2c0, 0x1f2c0,
+		0x1f2e0, 0x1f2e0,
+		0x1f300, 0x1f384,
+		0x1f3c0, 0x1f3c8,
+		0x1f408, 0x1f40c,
+		0x1f440, 0x1f44c,
+		0x1f684, 0x1f690,
+		0x1f6c0, 0x1f6c0,
+		0x1f6e0, 0x1f6e0,
+		0x1f700, 0x1f784,
+		0x1f7c0, 0x1f7c8,
+		0x1f808, 0x1f80c,
+		0x1f840, 0x1f84c,
+		0x1fa84, 0x1fa90,
+		0x1fac0, 0x1fac0,
+		0x1fae0, 0x1fae0,
+		0x1fb00, 0x1fb84,
+		0x1fbc0, 0x1fbc8,
+		0x1fc08, 0x1fc0c,
+		0x1fc40, 0x1fc4c,
+		0x1fe84, 0x1fe90,
+		0x1fec0, 0x1fec0,
+		0x1fee0, 0x1fee0,
+		0x1ff00, 0x1ff84,
+		0x1ffc0, 0x1ffc8,
+		0x30000, 0x30030,
+		0x30100, 0x30144,
+		0x30190, 0x301d0,
+		0x30200, 0x30318,
+		0x30400, 0x3052c,
+		0x30540, 0x3061c,
+		0x30800, 0x30834,
+		0x308c0, 0x30908,
+		0x30910, 0x309ac,
+		0x30a00, 0x30a04,
+		0x30a0c, 0x30a2c,
+		0x30a44, 0x30a50,
+		0x30a74, 0x30c24,
+		0x30d08, 0x30d14,
+		0x30d1c, 0x30d20,
+		0x30d3c, 0x30d50,
+		0x31200, 0x3120c,
+		0x31220, 0x31220,
+		0x31240, 0x31240,
+		0x31600, 0x31600,
+		0x31608, 0x3160c,
+		0x31a00, 0x31a1c,
+		0x31e04, 0x31e20,
+		0x31e38, 0x31e3c,
+		0x31e80, 0x31e80,
+		0x31e88, 0x31ea8,
+		0x31eb0, 0x31eb4,
+		0x31ec8, 0x31ed4,
+		0x31fb8, 0x32004,
+		0x32208, 0x3223c,
+		0x32600, 0x32630,
+		0x32a00, 0x32abc,
+		0x32b00, 0x32b70,
+		0x33000, 0x33048,
+		0x33060, 0x3309c,
+		0x330f0, 0x33148,
+		0x33160, 0x3319c,
+		0x331f0, 0x332e4,
+		0x332f8, 0x333e4,
+		0x333f8, 0x33448,
+		0x33460, 0x3349c,
+		0x334f0, 0x33548,
+		0x33560, 0x3359c,
+		0x335f0, 0x336e4,
+		0x336f8, 0x337e4,
+		0x337f8, 0x337fc,
+		0x33814, 0x33814,
+		0x3382c, 0x3382c,
+		0x33880, 0x3388c,
+		0x338e8, 0x338ec,
+		0x33900, 0x33948,
+		0x33960, 0x3399c,
+		0x339f0, 0x33ae4,
+		0x33af8, 0x33b10,
+		0x33b28, 0x33b28,
+		0x33b3c, 0x33b50,
+		0x33bf0, 0x33c10,
+		0x33c28, 0x33c28,
+		0x33c3c, 0x33c50,
+		0x33cf0, 0x33cfc,
+		0x34000, 0x34030,
+		0x34100, 0x34144,
+		0x34190, 0x341d0,
+		0x34200, 0x34318,
+		0x34400, 0x3452c,
+		0x34540, 0x3461c,
+		0x34800, 0x34834,
+		0x348c0, 0x34908,
+		0x34910, 0x349ac,
+		0x34a00, 0x34a04,
+		0x34a0c, 0x34a2c,
+		0x34a44, 0x34a50,
+		0x34a74, 0x34c24,
+		0x34d08, 0x34d14,
+		0x34d1c, 0x34d20,
+		0x34d3c, 0x34d50,
+		0x35200, 0x3520c,
+		0x35220, 0x35220,
+		0x35240, 0x35240,
+		0x35600, 0x35600,
+		0x35608, 0x3560c,
+		0x35a00, 0x35a1c,
+		0x35e04, 0x35e20,
+		0x35e38, 0x35e3c,
+		0x35e80, 0x35e80,
+		0x35e88, 0x35ea8,
+		0x35eb0, 0x35eb4,
+		0x35ec8, 0x35ed4,
+		0x35fb8, 0x36004,
+		0x36208, 0x3623c,
+		0x36600, 0x36630,
+		0x36a00, 0x36abc,
+		0x36b00, 0x36b70,
+		0x37000, 0x37048,
+		0x37060, 0x3709c,
+		0x370f0, 0x37148,
+		0x37160, 0x3719c,
+		0x371f0, 0x372e4,
+		0x372f8, 0x373e4,
+		0x373f8, 0x37448,
+		0x37460, 0x3749c,
+		0x374f0, 0x37548,
+		0x37560, 0x3759c,
+		0x375f0, 0x376e4,
+		0x376f8, 0x377e4,
+		0x377f8, 0x377fc,
+		0x37814, 0x37814,
+		0x3782c, 0x3782c,
+		0x37880, 0x3788c,
+		0x378e8, 0x378ec,
+		0x37900, 0x37948,
+		0x37960, 0x3799c,
+		0x379f0, 0x37ae4,
+		0x37af8, 0x37b10,
+		0x37b28, 0x37b28,
+		0x37b3c, 0x37b50,
+		0x37bf0, 0x37c10,
+		0x37c28, 0x37c28,
+		0x37c3c, 0x37c50,
+		0x37cf0, 0x37cfc,
+		0x38000, 0x38030,
+		0x38100, 0x38144,
+		0x38190, 0x381d0,
+		0x38200, 0x38318,
+		0x38400, 0x3852c,
+		0x38540, 0x3861c,
+		0x38800, 0x38834,
+		0x388c0, 0x38908,
+		0x38910, 0x389ac,
+		0x38a00, 0x38a04,
+		0x38a0c, 0x38a2c,
+		0x38a44, 0x38a50,
+		0x38a74, 0x38c24,
+		0x38d08, 0x38d14,
+		0x38d1c, 0x38d20,
+		0x38d3c, 0x38d50,
+		0x39200, 0x3920c,
+		0x39220, 0x39220,
+		0x39240, 0x39240,
+		0x39600, 0x39600,
+		0x39608, 0x3960c,
+		0x39a00, 0x39a1c,
+		0x39e04, 0x39e20,
+		0x39e38, 0x39e3c,
+		0x39e80, 0x39e80,
+		0x39e88, 0x39ea8,
+		0x39eb0, 0x39eb4,
+		0x39ec8, 0x39ed4,
+		0x39fb8, 0x3a004,
+		0x3a208, 0x3a23c,
+		0x3a600, 0x3a630,
+		0x3aa00, 0x3aabc,
+		0x3ab00, 0x3ab70,
+		0x3b000, 0x3b048,
+		0x3b060, 0x3b09c,
+		0x3b0f0, 0x3b148,
+		0x3b160, 0x3b19c,
+		0x3b1f0, 0x3b2e4,
+		0x3b2f8, 0x3b3e4,
+		0x3b3f8, 0x3b448,
+		0x3b460, 0x3b49c,
+		0x3b4f0, 0x3b548,
+		0x3b560, 0x3b59c,
+		0x3b5f0, 0x3b6e4,
+		0x3b6f8, 0x3b7e4,
+		0x3b7f8, 0x3b7fc,
+		0x3b814, 0x3b814,
+		0x3b82c, 0x3b82c,
+		0x3b880, 0x3b88c,
+		0x3b8e8, 0x3b8ec,
+		0x3b900, 0x3b948,
+		0x3b960, 0x3b99c,
+		0x3b9f0, 0x3bae4,
+		0x3baf8, 0x3bb10,
+		0x3bb28, 0x3bb28,
+		0x3bb3c, 0x3bb50,
+		0x3bbf0, 0x3bc10,
+		0x3bc28, 0x3bc28,
+		0x3bc3c, 0x3bc50,
+		0x3bcf0, 0x3bcfc,
+		0x3c000, 0x3c030,
+		0x3c100, 0x3c144,
+		0x3c190, 0x3c1d0,
+		0x3c200, 0x3c318,
+		0x3c400, 0x3c52c,
+		0x3c540, 0x3c61c,
+		0x3c800, 0x3c834,
+		0x3c8c0, 0x3c908,
+		0x3c910, 0x3c9ac,
+		0x3ca00, 0x3ca04,
+		0x3ca0c, 0x3ca2c,
+		0x3ca44, 0x3ca50,
+		0x3ca74, 0x3cc24,
+		0x3cd08, 0x3cd14,
+		0x3cd1c, 0x3cd20,
+		0x3cd3c, 0x3cd50,
+		0x3d200, 0x3d20c,
+		0x3d220, 0x3d220,
+		0x3d240, 0x3d240,
+		0x3d600, 0x3d600,
+		0x3d608, 0x3d60c,
+		0x3da00, 0x3da1c,
+		0x3de04, 0x3de20,
+		0x3de38, 0x3de3c,
+		0x3de80, 0x3de80,
+		0x3de88, 0x3dea8,
+		0x3deb0, 0x3deb4,
+		0x3dec8, 0x3ded4,
+		0x3dfb8, 0x3e004,
+		0x3e208, 0x3e23c,
+		0x3e600, 0x3e630,
+		0x3ea00, 0x3eabc,
+		0x3eb00, 0x3eb70,
+		0x3f000, 0x3f048,
+		0x3f060, 0x3f09c,
+		0x3f0f0, 0x3f148,
+		0x3f160, 0x3f19c,
+		0x3f1f0, 0x3f2e4,
+		0x3f2f8, 0x3f3e4,
+		0x3f3f8, 0x3f448,
+		0x3f460, 0x3f49c,
+		0x3f4f0, 0x3f548,
+		0x3f560, 0x3f59c,
+		0x3f5f0, 0x3f6e4,
+		0x3f6f8, 0x3f7e4,
+		0x3f7f8, 0x3f7fc,
+		0x3f814, 0x3f814,
+		0x3f82c, 0x3f82c,
+		0x3f880, 0x3f88c,
+		0x3f8e8, 0x3f8ec,
+		0x3f900, 0x3f948,
+		0x3f960, 0x3f99c,
+		0x3f9f0, 0x3fae4,
+		0x3faf8, 0x3fb10,
+		0x3fb28, 0x3fb28,
+		0x3fb3c, 0x3fb50,
+		0x3fbf0, 0x3fc10,
+		0x3fc28, 0x3fc28,
+		0x3fc3c, 0x3fc50,
+		0x3fcf0, 0x3fcfc,
+		0x40000, 0x4000c,
+		0x40040, 0x40068,
+		0x40080, 0x40144,
+		0x40180, 0x4018c,
+		0x40200, 0x40298,
+		0x402ac, 0x4033c,
+		0x403f8, 0x403fc,
+		0x41304, 0x413c4,
+		0x41400, 0x4141c,
+		0x41480, 0x414d0,
+		0x44000, 0x44078,
+		0x440c0, 0x44278,
+		0x442c0, 0x44478,
+		0x444c0, 0x44678,
+		0x446c0, 0x44878,
+		0x448c0, 0x449fc,
+		0x45000, 0x45068,
+		0x45080, 0x45084,
+		0x450a0, 0x450b0,
+		0x45200, 0x45268,
+		0x45280, 0x45284,
+		0x452a0, 0x452b0,
+		0x460c0, 0x460e4,
+		0x47000, 0x4708c,
+		0x47200, 0x47250,
+		0x47400, 0x47420,
+		0x47600, 0x47618,
+		0x47800, 0x47814,
+		0x48000, 0x4800c,
+		0x48040, 0x48068,
+		0x48080, 0x48144,
+		0x48180, 0x4818c,
+		0x48200, 0x48298,
+		0x482ac, 0x4833c,
+		0x483f8, 0x483fc,
+		0x49304, 0x493c4,
+		0x49400, 0x4941c,
+		0x49480, 0x494d0,
+		0x4c000, 0x4c078,
+		0x4c0c0, 0x4c278,
+		0x4c2c0, 0x4c478,
+		0x4c4c0, 0x4c678,
+		0x4c6c0, 0x4c878,
+		0x4c8c0, 0x4c9fc,
+		0x4d000, 0x4d068,
+		0x4d080, 0x4d084,
+		0x4d0a0, 0x4d0b0,
+		0x4d200, 0x4d268,
+		0x4d280, 0x4d284,
+		0x4d2a0, 0x4d2b0,
+		0x4e0c0, 0x4e0e4,
+		0x4f000, 0x4f08c,
+		0x4f200, 0x4f250,
+		0x4f400, 0x4f420,
+		0x4f600, 0x4f618,
+		0x4f800, 0x4f814,
+		0x50000, 0x500cc,
+		0x50400, 0x50400,
+		0x50800, 0x508cc,
+		0x50c00, 0x50c00,
+		0x51000, 0x5101c,
+		0x51300, 0x51308,
+	};
+
+	u32 *buf_end = (u32 *)((char *)buf + buf_size);
+	const unsigned int *reg_ranges;
+	int reg_ranges_size, range;
+	unsigned int chip_version = CHELSIO_CHIP_VERSION(adap->params.chip);
+
+	/* Select the right set of register ranges to dump depending on the
+	 * adapter chip type.
+	 */
+	switch (chip_version) {
+	case CHELSIO_T4:
+		reg_ranges = t4_reg_ranges;
+		reg_ranges_size = ARRAY_SIZE(t4_reg_ranges);
+		break;
+
+	case CHELSIO_T5:
+		reg_ranges = t5_reg_ranges;
+		reg_ranges_size = ARRAY_SIZE(t5_reg_ranges);
+		break;
+
+	default:
+		dev_err(adap->pdev_dev,
+			"Unsupported chip version %d\n", chip_version);
+		return;
+	}
+
+	/* Clear the register buffer and insert the appropriate register
+	 * values selected by the above register ranges.
+	 */
+	memset(buf, 0, buf_size);
+	for (range = 0; range < reg_ranges_size; range += 2) {
+		unsigned int reg = reg_ranges[range];
+		unsigned int last_reg = reg_ranges[range + 1];
+		u32 *bufp = (u32 *)((char *)buf + reg);
+
+		/* Iterate across the register range filling in the register
+		 * buffer but don't write past the end of the register buffer.
+		 */
+		while (reg <= last_reg && bufp < buf_end) {
+			*bufp++ = t4_read_reg(adap, reg);
+			reg += sizeof(u32);
+		}
+	}
+}
+
 #define EEPROM_STAT_ADDR   0x7bfc
 #define VPD_BASE           0x400
 #define VPD_BASE_OLD       0
@@ -867,7 +1595,7 @@ static int flash_wait_op(struct adapter *adapter, int attempts, int delay)
  *	Read the specified number of 32-bit words from the serial flash.
  *	If @byte_oriented is set the read data is stored as a byte array
  *	(i.e., big-endian), otherwise as 32-bit words in the platform's
- *	natural endianess.
+ *	natural endianness.
  */
 int t4_read_flash(struct adapter *adapter, unsigned int addr,
 		  unsigned int nwords, u32 *data, int byte_oriented)
@@ -1120,7 +1848,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 */
 	}
@@ -3558,7 +4286,7 @@ int t4_fixup_host_params(struct adapter *adap, unsigned int page_size,
 	 * For the single-MTU buffers in unpacked mode we need to include
 	 * space for the SGE Control Packet Shift, 14 byte Ethernet header,
 	 * possible 4 byte VLAN tag, all rounded up to the next Ingress Packet
-	 * Padding boundry.  All of these are accommodated in the Factory
+	 * Padding boundary.  All of these are accommodated in the Factory
 	 * Default Firmware Configuration File but we need to adjust it for
 	 * this host's cache line size.
 	 */
@@ -4459,6 +5187,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
  *
@@ -4529,7 +5310,7 @@ int t4_init_tp_params(struct adapter *adap)
 							       PROTOCOL_F);
 
 	/* If TP_INGRESS_CONFIG.VNID == 0, then TP_VLAN_PRI_MAP.VNIC_ID
-	 * represents the presense of an Outer VLAN instead of a VNIC ID.
+	 * represents the presence of an Outer VLAN instead of a VNIC ID.
 	 */
 	if ((adap->params.tp.ingress_config & VNIC_F) == 0)
 		adap->params.tp.vnic_shift = -1;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
index 0fb975e258b3..30a2f56e99c2 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
@@ -794,6 +794,14 @@ struct cpl_rx_pkt {
 	__be16 err_vec;
 };
 
+#define RXF_PSH_S    20
+#define RXF_PSH_V(x) ((x) << RXF_PSH_S)
+#define RXF_PSH_F    RXF_PSH_V(1U)
+
+#define RXF_SYN_S    21
+#define RXF_SYN_V(x) ((x) << RXF_SYN_S)
+#define RXF_SYN_F    RXF_SYN_V(1U)
+
 #define RXF_UDP_S    22
 #define RXF_UDP_V(x) ((x) << RXF_UDP_S)
 #define RXF_UDP_F    RXF_UDP_V(1U)
@@ -810,6 +818,18 @@ struct cpl_rx_pkt {
 #define RXF_IP6_V(x) ((x) << RXF_IP6_S)
 #define RXF_IP6_F    RXF_IP6_V(1U)
 
+#define RXF_SYN_COOKIE_S    26
+#define RXF_SYN_COOKIE_V(x) ((x) << RXF_SYN_COOKIE_S)
+#define RXF_SYN_COOKIE_F    RXF_SYN_COOKIE_V(1U)
+
+#define RXF_FCOE_S    26
+#define RXF_FCOE_V(x) ((x) << RXF_FCOE_S)
+#define RXF_FCOE_F    RXF_FCOE_V(1U)
+
+#define RXF_LRO_S    27
+#define RXF_LRO_V(x) ((x) << RXF_LRO_S)
+#define RXF_LRO_F    RXF_LRO_V(1U)
+
 /* rx_pkt.l2info fields */
 #define RX_ETHHDR_LEN_S    0
 #define RX_ETHHDR_LEN_M    0x1F
@@ -846,6 +866,11 @@ struct cpl_rx_pkt {
 #define RX_IPHDR_LEN_V(x) ((x) << RX_IPHDR_LEN_S)
 #define RX_IPHDR_LEN_G(x) (((x) >> RX_IPHDR_LEN_S) & RX_IPHDR_LEN_M)
 
+/* rx_pkt.err_vec fields */
+#define RXERR_CSUM_S    13
+#define RXERR_CSUM_V(x) ((x) << RXERR_CSUM_S)
+#define RXERR_CSUM_F    RXERR_CSUM_V(1U)
+
 struct cpl_trace_pkt {
 	u8 opcode;
 	u8 intf;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
index ddfb5b846045..1a9a6f334d2d 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
@@ -60,8 +60,6 @@
  *   -- Used to finish the definition of the PCI ID Table.  Note that we
  *   -- will be adding a trailing semi-colon (";") here.
  */
-#ifdef CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN
-
 #ifndef CH_PCI_DEVICE_ID_FUNCTION
 #error CH_PCI_DEVICE_ID_FUNCTION not defined!
 #endif
@@ -154,8 +152,7 @@ CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN
 	CH_PCI_ID_TABLE_FENTRY(0x5087),	/* Custom T580-CR */
 	CH_PCI_ID_TABLE_FENTRY(0x5088),	/* Custom T570-CR */
 	CH_PCI_ID_TABLE_FENTRY(0x5089),	/* Custom T520-CR */
+	CH_PCI_ID_TABLE_FENTRY(0x5090),	/* Custom T540-CR */
 CH_PCI_DEVICE_ID_TABLE_DEFINE_END;
 
-#endif /* CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN */
-
 #endif /* __T4_PCI_ID_TBL_H__ */
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..03fbfd1fb3df 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -36,7 +36,7 @@
 #define _T4FW_INTERFACE_H_
 
 enum fw_retval {
-	FW_SUCCESS		= 0,	/* completed sucessfully */
+	FW_SUCCESS		= 0,	/* completed successfully */
 	FW_EPERM		= 1,	/* operation not permitted */
 	FW_ENOENT		= 2,	/* no such file or directory */
 	FW_EIO			= 5,	/* input/output error; hw bad */
@@ -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/cxgb4vf_main.c b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
index 122e2964e63b..1d893b0b7ddf 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
@@ -3034,7 +3034,7 @@ static void cxgb4vf_pci_shutdown(struct pci_dev *pdev)
 /* Macros needed to support the PCI Device ID Table ...
  */
 #define CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN \
-	static struct pci_device_id cxgb4vf_pci_tbl[] = {
+	static const struct pci_device_id cxgb4vf_pci_tbl[] = {
 #define CH_PCI_DEVICE_ID_FUNCTION	0x8
 
 #define CH_PCI_ID_TABLE_ENTRY(devid) \
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index 0545f0de1c52..482f6de6817d 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -875,7 +875,7 @@ static inline unsigned int calc_tx_flits(const struct sk_buff *skb)
 	 * Write Header (incorporated as part of the cpl_tx_pkt_lso and
 	 * cpl_tx_pkt structures), followed by either a TX Packet Write CPL
 	 * message or, if we're doing a Large Send Offload, an LSO CPL message
-	 * with an embeded TX Packet Write CPL message.
+	 * with an embedded TX Packet Write CPL message.
 	 */
 	flits = sgl_len(skb_shinfo(skb)->nr_frags + 1);
 	if (skb_shinfo(skb)->gso_size)
@@ -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;
 
@@ -1747,7 +1751,7 @@ static int process_responses(struct sge_rspq *rspq, int budget)
 		 * Figure out what kind of response we've received from the
 		 * SGE.
 		 */
-		rmb();
+		dma_rmb();
 		rsp_type = RSPD_TYPE(rc->type_gen);
 		if (likely(rsp_type == RSP_TYPE_FLBUF)) {
 			struct page_frag *fp;
@@ -1931,7 +1935,7 @@ static unsigned int process_intrq(struct adapter *adapter)
 		 * error and go on to the next response message.  This should
 		 * never happen ...
 		 */
-		rmb();
+		dma_rmb();
 		if (unlikely(RSPD_TYPE(rc->type_gen) != RSP_TYPE_INTR)) {
 			dev_err(adapter->pdev_dev,
 				"Unexpected INTRQ response type %d\n",
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index 1b5506df35b1..966ee900ed00 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,
@@ -339,7 +339,7 @@ int t4vf_port_init(struct adapter *adapter, int pidx)
  *      @adapter: the adapter
  *
  *	Issues a reset command to FW.  For a Physical Function this would
- *	result in the Firmware reseting all of its state.  For a Virtual
+ *	result in the Firmware resetting all of its state.  For a Virtual
  *	Function this just resets the state associated with the VF.
  */
 int t4vf_fw_reset(struct adapter *adapter)
@@ -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/cirrus/cs89x0.c b/drivers/net/ethernet/cirrus/cs89x0.c
index d1c025fd9726..60383040d6c6 100644
--- a/drivers/net/ethernet/cirrus/cs89x0.c
+++ b/drivers/net/ethernet/cirrus/cs89x0.c
@@ -1578,7 +1578,7 @@ out1:
 
 #ifndef CONFIG_CS89x0_PLATFORM
 /*
- * This function converts the I/O port addres used by the cs89x0_probe() and
+ * This function converts the I/O port address used by the cs89x0_probe() and
  * init_module() functions to the I/O memory address used by the
  * cs89x0_probe1() function.
  */
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index a5179bfcdc2c..204bd182473b 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -893,7 +893,7 @@ static int enic_set_vf_port(struct net_device *netdev, int vf,
 		} else {
 			memset(pp, 0, sizeof(*pp));
 			if (vf == PORT_SELF_VF)
-				memset(netdev->dev_addr, 0, ETH_ALEN);
+				eth_zero_addr(netdev->dev_addr);
 		}
 	} else {
 		/* Set flag to indicate that the port assoc/disassoc
@@ -903,14 +903,14 @@ static int enic_set_vf_port(struct net_device *netdev, int vf,
 
 		/* If DISASSOCIATE, clean up all assigned/saved macaddresses */
 		if (pp->request == PORT_REQUEST_DISASSOCIATE) {
-			memset(pp->mac_addr, 0, ETH_ALEN);
+			eth_zero_addr(pp->mac_addr);
 			if (vf == PORT_SELF_VF)
-				memset(netdev->dev_addr, 0, ETH_ALEN);
+				eth_zero_addr(netdev->dev_addr);
 		}
 	}
 
 	if (vf == PORT_SELF_VF)
-		memset(pp->vf_mac, 0, ETH_ALEN);
+		eth_zero_addr(pp->vf_mac);
 
 	return err;
 }
diff --git a/drivers/net/ethernet/dec/tulip/dmfe.c b/drivers/net/ethernet/dec/tulip/dmfe.c
index 50a00777228e..afd8e78e024e 100644
--- a/drivers/net/ethernet/dec/tulip/dmfe.c
+++ b/drivers/net/ethernet/dec/tulip/dmfe.c
@@ -653,7 +653,7 @@ static void dmfe_init_dm910x(struct DEVICE *dev)
 	if ( !(db->media_mode & DMFE_AUTO) )
 		db->op_mode = db->media_mode; 	/* Force Mode */
 
-	/* Initialize Transmit/Receive decriptor and CR3/4 */
+	/* Initialize Transmit/Receive descriptor and CR3/4 */
 	dmfe_descriptor_init(dev);
 
 	/* Init CR6 to program DM910x operation */
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/dec/tulip/uli526x.c b/drivers/net/ethernet/dec/tulip/uli526x.c
index 1c5916b13778..2c30c0c83f98 100644
--- a/drivers/net/ethernet/dec/tulip/uli526x.c
+++ b/drivers/net/ethernet/dec/tulip/uli526x.c
@@ -564,7 +564,7 @@ static void uli526x_init(struct net_device *dev)
 	if ( !(db->media_mode & ULI526X_AUTO) )
 		db->op_mode = db->media_mode;		/* Force Mode */
 
-	/* Initialize Transmit/Receive decriptor and CR3/4 */
+	/* Initialize Transmit/Receive descriptor and CR3/4 */
 	uli526x_descriptor_init(dev, ioaddr);
 
 	/* Init CR6 to program M526X operation */
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 27de37aa90af..1bf1cdce74ac 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -30,11 +30,12 @@
 #include <linux/firmware.h>
 #include <linux/slab.h>
 #include <linux/u64_stats_sync.h>
+#include <linux/cpumask.h>
 
 #include "be_hw.h"
 #include "be_roce.h"
 
-#define DRV_VER			"10.4u"
+#define DRV_VER			"10.6.0.1"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"Emulex BladeEngine2"
 #define BE3_NAME		"Emulex BladeEngine3"
@@ -87,6 +88,7 @@
 #define BE3_MAX_EVT_QS		16
 #define BE3_SRIOV_MAX_EVT_QS	8
 
+#define MAX_RSS_IFACES		15
 #define MAX_RX_QS		32
 #define MAX_EVT_QS		32
 #define MAX_TX_QS		32
@@ -97,6 +99,7 @@
 #define BE_NAPI_WEIGHT		64
 #define MAX_RX_POST		BE_NAPI_WEIGHT /* Frags posted at a time */
 #define RX_FRAGS_REFILL_WM	(RX_Q_LEN - MAX_RX_POST)
+#define MAX_NUM_POST_ERX_DB	255u
 
 #define MAX_VFS			30 /* Max VFs supported by BE3 FW */
 #define FW_VER_LEN		32
@@ -182,6 +185,7 @@ struct be_eq_obj {
 	u16 spurious_intr;
 	struct napi_struct napi;
 	struct be_adapter *adapter;
+	cpumask_var_t  affinity_mask;
 
 #ifdef CONFIG_NET_RX_BUSY_POLL
 #define BE_EQ_IDLE		0
@@ -238,10 +242,17 @@ struct be_tx_stats {
 	struct u64_stats_sync sync_compl;
 };
 
+/* Structure to hold some data of interest obtained from a TX CQE */
+struct be_tx_compl_info {
+	u8 status;		/* Completion status */
+	u16 end_index;		/* Completed TXQ Index */
+};
+
 struct be_tx_obj {
 	u32 db_offset;
 	struct be_queue_info q;
 	struct be_queue_info cq;
+	struct be_tx_compl_info txcp;
 	/* Remember the skbs that were transmitted */
 	struct sk_buff *sent_skb_list[TX_Q_LEN];
 	struct be_tx_stats stats;
@@ -354,6 +365,7 @@ struct be_vf_cfg {
 	u16 vlan_tag;
 	u32 tx_rate;
 	u32 plink_tracking;
+	u32 privileges;
 };
 
 enum vf_state {
@@ -369,6 +381,7 @@ enum vf_state {
 #define BE_FLAGS_VXLAN_OFFLOADS			BIT(8)
 #define BE_FLAGS_SETUP_DONE			BIT(9)
 #define BE_FLAGS_EVT_INCOMPATIBLE_SFP		BIT(10)
+#define BE_FLAGS_ERR_DETECTION_SCHEDULED	BIT(11)
 
 #define BE_UC_PMAC_COUNT			30
 #define BE_VF_UC_PMAC_COUNT			2
@@ -403,8 +416,11 @@ struct be_resources {
 	u16 max_tx_qs;
 	u16 max_rss_qs;
 	u16 max_rx_qs;
+	u16 max_cq_count;
 	u16 max_uc_mac;		/* Max UC MACs programmable */
 	u16 max_vlans;		/* Number of vlans supported */
+	u16 max_iface_count;
+	u16 max_mcc_count;
 	u16 max_evt_qs;
 	u32 if_cap_flags;
 	u32 vf_if_cap_flags;	/* VF if capability flags */
@@ -417,12 +433,46 @@ struct rss_info {
 	u8 rss_hkey[RSS_HASH_KEY_LEN];
 };
 
+/* Macros to read/write the 'features' word of be_wrb_params structure.
+ */
+#define	BE_WRB_F_BIT(name)			BE_WRB_F_##name##_BIT
+#define	BE_WRB_F_MASK(name)			BIT_MASK(BE_WRB_F_##name##_BIT)
+
+#define	BE_WRB_F_GET(word, name)	\
+	(((word) & (BE_WRB_F_MASK(name))) >> BE_WRB_F_BIT(name))
+
+#define	BE_WRB_F_SET(word, name, val)	\
+	((word) |= (((val) << BE_WRB_F_BIT(name)) & BE_WRB_F_MASK(name)))
+
+/* Feature/offload bits */
+enum {
+	BE_WRB_F_CRC_BIT,		/* Ethernet CRC */
+	BE_WRB_F_IPCS_BIT,		/* IP csum */
+	BE_WRB_F_TCPCS_BIT,		/* TCP csum */
+	BE_WRB_F_UDPCS_BIT,		/* UDP csum */
+	BE_WRB_F_LSO_BIT,		/* LSO */
+	BE_WRB_F_LSO6_BIT,		/* LSO6 */
+	BE_WRB_F_VLAN_BIT,		/* VLAN */
+	BE_WRB_F_VLAN_SKIP_HW_BIT	/* Skip VLAN tag (workaround) */
+};
+
+/* The structure below provides a HW-agnostic abstraction of WRB params
+ * retrieved from a TX skb. This is in turn passed to chip specific routines
+ * during transmit, to set the corresponding params in the WRB.
+ */
+struct be_wrb_params {
+	u32 features;	/* Feature bits */
+	u16 vlan_tag;	/* VLAN tag */
+	u16 lso_mss;	/* MSS for LSO */
+};
+
 struct be_adapter {
 	struct pci_dev *pdev;
 	struct net_device *netdev;
 
 	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;
@@ -447,6 +497,8 @@ struct be_adapter {
 
 	/* Rx rings */
 	u16 num_rx_qs;
+	u16 num_rss_qs;
+	u16 need_def_rxq;
 	struct be_rx_obj rx_obj[MAX_RX_QS];
 	u32 big_page_size;	/* Compounded page size shared by rx wrbs */
 
@@ -461,7 +513,7 @@ struct be_adapter {
 	struct delayed_work work;
 	u16 work_counter;
 
-	struct delayed_work func_recovery_work;
+	struct delayed_work be_err_detection_work;
 	u32 flags;
 	u32 cmd_privileges;
 	/* Ethtool knobs and info */
@@ -594,9 +646,8 @@ extern const struct ethtool_ops be_ethtool_ops;
 	for (i = 0, rxo = &adapter->rx_obj[i]; i < adapter->num_rx_qs;	\
 		i++, rxo++)
 
-/* Skip the default non-rss queue (last one)*/
 #define for_all_rss_queues(adapter, rxo, i)				\
-	for (i = 0, rxo = &adapter->rx_obj[i]; i < (adapter->num_rx_qs - 1);\
+	for (i = 0, rxo = &adapter->rx_obj[i]; i < adapter->num_rss_qs;	\
 		i++, rxo++)
 
 #define for_all_tx_queues(adapter, txo, i)				\
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 36916cfa70f9..fb140faeafb1 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -635,73 +635,16 @@ static int lancer_wait_ready(struct be_adapter *adapter)
 	for (i = 0; i < SLIPORT_READY_TIMEOUT; i++) {
 		sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
 		if (sliport_status & SLIPORT_STATUS_RDY_MASK)
-			break;
-
-		msleep(1000);
-	}
-
-	if (i == SLIPORT_READY_TIMEOUT)
-		return sliport_status ? : -1;
-
-	return 0;
-}
-
-static bool lancer_provisioning_error(struct be_adapter *adapter)
-{
-	u32 sliport_status = 0, sliport_err1 = 0, sliport_err2 = 0;
-
-	sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
-	if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
-		sliport_err1 = ioread32(adapter->db + SLIPORT_ERROR1_OFFSET);
-		sliport_err2 = ioread32(adapter->db + SLIPORT_ERROR2_OFFSET);
-
-		if (sliport_err1 == SLIPORT_ERROR_NO_RESOURCE1 &&
-		    sliport_err2 == SLIPORT_ERROR_NO_RESOURCE2)
-			return true;
-	}
-	return false;
-}
-
-int lancer_test_and_set_rdy_state(struct be_adapter *adapter)
-{
-	int status;
-	u32 sliport_status, err, reset_needed;
-	bool resource_error;
+			return 0;
 
-	resource_error = lancer_provisioning_error(adapter);
-	if (resource_error)
-		return -EAGAIN;
+		if (sliport_status & SLIPORT_STATUS_ERR_MASK &&
+		    !(sliport_status & SLIPORT_STATUS_RN_MASK))
+			return -EIO;
 
-	status = lancer_wait_ready(adapter);
-	if (!status) {
-		sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
-		err = sliport_status & SLIPORT_STATUS_ERR_MASK;
-		reset_needed = sliport_status & SLIPORT_STATUS_RN_MASK;
-		if (err && reset_needed) {
-			iowrite32(SLI_PORT_CONTROL_IP_MASK,
-				  adapter->db + SLIPORT_CONTROL_OFFSET);
-
-			/* check if adapter has corrected the error */
-			status = lancer_wait_ready(adapter);
-			sliport_status = ioread32(adapter->db +
-						  SLIPORT_STATUS_OFFSET);
-			sliport_status &= (SLIPORT_STATUS_ERR_MASK |
-						SLIPORT_STATUS_RN_MASK);
-			if (status || sliport_status)
-				status = -1;
-		} else if (err || reset_needed) {
-			status = -1;
-		}
+		msleep(1000);
 	}
-	/* Stop error recovery if error is not recoverable.
-	 * No resource error is temporary errors and will go away
-	 * when PF provisions resources.
-	 */
-	resource_error = lancer_provisioning_error(adapter);
-	if (resource_error)
-		status = -EAGAIN;
 
-	return status;
+	return sliport_status ? : -1;
 }
 
 int be_fw_wait_ready(struct be_adapter *adapter)
@@ -720,6 +663,10 @@ int be_fw_wait_ready(struct be_adapter *adapter)
 	}
 
 	do {
+		/* There's no means to poll POST state on BE2/3 VFs */
+		if (BEx_chip(adapter) && be_virtfn(adapter))
+			return 0;
+
 		stage = be_POST_stage_get(adapter);
 		if (stage == POST_STAGE_ARMFW_RDY)
 			return 0;
@@ -734,7 +681,7 @@ int be_fw_wait_ready(struct be_adapter *adapter)
 
 err:
 	dev_err(dev, "POST timeout; stage=%#x\n", stage);
-	return -1;
+	return -ETIMEDOUT;
 }
 
 static inline struct be_sge *nonembedded_sgl(struct be_mcc_wrb *wrb)
@@ -1902,15 +1849,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 +1861,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 +1879,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;
@@ -2126,16 +2070,12 @@ int be_cmd_reset_function(struct be_adapter *adapter)
 	int status;
 
 	if (lancer_chip(adapter)) {
+		iowrite32(SLI_PORT_CONTROL_IP_MASK,
+			  adapter->db + SLIPORT_CONTROL_OFFSET);
 		status = lancer_wait_ready(adapter);
-		if (!status) {
-			iowrite32(SLI_PORT_CONTROL_IP_MASK,
-				  adapter->db + SLIPORT_CONTROL_OFFSET);
-			status = lancer_test_and_set_rdy_state(adapter);
-		}
-		if (status) {
+		if (status)
 			dev_err(&adapter->pdev->dev,
 				"Adapter in non recoverable error\n");
-		}
 		return status;
 	}
 
@@ -3078,7 +3018,7 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
 
 		mac_count = resp->true_mac_count + resp->pseudo_mac_count;
 		/* Mac list returned could contain one or more active mac_ids
-		 * or one or more true or pseudo permanant mac addresses.
+		 * or one or more true or pseudo permanent mac addresses.
 		 * If an active mac_id is present, return first active mac_id
 		 * found.
 		 */
@@ -3133,7 +3073,7 @@ int be_cmd_get_perm_mac(struct be_adapter *adapter, u8 *mac)
 	int status;
 	bool pmac_valid = false;
 
-	memset(mac, 0, ETH_ALEN);
+	eth_zero_addr(mac);
 
 	if (BEx_chip(adapter)) {
 		if (be_physfn(adapter))
@@ -3634,12 +3574,12 @@ static void be_copy_nic_desc(struct be_resources *res,
 	res->max_rss_qs = le16_to_cpu(desc->rssq_count);
 	res->max_rx_qs = le16_to_cpu(desc->rq_count);
 	res->max_evt_qs = le16_to_cpu(desc->eq_count);
+	res->max_cq_count = le16_to_cpu(desc->cq_count);
+	res->max_iface_count = le16_to_cpu(desc->iface_count);
+	res->max_mcc_count = le16_to_cpu(desc->mcc_count);
 	/* Clear flags that driver is not interested in */
 	res->if_cap_flags = le32_to_cpu(desc->cap_flags) &
 				BE_IF_CAP_FLAGS_WANT;
-	/* Need 1 RXQ as the default RXQ */
-	if (res->max_rss_qs && res->max_rss_qs == res->max_rx_qs)
-		res->max_rss_qs -= 1;
 }
 
 /* Uses Mbox */
@@ -3701,7 +3641,7 @@ err:
 
 /* Will use MBOX only if MCCQ has not been created */
 int be_cmd_get_profile_config(struct be_adapter *adapter,
-			      struct be_resources *res, u8 domain)
+			      struct be_resources *res, u8 query, u8 domain)
 {
 	struct be_cmd_resp_get_profile_config *resp;
 	struct be_cmd_req_get_profile_config *req;
@@ -3711,7 +3651,7 @@ int be_cmd_get_profile_config(struct be_adapter *adapter,
 	struct be_nic_res_desc *nic;
 	struct be_mcc_wrb wrb = {0};
 	struct be_dma_mem cmd;
-	u32 desc_count;
+	u16 desc_count;
 	int status;
 
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
@@ -3730,12 +3670,19 @@ int be_cmd_get_profile_config(struct be_adapter *adapter,
 		req->hdr.version = 1;
 	req->type = ACTIVE_PROFILE_TYPE;
 
+	/* When QUERY_MODIFIABLE_FIELDS_TYPE bit is set, cmd returns the
+	 * descriptors with all bits set to "1" for the fields which can be
+	 * modified using SET_PROFILE_CONFIG cmd.
+	 */
+	if (query == RESOURCE_MODIFIABLE)
+		req->type |= QUERY_MODIFIABLE_FIELDS_TYPE;
+
 	status = be_cmd_notify_wait(adapter, &wrb);
 	if (status)
 		goto err;
 
 	resp = cmd.va;
-	desc_count = le32_to_cpu(resp->desc_count);
+	desc_count = le16_to_cpu(resp->desc_count);
 
 	pcie = be_get_pcie_desc(adapter->pdev->devfn, resp->func_param,
 				desc_count);
@@ -3860,23 +3807,80 @@ int be_cmd_config_qos(struct be_adapter *adapter, u32 max_rate, u16 link_speed,
 					 1, version, domain);
 }
 
+static void be_fill_vf_res_template(struct be_adapter *adapter,
+				    struct be_resources pool_res,
+				    u16 num_vfs, u16 num_vf_qs,
+				    struct be_nic_res_desc *nic_vft)
+{
+	u32 vf_if_cap_flags = pool_res.vf_if_cap_flags;
+	struct be_resources res_mod = {0};
+
+	/* Resource with fields set to all '1's by GET_PROFILE_CONFIG cmd,
+	 * which are modifiable using SET_PROFILE_CONFIG cmd.
+	 */
+	be_cmd_get_profile_config(adapter, &res_mod, RESOURCE_MODIFIABLE, 0);
+
+	/* If RSS IFACE capability flags are modifiable for a VF, set the
+	 * capability flag as valid and set RSS and DEFQ_RSS IFACE flags if
+	 * more than 1 RSSQ is available for a VF.
+	 * Otherwise, provision only 1 queue pair for VF.
+	 */
+	if (res_mod.vf_if_cap_flags & BE_IF_FLAGS_RSS) {
+		nic_vft->flags |= BIT(IF_CAPS_FLAGS_VALID_SHIFT);
+		if (num_vf_qs > 1) {
+			vf_if_cap_flags |= BE_IF_FLAGS_RSS;
+			if (pool_res.if_cap_flags & BE_IF_FLAGS_DEFQ_RSS)
+				vf_if_cap_flags |= BE_IF_FLAGS_DEFQ_RSS;
+		} else {
+			vf_if_cap_flags &= ~(BE_IF_FLAGS_RSS |
+					     BE_IF_FLAGS_DEFQ_RSS);
+		}
+
+		nic_vft->cap_flags = cpu_to_le32(vf_if_cap_flags);
+	} else {
+		num_vf_qs = 1;
+	}
+
+	nic_vft->rq_count = cpu_to_le16(num_vf_qs);
+	nic_vft->txq_count = cpu_to_le16(num_vf_qs);
+	nic_vft->rssq_count = cpu_to_le16(num_vf_qs);
+	nic_vft->cq_count = cpu_to_le16(pool_res.max_cq_count /
+					(num_vfs + 1));
+
+	/* Distribute unicast MACs, VLANs, IFACE count and MCCQ count equally
+	 * among the PF and it's VFs, if the fields are changeable
+	 */
+	if (res_mod.max_uc_mac == FIELD_MODIFIABLE)
+		nic_vft->unicast_mac_count = cpu_to_le16(pool_res.max_uc_mac /
+							 (num_vfs + 1));
+
+	if (res_mod.max_vlans == FIELD_MODIFIABLE)
+		nic_vft->vlan_count = cpu_to_le16(pool_res.max_vlans /
+						  (num_vfs + 1));
+
+	if (res_mod.max_iface_count == FIELD_MODIFIABLE)
+		nic_vft->iface_count = cpu_to_le16(pool_res.max_iface_count /
+						   (num_vfs + 1));
+
+	if (res_mod.max_mcc_count == FIELD_MODIFIABLE)
+		nic_vft->mcc_count = cpu_to_le16(pool_res.max_mcc_count /
+						 (num_vfs + 1));
+}
+
 int be_cmd_set_sriov_config(struct be_adapter *adapter,
-			    struct be_resources res, u16 num_vfs)
+			    struct be_resources pool_res, u16 num_vfs,
+			    u16 num_vf_qs)
 {
 	struct {
 		struct be_pcie_res_desc pcie;
 		struct be_nic_res_desc nic_vft;
 	} __packed desc;
-	u16 vf_q_count;
-
-	if (BEx_chip(adapter) || lancer_chip(adapter))
-		return 0;
 
 	/* PF PCIE descriptor */
 	be_reset_pcie_desc(&desc.pcie);
 	desc.pcie.hdr.desc_type = PCIE_RESOURCE_DESC_TYPE_V1;
 	desc.pcie.hdr.desc_len = RESOURCE_DESC_SIZE_V1;
-	desc.pcie.flags = (1 << IMM_SHIFT) | (1 << NOSV_SHIFT);
+	desc.pcie.flags = BIT(IMM_SHIFT) | BIT(NOSV_SHIFT);
 	desc.pcie.pf_num = adapter->pdev->devfn;
 	desc.pcie.sriov_state = num_vfs ? 1 : 0;
 	desc.pcie.num_vfs = cpu_to_le16(num_vfs);
@@ -3885,32 +3889,12 @@ int be_cmd_set_sriov_config(struct be_adapter *adapter,
 	be_reset_nic_desc(&desc.nic_vft);
 	desc.nic_vft.hdr.desc_type = NIC_RESOURCE_DESC_TYPE_V1;
 	desc.nic_vft.hdr.desc_len = RESOURCE_DESC_SIZE_V1;
-	desc.nic_vft.flags = (1 << VFT_SHIFT) | (1 << IMM_SHIFT) |
-				(1 << NOSV_SHIFT);
+	desc.nic_vft.flags = BIT(VFT_SHIFT) | BIT(IMM_SHIFT) | BIT(NOSV_SHIFT);
 	desc.nic_vft.pf_num = adapter->pdev->devfn;
 	desc.nic_vft.vf_num = 0;
 
-	if (num_vfs && res.vf_if_cap_flags & BE_IF_FLAGS_RSS) {
-		/* If number of VFs requested is 8 less than max supported,
-		 * assign 8 queue pairs to the PF and divide the remaining
-		 * resources evenly among the VFs
-		 */
-		if (num_vfs < (be_max_vfs(adapter) - 8))
-			vf_q_count = (res.max_rss_qs - 8) / num_vfs;
-		else
-			vf_q_count = res.max_rss_qs / num_vfs;
-
-		desc.nic_vft.rq_count = cpu_to_le16(vf_q_count);
-		desc.nic_vft.txq_count = cpu_to_le16(vf_q_count);
-		desc.nic_vft.rssq_count = cpu_to_le16(vf_q_count - 1);
-		desc.nic_vft.cq_count = cpu_to_le16(3 * vf_q_count);
-	} else {
-		desc.nic_vft.txq_count = cpu_to_le16(1);
-		desc.nic_vft.rq_count = cpu_to_le16(1);
-		desc.nic_vft.rssq_count = cpu_to_le16(0);
-		/* One CQ for each TX, RX and MCCQ */
-		desc.nic_vft.cq_count = cpu_to_le16(3);
-	}
+	be_fill_vf_res_template(adapter, pool_res, num_vfs, num_vf_qs,
+				&desc.nic_vft);
 
 	return be_cmd_set_profile_config(adapter, &desc,
 					 2 * RESOURCE_DESC_SIZE_V1, 2, 1, 0);
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index db761e8e42a3..1ec22300e254 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -588,14 +588,15 @@ enum be_if_flags {
 	BE_IF_FLAGS_MCAST_PROMISCUOUS = 0x200,
 	BE_IF_FLAGS_PASS_L2_ERRORS = 0x400,
 	BE_IF_FLAGS_PASS_L3L4_ERRORS = 0x800,
-	BE_IF_FLAGS_MULTICAST = 0x1000
+	BE_IF_FLAGS_MULTICAST = 0x1000,
+	BE_IF_FLAGS_DEFQ_RSS = 0x1000000
 };
 
 #define BE_IF_CAP_FLAGS_WANT (BE_IF_FLAGS_RSS | BE_IF_FLAGS_PROMISCUOUS |\
 			 BE_IF_FLAGS_BROADCAST | BE_IF_FLAGS_VLAN_PROMISCUOUS |\
 			 BE_IF_FLAGS_VLAN | BE_IF_FLAGS_MCAST_PROMISCUOUS |\
 			 BE_IF_FLAGS_PASS_L3L4_ERRORS | BE_IF_FLAGS_MULTICAST |\
-			 BE_IF_FLAGS_UNTAGGED)
+			 BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_DEFQ_RSS)
 
 #define BE_IF_FLAGS_ALL_PROMISCUOUS	(BE_IF_FLAGS_PROMISCUOUS | \
 					 BE_IF_FLAGS_VLAN_PROMISCUOUS |\
@@ -2021,6 +2022,7 @@ struct be_cmd_req_set_ext_fat_caps {
 #define PORT_RESOURCE_DESC_TYPE_V1		0x55
 #define MAX_RESOURCE_DESC			264
 
+#define IF_CAPS_FLAGS_VALID_SHIFT		0	/* IF caps valid */
 #define VFT_SHIFT				3	/* VF template */
 #define IMM_SHIFT				6	/* Immediate */
 #define NOSV_SHIFT				7	/* No save */
@@ -2131,20 +2133,28 @@ struct be_cmd_resp_get_func_config {
 	u8 func_param[MAX_RESOURCE_DESC * RESOURCE_DESC_SIZE_V1];
 };
 
-#define ACTIVE_PROFILE_TYPE			0x2
+enum {
+	RESOURCE_LIMITS,
+	RESOURCE_MODIFIABLE
+};
+
 struct be_cmd_req_get_profile_config {
 	struct be_cmd_req_hdr hdr;
 	u8 rsvd;
+#define ACTIVE_PROFILE_TYPE			0x2
+#define QUERY_MODIFIABLE_FIELDS_TYPE		BIT(3)
 	u8 type;
 	u16 rsvd1;
 };
 
 struct be_cmd_resp_get_profile_config {
 	struct be_cmd_resp_hdr hdr;
-	u32 desc_count;
+	__le16 desc_count;
+	u16 rsvd;
 	u8 func_param[MAX_RESOURCE_DESC * RESOURCE_DESC_SIZE_V1];
 };
 
+#define FIELD_MODIFIABLE			0xFFFF
 struct be_cmd_req_set_profile_config {
 	struct be_cmd_req_hdr hdr;
 	u32 rsvd;
@@ -2256,7 +2266,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);
@@ -2344,7 +2354,7 @@ int be_cmd_query_port_name(struct be_adapter *adapter);
 int be_cmd_get_func_config(struct be_adapter *adapter,
 			   struct be_resources *res);
 int be_cmd_get_profile_config(struct be_adapter *adapter,
-			      struct be_resources *res, u8 domain);
+			      struct be_resources *res, u8 query, u8 domain);
 int be_cmd_get_active_profile(struct be_adapter *adapter, u16 *profile);
 int be_cmd_get_if_id(struct be_adapter *adapter, struct be_vf_cfg *vf_cfg,
 		     int vf_num);
@@ -2355,4 +2365,5 @@ int be_cmd_set_logical_link_config(struct be_adapter *adapter,
 int be_cmd_set_vxlan_port(struct be_adapter *adapter, __be16 port);
 int be_cmd_manage_iface(struct be_adapter *adapter, u32 iface, u8 op);
 int be_cmd_set_sriov_config(struct be_adapter *adapter,
-			    struct be_resources res, u16 num_vfs);
+			    struct be_resources res, u16 num_vfs,
+			    u16 num_vf_qs);
diff --git a/drivers/net/ethernet/emulex/benet/be_ethtool.c b/drivers/net/ethernet/emulex/benet/be_ethtool.c
index 4d2de4700769..b765c24625bf 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -1097,7 +1097,7 @@ static int be_set_rss_hash_opts(struct be_adapter *adapter,
 		return status;
 
 	if (be_multi_rxq(adapter)) {
-		for (j = 0; j < 128; j += adapter->num_rx_qs - 1) {
+		for (j = 0; j < 128; j += adapter->num_rss_qs) {
 			for_all_rss_queues(adapter, rxo, i) {
 				if ((j + i) >= 128)
 					break;
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 0a816859aca5..fb0bc3c3620e 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -30,6 +30,9 @@ MODULE_DESCRIPTION(DRV_DESC " " DRV_VER);
 MODULE_AUTHOR("Emulex Corporation");
 MODULE_LICENSE("GPL");
 
+/* num_vfs module param is obsolete.
+ * Use sysfs method to enable/disable VFs.
+ */
 static unsigned int num_vfs;
 module_param(num_vfs, uint, S_IRUGO);
 MODULE_PARM_DESC(num_vfs, "Number of PCI VFs to initialize");
@@ -727,48 +730,86 @@ static u16 skb_ip_proto(struct sk_buff *skb)
 		ip_hdr(skb)->protocol : ipv6_hdr(skb)->nexthdr;
 }
 
-static void wrb_fill_hdr(struct be_adapter *adapter, struct be_eth_hdr_wrb *hdr,
-			 struct sk_buff *skb, u32 wrb_cnt, u32 len,
-			 bool skip_hw_vlan)
+static inline bool be_is_txq_full(struct be_tx_obj *txo)
 {
-	u16 vlan_tag, proto;
+	return atomic_read(&txo->q.used) + BE_MAX_TX_FRAG_COUNT >= txo->q.len;
+}
 
-	memset(hdr, 0, sizeof(*hdr));
+static inline bool be_can_txq_wake(struct be_tx_obj *txo)
+{
+	return atomic_read(&txo->q.used) < txo->q.len / 2;
+}
 
-	SET_TX_WRB_HDR_BITS(crc, hdr, 1);
+static inline bool be_is_tx_compl_pending(struct be_tx_obj *txo)
+{
+	return atomic_read(&txo->q.used) > txo->pend_wrb_cnt;
+}
+
+static void be_get_wrb_params_from_skb(struct be_adapter *adapter,
+				       struct sk_buff *skb,
+				       struct be_wrb_params *wrb_params)
+{
+	u16 proto;
 
 	if (skb_is_gso(skb)) {
-		SET_TX_WRB_HDR_BITS(lso, hdr, 1);
-		SET_TX_WRB_HDR_BITS(lso_mss, hdr, skb_shinfo(skb)->gso_size);
+		BE_WRB_F_SET(wrb_params->features, LSO, 1);
+		wrb_params->lso_mss = skb_shinfo(skb)->gso_size;
 		if (skb_is_gso_v6(skb) && !lancer_chip(adapter))
-			SET_TX_WRB_HDR_BITS(lso6, hdr, 1);
+			BE_WRB_F_SET(wrb_params->features, LSO6, 1);
 	} else if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		if (skb->encapsulation) {
-			SET_TX_WRB_HDR_BITS(ipcs, hdr, 1);
+			BE_WRB_F_SET(wrb_params->features, IPCS, 1);
 			proto = skb_inner_ip_proto(skb);
 		} else {
 			proto = skb_ip_proto(skb);
 		}
 		if (proto == IPPROTO_TCP)
-			SET_TX_WRB_HDR_BITS(tcpcs, hdr, 1);
+			BE_WRB_F_SET(wrb_params->features, TCPCS, 1);
 		else if (proto == IPPROTO_UDP)
-			SET_TX_WRB_HDR_BITS(udpcs, hdr, 1);
+			BE_WRB_F_SET(wrb_params->features, UDPCS, 1);
 	}
 
 	if (skb_vlan_tag_present(skb)) {
-		SET_TX_WRB_HDR_BITS(vlan, hdr, 1);
-		vlan_tag = be_get_tx_vlan_tag(adapter, skb);
-		SET_TX_WRB_HDR_BITS(vlan_tag, hdr, vlan_tag);
+		BE_WRB_F_SET(wrb_params->features, VLAN, 1);
+		wrb_params->vlan_tag = be_get_tx_vlan_tag(adapter, skb);
 	}
 
-	SET_TX_WRB_HDR_BITS(num_wrb, hdr, wrb_cnt);
-	SET_TX_WRB_HDR_BITS(len, hdr, len);
+	BE_WRB_F_SET(wrb_params->features, CRC, 1);
+}
+
+static void wrb_fill_hdr(struct be_adapter *adapter,
+			 struct be_eth_hdr_wrb *hdr,
+			 struct be_wrb_params *wrb_params,
+			 struct sk_buff *skb)
+{
+	memset(hdr, 0, sizeof(*hdr));
 
-	/* Hack to skip HW VLAN tagging needs evt = 1, compl = 0
-	 * When this hack is not needed, the evt bit is set while ringing DB
+	SET_TX_WRB_HDR_BITS(crc, hdr,
+			    BE_WRB_F_GET(wrb_params->features, CRC));
+	SET_TX_WRB_HDR_BITS(ipcs, hdr,
+			    BE_WRB_F_GET(wrb_params->features, IPCS));
+	SET_TX_WRB_HDR_BITS(tcpcs, hdr,
+			    BE_WRB_F_GET(wrb_params->features, TCPCS));
+	SET_TX_WRB_HDR_BITS(udpcs, hdr,
+			    BE_WRB_F_GET(wrb_params->features, UDPCS));
+
+	SET_TX_WRB_HDR_BITS(lso, hdr,
+			    BE_WRB_F_GET(wrb_params->features, LSO));
+	SET_TX_WRB_HDR_BITS(lso6, hdr,
+			    BE_WRB_F_GET(wrb_params->features, LSO6));
+	SET_TX_WRB_HDR_BITS(lso_mss, hdr, wrb_params->lso_mss);
+
+	/* Hack to skip HW VLAN tagging needs evt = 1, compl = 0. When this
+	 * hack is not needed, the evt bit is set while ringing DB.
 	 */
-	if (skip_hw_vlan)
-		SET_TX_WRB_HDR_BITS(event, hdr, 1);
+	SET_TX_WRB_HDR_BITS(event, hdr,
+			    BE_WRB_F_GET(wrb_params->features, VLAN_SKIP_HW));
+	SET_TX_WRB_HDR_BITS(vlan, hdr,
+			    BE_WRB_F_GET(wrb_params->features, VLAN));
+	SET_TX_WRB_HDR_BITS(vlan_tag, hdr, wrb_params->vlan_tag);
+
+	SET_TX_WRB_HDR_BITS(num_wrb, hdr, skb_wrb_cnt(skb));
+	SET_TX_WRB_HDR_BITS(len, hdr, skb->len);
 }
 
 static void unmap_tx_frag(struct device *dev, struct be_eth_wrb *wrb,
@@ -788,77 +829,124 @@ static void unmap_tx_frag(struct device *dev, struct be_eth_wrb *wrb,
 	}
 }
 
-/* Returns the number of WRBs used up by the skb */
+/* Grab a WRB header for xmit */
+static u16 be_tx_get_wrb_hdr(struct be_tx_obj *txo)
+{
+	u16 head = txo->q.head;
+
+	queue_head_inc(&txo->q);
+	return head;
+}
+
+/* Set up the WRB header for xmit */
+static void be_tx_setup_wrb_hdr(struct be_adapter *adapter,
+				struct be_tx_obj *txo,
+				struct be_wrb_params *wrb_params,
+				struct sk_buff *skb, u16 head)
+{
+	u32 num_frags = skb_wrb_cnt(skb);
+	struct be_queue_info *txq = &txo->q;
+	struct be_eth_hdr_wrb *hdr = queue_index_node(txq, head);
+
+	wrb_fill_hdr(adapter, hdr, wrb_params, skb);
+	be_dws_cpu_to_le(hdr, sizeof(*hdr));
+
+	BUG_ON(txo->sent_skb_list[head]);
+	txo->sent_skb_list[head] = skb;
+	txo->last_req_hdr = head;
+	atomic_add(num_frags, &txq->used);
+	txo->last_req_wrb_cnt = num_frags;
+	txo->pend_wrb_cnt += num_frags;
+}
+
+/* Setup a WRB fragment (buffer descriptor) for xmit */
+static void be_tx_setup_wrb_frag(struct be_tx_obj *txo, dma_addr_t busaddr,
+				 int len)
+{
+	struct be_eth_wrb *wrb;
+	struct be_queue_info *txq = &txo->q;
+
+	wrb = queue_head_node(txq);
+	wrb_fill(wrb, busaddr, len);
+	queue_head_inc(txq);
+}
+
+/* Bring the queue back to the state it was in before be_xmit_enqueue() routine
+ * was invoked. The producer index is restored to the previous packet and the
+ * WRBs of the current packet are unmapped. Invoked to handle tx setup errors.
+ */
+static void be_xmit_restore(struct be_adapter *adapter,
+			    struct be_tx_obj *txo, u16 head, bool map_single,
+			    u32 copied)
+{
+	struct device *dev;
+	struct be_eth_wrb *wrb;
+	struct be_queue_info *txq = &txo->q;
+
+	dev = &adapter->pdev->dev;
+	txq->head = head;
+
+	/* skip the first wrb (hdr); it's not mapped */
+	queue_head_inc(txq);
+	while (copied) {
+		wrb = queue_head_node(txq);
+		unmap_tx_frag(dev, wrb, map_single);
+		map_single = false;
+		copied -= le32_to_cpu(wrb->frag_len);
+		queue_head_inc(txq);
+	}
+
+	txq->head = head;
+}
+
+/* Enqueue the given packet for transmit. This routine allocates WRBs for the
+ * packet, dma maps the packet buffers and sets up the WRBs. Returns the number
+ * of WRBs used up by the packet.
+ */
 static u32 be_xmit_enqueue(struct be_adapter *adapter, struct be_tx_obj *txo,
-			   struct sk_buff *skb, bool skip_hw_vlan)
+			   struct sk_buff *skb,
+			   struct be_wrb_params *wrb_params)
 {
 	u32 i, copied = 0, wrb_cnt = skb_wrb_cnt(skb);
 	struct device *dev = &adapter->pdev->dev;
 	struct be_queue_info *txq = &txo->q;
-	struct be_eth_hdr_wrb *hdr;
 	bool map_single = false;
-	struct be_eth_wrb *wrb;
-	dma_addr_t busaddr;
 	u16 head = txq->head;
+	dma_addr_t busaddr;
+	int len;
 
-	hdr = queue_head_node(txq);
-	wrb_fill_hdr(adapter, hdr, skb, wrb_cnt, skb->len, skip_hw_vlan);
-	be_dws_cpu_to_le(hdr, sizeof(*hdr));
-
-	queue_head_inc(txq);
+	head = be_tx_get_wrb_hdr(txo);
 
 	if (skb->len > skb->data_len) {
-		int len = skb_headlen(skb);
+		len = skb_headlen(skb);
 
 		busaddr = dma_map_single(dev, skb->data, len, DMA_TO_DEVICE);
 		if (dma_mapping_error(dev, busaddr))
 			goto dma_err;
 		map_single = true;
-		wrb = queue_head_node(txq);
-		wrb_fill(wrb, busaddr, len);
-		queue_head_inc(txq);
+		be_tx_setup_wrb_frag(txo, busaddr, len);
 		copied += len;
 	}
 
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 		const struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i];
+		len = skb_frag_size(frag);
 
-		busaddr = skb_frag_dma_map(dev, frag, 0,
-					   skb_frag_size(frag), DMA_TO_DEVICE);
+		busaddr = skb_frag_dma_map(dev, frag, 0, len, DMA_TO_DEVICE);
 		if (dma_mapping_error(dev, busaddr))
 			goto dma_err;
-		wrb = queue_head_node(txq);
-		wrb_fill(wrb, busaddr, skb_frag_size(frag));
-		queue_head_inc(txq);
-		copied += skb_frag_size(frag);
+		be_tx_setup_wrb_frag(txo, busaddr, len);
+		copied += len;
 	}
 
-	BUG_ON(txo->sent_skb_list[head]);
-	txo->sent_skb_list[head] = skb;
-	txo->last_req_hdr = head;
-	atomic_add(wrb_cnt, &txq->used);
-	txo->last_req_wrb_cnt = wrb_cnt;
-	txo->pend_wrb_cnt += wrb_cnt;
+	be_tx_setup_wrb_hdr(adapter, txo, wrb_params, skb, head);
 
 	be_tx_stats_update(txo, skb);
 	return wrb_cnt;
 
 dma_err:
-	/* Bring the queue back to the state it was in before this
-	 * routine was invoked.
-	 */
-	txq->head = head;
-	/* skip the first wrb (hdr); it's not mapped */
-	queue_head_inc(txq);
-	while (copied) {
-		wrb = queue_head_node(txq);
-		unmap_tx_frag(dev, wrb, map_single);
-		map_single = false;
-		copied -= le32_to_cpu(wrb->frag_len);
-		adapter->drv_stats.dma_map_errors++;
-		queue_head_inc(txq);
-	}
-	txq->head = head;
+	adapter->drv_stats.dma_map_errors++;
+	be_xmit_restore(adapter, txo, head, map_single, copied);
 	return 0;
 }
 
@@ -869,7 +957,8 @@ static inline int qnq_async_evt_rcvd(struct be_adapter *adapter)
 
 static struct sk_buff *be_insert_vlan_in_pkt(struct be_adapter *adapter,
 					     struct sk_buff *skb,
-					     bool *skip_hw_vlan)
+					     struct be_wrb_params
+					     *wrb_params)
 {
 	u16 vlan_tag = 0;
 
@@ -886,8 +975,7 @@ static struct sk_buff *be_insert_vlan_in_pkt(struct be_adapter *adapter,
 		/* f/w workaround to set skip_hw_vlan = 1, informs the F/W to
 		 * skip VLAN insertion
 		 */
-		if (skip_hw_vlan)
-			*skip_hw_vlan = true;
+		BE_WRB_F_SET(wrb_params->features, VLAN_SKIP_HW, 1);
 	}
 
 	if (vlan_tag) {
@@ -905,8 +993,7 @@ static struct sk_buff *be_insert_vlan_in_pkt(struct be_adapter *adapter,
 						vlan_tag);
 		if (unlikely(!skb))
 			return skb;
-		if (skip_hw_vlan)
-			*skip_hw_vlan = true;
+		BE_WRB_F_SET(wrb_params->features, VLAN_SKIP_HW, 1);
 	}
 
 	return skb;
@@ -946,7 +1033,8 @@ static int be_ipv6_tx_stall_chk(struct be_adapter *adapter, struct sk_buff *skb)
 
 static struct sk_buff *be_lancer_xmit_workarounds(struct be_adapter *adapter,
 						  struct sk_buff *skb,
-						  bool *skip_hw_vlan)
+						  struct be_wrb_params
+						  *wrb_params)
 {
 	struct vlan_ethhdr *veh = (struct vlan_ethhdr *)skb->data;
 	unsigned int eth_hdr_len;
@@ -970,7 +1058,7 @@ static struct sk_buff *be_lancer_xmit_workarounds(struct be_adapter *adapter,
 	 */
 	if (be_pvid_tagging_enabled(adapter) &&
 	    veh->h_vlan_proto == htons(ETH_P_8021Q))
-		*skip_hw_vlan = true;
+		BE_WRB_F_SET(wrb_params->features, VLAN_SKIP_HW, 1);
 
 	/* HW has a bug wherein it will calculate CSUM for VLAN
 	 * pkts even though it is disabled.
@@ -978,7 +1066,7 @@ static struct sk_buff *be_lancer_xmit_workarounds(struct be_adapter *adapter,
 	 */
 	if (skb->ip_summed != CHECKSUM_PARTIAL &&
 	    skb_vlan_tag_present(skb)) {
-		skb = be_insert_vlan_in_pkt(adapter, skb, skip_hw_vlan);
+		skb = be_insert_vlan_in_pkt(adapter, skb, wrb_params);
 		if (unlikely(!skb))
 			goto err;
 	}
@@ -1000,7 +1088,7 @@ static struct sk_buff *be_lancer_xmit_workarounds(struct be_adapter *adapter,
 	 */
 	if (be_ipv6_tx_stall_chk(adapter, skb) &&
 	    be_vlan_tag_tx_chk(adapter, skb)) {
-		skb = be_insert_vlan_in_pkt(adapter, skb, skip_hw_vlan);
+		skb = be_insert_vlan_in_pkt(adapter, skb, wrb_params);
 		if (unlikely(!skb))
 			goto err;
 	}
@@ -1014,7 +1102,7 @@ err:
 
 static struct sk_buff *be_xmit_workarounds(struct be_adapter *adapter,
 					   struct sk_buff *skb,
-					   bool *skip_hw_vlan)
+					   struct be_wrb_params *wrb_params)
 {
 	/* Lancer, SH-R ASICs have a bug wherein Packets that are 32 bytes or
 	 * less may cause a transmit stall on that port. So the work-around is
@@ -1026,7 +1114,7 @@ static struct sk_buff *be_xmit_workarounds(struct be_adapter *adapter,
 	}
 
 	if (BEx_chip(adapter) || lancer_chip(adapter)) {
-		skb = be_lancer_xmit_workarounds(adapter, skb, skip_hw_vlan);
+		skb = be_lancer_xmit_workarounds(adapter, skb, wrb_params);
 		if (!skb)
 			return NULL;
 	}
@@ -1060,24 +1148,26 @@ static void be_xmit_flush(struct be_adapter *adapter, struct be_tx_obj *txo)
 
 static netdev_tx_t be_xmit(struct sk_buff *skb, struct net_device *netdev)
 {
-	bool skip_hw_vlan = false, flush = !skb->xmit_more;
 	struct be_adapter *adapter = netdev_priv(netdev);
 	u16 q_idx = skb_get_queue_mapping(skb);
 	struct be_tx_obj *txo = &adapter->tx_obj[q_idx];
-	struct be_queue_info *txq = &txo->q;
+	struct be_wrb_params wrb_params = { 0 };
+	bool flush = !skb->xmit_more;
 	u16 wrb_cnt;
 
-	skb = be_xmit_workarounds(adapter, skb, &skip_hw_vlan);
+	skb = be_xmit_workarounds(adapter, skb, &wrb_params);
 	if (unlikely(!skb))
 		goto drop;
 
-	wrb_cnt = be_xmit_enqueue(adapter, txo, skb, skip_hw_vlan);
+	be_get_wrb_params_from_skb(adapter, skb, &wrb_params);
+
+	wrb_cnt = be_xmit_enqueue(adapter, txo, skb, &wrb_params);
 	if (unlikely(!wrb_cnt)) {
 		dev_kfree_skb_any(skb);
 		goto drop;
 	}
 
-	if ((atomic_read(&txq->used) + BE_MAX_TX_FRAG_COUNT) >= txq->len) {
+	if (be_is_txq_full(txo)) {
 		netif_stop_subqueue(netdev, q_idx);
 		tx_stats(txo)->tx_stops++;
 	}
@@ -1171,7 +1261,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 +1470,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 +1540,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;
 }
 
@@ -1981,7 +2122,7 @@ static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp, u32 frags_needed)
 		if (rxo->rx_post_starved)
 			rxo->rx_post_starved = false;
 		do {
-			notify = min(256u, posted);
+			notify = min(MAX_NUM_POST_ERX_DB, posted);
 			be_rxq_notify(adapter, rxq->id, notify);
 			posted -= notify;
 		} while (posted);
@@ -1991,18 +2132,23 @@ static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp, u32 frags_needed)
 	}
 }
 
-static struct be_eth_tx_compl *be_tx_compl_get(struct be_queue_info *tx_cq)
+static struct be_tx_compl_info *be_tx_compl_get(struct be_tx_obj *txo)
 {
-	struct be_eth_tx_compl *txcp = queue_tail_node(tx_cq);
+	struct be_queue_info *tx_cq = &txo->cq;
+	struct be_tx_compl_info *txcp = &txo->txcp;
+	struct be_eth_tx_compl *compl = queue_tail_node(tx_cq);
 
-	if (txcp->dw[offsetof(struct amap_eth_tx_compl, valid) / 32] == 0)
+	if (compl->dw[offsetof(struct amap_eth_tx_compl, valid) / 32] == 0)
 		return NULL;
 
+	/* Ensure load ordering of valid bit dword and other dwords below */
 	rmb();
-	be_dws_le_to_cpu(txcp, sizeof(*txcp));
+	be_dws_le_to_cpu(compl, sizeof(*compl));
 
-	txcp->dw[offsetof(struct amap_eth_tx_compl, valid) / 32] = 0;
+	txcp->status = GET_TX_COMPL_BITS(status, compl);
+	txcp->end_index = GET_TX_COMPL_BITS(wrb_index, compl);
 
+	compl->dw[offsetof(struct amap_eth_tx_compl, valid) / 32] = 0;
 	queue_tail_inc(tx_cq);
 	return txcp;
 }
@@ -2123,9 +2269,9 @@ static void be_tx_compl_clean(struct be_adapter *adapter)
 {
 	u16 end_idx, notified_idx, cmpl = 0, timeo = 0, num_wrbs = 0;
 	struct device *dev = &adapter->pdev->dev;
-	struct be_tx_obj *txo;
+	struct be_tx_compl_info *txcp;
 	struct be_queue_info *txq;
-	struct be_eth_tx_compl *txcp;
+	struct be_tx_obj *txo;
 	int i, pending_txqs;
 
 	/* Stop polling for compls when HW has been silent for 10ms */
@@ -2136,10 +2282,10 @@ static void be_tx_compl_clean(struct be_adapter *adapter)
 			cmpl = 0;
 			num_wrbs = 0;
 			txq = &txo->q;
-			while ((txcp = be_tx_compl_get(&txo->cq))) {
-				end_idx = GET_TX_COMPL_BITS(wrb_index, txcp);
-				num_wrbs += be_tx_compl_process(adapter, txo,
-								end_idx);
+			while ((txcp = be_tx_compl_get(txo))) {
+				num_wrbs +=
+					be_tx_compl_process(adapter, txo,
+							    txcp->end_index);
 				cmpl++;
 			}
 			if (cmpl) {
@@ -2147,7 +2293,7 @@ static void be_tx_compl_clean(struct be_adapter *adapter)
 				atomic_sub(num_wrbs, &txq->used);
 				timeo = 0;
 			}
-			if (atomic_read(&txq->used) == txo->pend_wrb_cnt)
+			if (!be_is_tx_compl_pending(txo))
 				pending_txqs--;
 		}
 
@@ -2196,6 +2342,7 @@ static void be_evt_queues_destroy(struct be_adapter *adapter)
 			napi_hash_del(&eqo->napi);
 			netif_napi_del(&eqo->napi);
 		}
+		free_cpumask_var(eqo->affinity_mask);
 		be_queue_free(adapter, &eqo->q);
 	}
 }
@@ -2211,6 +2358,11 @@ static int be_evt_queues_create(struct be_adapter *adapter)
 				    adapter->cfg_num_qs);
 
 	for_all_evt_queues(adapter, eqo, i) {
+		if (!zalloc_cpumask_var(&eqo->affinity_mask, GFP_KERNEL))
+			return -ENOMEM;
+		cpumask_set_cpu_local_first(i, dev_to_node(&adapter->pdev->dev),
+					    eqo->affinity_mask);
+
 		netif_napi_add(adapter->netdev, &eqo->napi, be_poll,
 			       BE_NAPI_WEIGHT);
 		napi_hash_add(&eqo->napi);
@@ -2302,8 +2454,9 @@ static void be_tx_queues_destroy(struct be_adapter *adapter)
 
 static int be_tx_qs_create(struct be_adapter *adapter)
 {
-	struct be_queue_info *cq, *eq;
+	struct be_queue_info *cq;
 	struct be_tx_obj *txo;
+	struct be_eq_obj *eqo;
 	int status, i;
 
 	adapter->num_tx_qs = min(adapter->num_evt_qs, be_max_txqs(adapter));
@@ -2321,8 +2474,8 @@ static int be_tx_qs_create(struct be_adapter *adapter)
 		/* If num_evt_qs is less than num_tx_qs, then more than
 		 * one txq share an eq
 		 */
-		eq = &adapter->eq_obj[i % adapter->num_evt_qs].q;
-		status = be_cmd_cq_create(adapter, cq, eq, false, 3);
+		eqo = &adapter->eq_obj[i % adapter->num_evt_qs];
+		status = be_cmd_cq_create(adapter, cq, &eqo->q, false, 3);
 		if (status)
 			return status;
 
@@ -2334,6 +2487,9 @@ static int be_tx_qs_create(struct be_adapter *adapter)
 		status = be_cmd_txq_create(adapter, txo);
 		if (status)
 			return status;
+
+		netif_set_xps_queue(adapter->netdev, eqo->affinity_mask,
+				    eqo->idx);
 	}
 
 	dev_info(&adapter->pdev->dev, "created %d TX queue(s)\n",
@@ -2362,13 +2518,19 @@ static int be_rx_cqs_create(struct be_adapter *adapter)
 	int rc, i;
 
 	/* We can create as many RSS rings as there are EQs. */
-	adapter->num_rx_qs = adapter->num_evt_qs;
+	adapter->num_rss_qs = adapter->num_evt_qs;
+
+	/* We'll use RSS only if atleast 2 RSS rings are supported. */
+	if (adapter->num_rss_qs <= 1)
+		adapter->num_rss_qs = 0;
 
-	/* We'll use RSS only if atleast 2 RSS rings are supported.
-	 * When RSS is used, we'll need a default RXQ for non-IP traffic.
+	adapter->num_rx_qs = adapter->num_rss_qs + adapter->need_def_rxq;
+
+	/* When the interface is not capable of RSS rings (and there is no
+	 * need to create a default RXQ) we'll still need one RXQ
 	 */
-	if (adapter->num_rx_qs > 1)
-		adapter->num_rx_qs++;
+	if (adapter->num_rx_qs == 0)
+		adapter->num_rx_qs = 1;
 
 	adapter->big_page_size = (1 << get_order(rx_frag_size)) * PAGE_SIZE;
 	for_all_rx_queues(adapter, rxo, i) {
@@ -2387,8 +2549,7 @@ static int be_rx_cqs_create(struct be_adapter *adapter)
 	}
 
 	dev_info(&adapter->pdev->dev,
-		 "created %d RSS queue(s) and 1 default RX queue\n",
-		 adapter->num_rx_qs - 1);
+		 "created %d RX queue(s)\n", adapter->num_rx_qs);
 	return 0;
 }
 
@@ -2498,7 +2659,7 @@ loop_continue:
 	return work_done;
 }
 
-static inline void be_update_tx_err(struct be_tx_obj *txo, u32 status)
+static inline void be_update_tx_err(struct be_tx_obj *txo, u8 status)
 {
 	switch (status) {
 	case BE_TX_COMP_HDR_PARSE_ERR:
@@ -2513,7 +2674,7 @@ static inline void be_update_tx_err(struct be_tx_obj *txo, u32 status)
 	}
 }
 
-static inline void lancer_update_tx_err(struct be_tx_obj *txo, u32 status)
+static inline void lancer_update_tx_err(struct be_tx_obj *txo, u8 status)
 {
 	switch (status) {
 	case LANCER_TX_COMP_LSO_ERR:
@@ -2538,22 +2699,18 @@ static inline void lancer_update_tx_err(struct be_tx_obj *txo, u32 status)
 static void be_process_tx(struct be_adapter *adapter, struct be_tx_obj *txo,
 			  int idx)
 {
-	struct be_eth_tx_compl *txcp;
 	int num_wrbs = 0, work_done = 0;
-	u32 compl_status;
-	u16 last_idx;
+	struct be_tx_compl_info *txcp;
 
-	while ((txcp = be_tx_compl_get(&txo->cq))) {
-		last_idx = GET_TX_COMPL_BITS(wrb_index, txcp);
-		num_wrbs += be_tx_compl_process(adapter, txo, last_idx);
+	while ((txcp = be_tx_compl_get(txo))) {
+		num_wrbs += be_tx_compl_process(adapter, txo, txcp->end_index);
 		work_done++;
 
-		compl_status = GET_TX_COMPL_BITS(status, txcp);
-		if (compl_status) {
+		if (txcp->status) {
 			if (lancer_chip(adapter))
-				lancer_update_tx_err(txo, compl_status);
+				lancer_update_tx_err(txo, txcp->status);
 			else
-				be_update_tx_err(txo, compl_status);
+				be_update_tx_err(txo, txcp->status);
 		}
 	}
 
@@ -2564,7 +2721,7 @@ static void be_process_tx(struct be_adapter *adapter, struct be_tx_obj *txo,
 		/* As Tx wrbs have been freed up, wake up netdev queue
 		 * if it was stopped due to lack of tx wrbs.  */
 		if (__netif_subqueue_stopped(adapter->netdev, idx) &&
-		    atomic_read(&txo->q.used) < txo->q.len / 2) {
+		    be_can_txq_wake(txo)) {
 			netif_wake_subqueue(adapter->netdev, idx);
 		}
 
@@ -2756,12 +2913,12 @@ void be_detect_error(struct be_adapter *adapter)
 			sliport_err2 = ioread32(adapter->db +
 						SLIPORT_ERROR2_OFFSET);
 			adapter->hw_error = true;
+			error_detected = true;
 			/* Do not log error messages if its a FW reset */
 			if (sliport_err1 == SLIPORT_ERROR_FW_RESET1 &&
 			    sliport_err2 == SLIPORT_ERROR_FW_RESET2) {
 				dev_info(dev, "Firmware update in progress\n");
 			} else {
-				error_detected = true;
 				dev_err(dev, "Error detected in the card\n");
 				dev_err(dev, "ERR: sliport status 0x%x\n",
 					sliport_status);
@@ -2772,14 +2929,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);
@@ -2883,6 +3038,8 @@ static int be_msix_register(struct be_adapter *adapter)
 		status = request_irq(vec, be_msix, 0, eqo->desc, eqo);
 		if (status)
 			goto err_msix;
+
+		irq_set_affinity_hint(vec, eqo->affinity_mask);
 	}
 
 	return 0;
@@ -2927,7 +3084,7 @@ static void be_irq_unregister(struct be_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 	struct be_eq_obj *eqo;
-	int i;
+	int i, vec;
 
 	if (!adapter->isr_registered)
 		return;
@@ -2939,8 +3096,11 @@ static void be_irq_unregister(struct be_adapter *adapter)
 	}
 
 	/* MSIx */
-	for_all_evt_queues(adapter, eqo, i)
-		free_irq(be_msix_vec_get(adapter, eqo), eqo);
+	for_all_evt_queues(adapter, eqo, i) {
+		vec = be_msix_vec_get(adapter, eqo);
+		irq_set_affinity_hint(vec, NULL);
+		free_irq(vec, eqo);
+	}
 
 done:
 	adapter->isr_registered = false;
@@ -3022,12 +3182,14 @@ static int be_rx_qs_create(struct be_adapter *adapter)
 			return rc;
 	}
 
-	/* The FW would like the default RXQ to be created first */
-	rxo = default_rxo(adapter);
-	rc = be_cmd_rxq_create(adapter, &rxo->q, rxo->cq.id, rx_frag_size,
-			       adapter->if_handle, false, &rxo->rss_id);
-	if (rc)
-		return rc;
+	if (adapter->need_def_rxq || !adapter->num_rss_qs) {
+		rxo = default_rxo(adapter);
+		rc = be_cmd_rxq_create(adapter, &rxo->q, rxo->cq.id,
+				       rx_frag_size, adapter->if_handle,
+				       false, &rxo->rss_id);
+		if (rc)
+			return rc;
+	}
 
 	for_all_rss_queues(adapter, rxo, i) {
 		rc = be_cmd_rxq_create(adapter, &rxo->q, rxo->cq.id,
@@ -3038,8 +3200,7 @@ static int be_rx_qs_create(struct be_adapter *adapter)
 	}
 
 	if (be_multi_rxq(adapter)) {
-		for (j = 0; j < RSS_INDIR_TABLE_LEN;
-			j += adapter->num_rx_qs - 1) {
+		for (j = 0; j < RSS_INDIR_TABLE_LEN; j += adapter->num_rss_qs) {
 			for_all_rss_queues(adapter, rxo, i) {
 				if ((j + i) >= RSS_INDIR_TABLE_LEN)
 					break;
@@ -3130,7 +3291,7 @@ static int be_setup_wol(struct be_adapter *adapter, bool enable)
 	int status = 0;
 	u8 mac[ETH_ALEN];
 
-	memset(mac, 0, ETH_ALEN);
+	eth_zero_addr(mac);
 
 	cmd.size = sizeof(struct be_cmd_req_acpi_wol_magic_config);
 	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
@@ -3275,6 +3436,14 @@ static void be_cancel_worker(struct be_adapter *adapter)
 	}
 }
 
+static void be_cancel_err_detection(struct be_adapter *adapter)
+{
+	if (adapter->flags & BE_FLAGS_ERR_DETECTION_SCHEDULED) {
+		cancel_delayed_work_sync(&adapter->be_err_detection_work);
+		adapter->flags &= ~BE_FLAGS_ERR_DETECTION_SCHEDULED;
+	}
+}
+
 static void be_mac_clear(struct be_adapter *adapter)
 {
 	if (adapter->pmac_id) {
@@ -3306,8 +3475,39 @@ static void be_disable_vxlan_offloads(struct be_adapter *adapter)
 }
 #endif
 
+static u16 be_calculate_vf_qs(struct be_adapter *adapter, u16 num_vfs)
+{
+	struct be_resources res = adapter->pool_res;
+	u16 num_vf_qs = 1;
+
+	/* Distribute the queue resources equally among the PF and it's VFs
+	 * Do not distribute queue resources in multi-channel configuration.
+	 */
+	if (num_vfs && !be_is_mc(adapter)) {
+		/* If number of VFs requested is 8 less than max supported,
+		 * assign 8 queue pairs to the PF and divide the remaining
+		 * resources evenly among the VFs
+		 */
+		if (num_vfs < (be_max_vfs(adapter) - 8))
+			num_vf_qs = (res.max_rss_qs - 8) / num_vfs;
+		else
+			num_vf_qs = res.max_rss_qs / num_vfs;
+
+		/* Skyhawk-R chip supports only MAX_RSS_IFACES RSS capable
+		 * interfaces per port. Provide RSS on VFs, only if number
+		 * of VFs requested is less than MAX_RSS_IFACES limit.
+		 */
+		if (num_vfs >= MAX_RSS_IFACES)
+			num_vf_qs = 1;
+	}
+	return num_vf_qs;
+}
+
 static int be_clear(struct be_adapter *adapter)
 {
+	struct pci_dev *pdev = adapter->pdev;
+	u16 num_vf_qs;
+
 	be_cancel_worker(adapter);
 
 	if (sriov_enabled(adapter))
@@ -3316,9 +3516,14 @@ static int be_clear(struct be_adapter *adapter)
 	/* Re-configure FW to distribute resources evenly across max-supported
 	 * number of VFs, only when VFs are not already enabled.
 	 */
-	if (be_physfn(adapter) && !pci_vfs_assigned(adapter->pdev))
+	if (skyhawk_chip(adapter) && be_physfn(adapter) &&
+	    !pci_vfs_assigned(pdev)) {
+		num_vf_qs = be_calculate_vf_qs(adapter,
+					       pci_sriov_get_totalvfs(pdev));
 		be_cmd_set_sriov_config(adapter, adapter->pool_res,
-					pci_sriov_get_totalvfs(adapter->pdev));
+					pci_sriov_get_totalvfs(pdev),
+					num_vf_qs);
+	}
 
 #ifdef CONFIG_BE2NET_VXLAN
 	be_disable_vxlan_offloads(adapter);
@@ -3339,18 +3544,14 @@ 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 |
-		   BE_IF_FLAGS_RSS;
+		   BE_IF_FLAGS_RSS | BE_IF_FLAGS_DEFQ_RSS;
 
 	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)
@@ -3367,9 +3568,15 @@ static int be_vfs_if_create(struct be_adapter *adapter)
 	for_all_vfs(adapter, vf_cfg, vf) {
 		if (!BE3_chip(adapter)) {
 			status = be_cmd_get_profile_config(adapter, &res,
+							   RESOURCE_LIMITS,
 							   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 +3610,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 +3639,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 */
@@ -3533,7 +3742,8 @@ static void BEx_get_resources(struct be_adapter *adapter,
 		/* On a SuperNIC profile, the driver needs to use the
 		 * GET_PROFILE_CONFIG cmd to query the per-function TXQ limits
 		 */
-		be_cmd_get_profile_config(adapter, &super_nic_res, 0);
+		be_cmd_get_profile_config(adapter, &super_nic_res,
+					  RESOURCE_LIMITS, 0);
 		/* Some old versions of BE3 FW don't report max_tx_qs value */
 		res->max_tx_qs = super_nic_res.max_tx_qs ? : BE3_MAX_TX_QS;
 	} else {
@@ -3553,6 +3763,7 @@ static void BEx_get_resources(struct be_adapter *adapter,
 		res->max_evt_qs = 1;
 
 	res->if_cap_flags = BE_IF_CAP_FLAGS_WANT;
+	res->if_cap_flags &= ~BE_IF_FLAGS_DEFQ_RSS;
 	if (!(adapter->function_caps & BE_FUNCTION_CAPS_RSS))
 		res->if_cap_flags &= ~BE_IF_FLAGS_RSS;
 }
@@ -3572,13 +3783,12 @@ static void be_setup_init(struct be_adapter *adapter)
 
 static int be_get_sriov_config(struct be_adapter *adapter)
 {
-	struct device *dev = &adapter->pdev->dev;
 	struct be_resources res = {0};
 	int max_vfs, old_vfs;
 
-	/* Some old versions of BE3 FW don't report max_vfs value */
-	be_cmd_get_profile_config(adapter, &res, 0);
+	be_cmd_get_profile_config(adapter, &res, RESOURCE_LIMITS, 0);
 
+	/* Some old versions of BE3 FW don't report max_vfs value */
 	if (BE3_chip(adapter) && !res.max_vfs) {
 		max_vfs = pci_sriov_get_totalvfs(adapter->pdev);
 		res.max_vfs = max_vfs > 0 ? min(MAX_VFS, max_vfs) : 0;
@@ -3586,35 +3796,49 @@ static int be_get_sriov_config(struct be_adapter *adapter)
 
 	adapter->pool_res = res;
 
-	if (!be_max_vfs(adapter)) {
-		if (num_vfs)
-			dev_warn(dev, "SRIOV is disabled. Ignoring num_vfs\n");
-		adapter->num_vfs = 0;
-		return 0;
-	}
-
-	pci_sriov_set_totalvfs(adapter->pdev, be_max_vfs(adapter));
-
-	/* validate num_vfs module param */
+	/* If during previous unload of the driver, the VFs were not disabled,
+	 * then we cannot rely on the PF POOL limits for the TotalVFs value.
+	 * Instead use the TotalVFs value stored in the pci-dev struct.
+	 */
 	old_vfs = pci_num_vf(adapter->pdev);
 	if (old_vfs) {
-		dev_info(dev, "%d VFs are already enabled\n", old_vfs);
-		if (old_vfs != num_vfs)
-			dev_warn(dev, "Ignoring num_vfs=%d setting\n", num_vfs);
+		dev_info(&adapter->pdev->dev, "%d VFs are already enabled\n",
+			 old_vfs);
+
+		adapter->pool_res.max_vfs =
+			pci_sriov_get_totalvfs(adapter->pdev);
 		adapter->num_vfs = old_vfs;
-	} else {
-		if (num_vfs > be_max_vfs(adapter)) {
-			dev_info(dev, "Resources unavailable to init %d VFs\n",
-				 num_vfs);
-			dev_info(dev, "Limiting to %d VFs\n",
-				 be_max_vfs(adapter));
-		}
-		adapter->num_vfs = min_t(u16, num_vfs, be_max_vfs(adapter));
 	}
 
 	return 0;
 }
 
+static void be_alloc_sriov_res(struct be_adapter *adapter)
+{
+	int old_vfs = pci_num_vf(adapter->pdev);
+	u16 num_vf_qs;
+	int status;
+
+	be_get_sriov_config(adapter);
+
+	if (!old_vfs)
+		pci_sriov_set_totalvfs(adapter->pdev, be_max_vfs(adapter));
+
+	/* When the HW is in SRIOV capable configuration, the PF-pool
+	 * resources are given to PF during driver load, if there are no
+	 * old VFs. This facility is not available in BE3 FW.
+	 * Also, this is done by FW in Lancer chip.
+	 */
+	if (skyhawk_chip(adapter) && be_max_vfs(adapter) && !old_vfs) {
+		num_vf_qs = be_calculate_vf_qs(adapter, 0);
+		status = be_cmd_set_sriov_config(adapter, adapter->pool_res, 0,
+						 num_vf_qs);
+		if (status)
+			dev_err(&adapter->pdev->dev,
+				"Failed to optimize SRIOV resources\n");
+	}
+}
+
 static int be_get_resources(struct be_adapter *adapter)
 {
 	struct device *dev = &adapter->pdev->dev;
@@ -3635,12 +3859,23 @@ static int be_get_resources(struct be_adapter *adapter)
 		if (status)
 			return status;
 
+		/* If a deafault RXQ must be created, we'll use up one RSSQ*/
+		if (res.max_rss_qs && res.max_rss_qs == res.max_rx_qs &&
+		    !(res.if_cap_flags & BE_IF_FLAGS_DEFQ_RSS))
+			res.max_rss_qs -= 1;
+
 		/* If RoCE may be enabled stash away half the EQs for RoCE */
 		if (be_roce_supported(adapter))
 			res.max_evt_qs /= 2;
 		adapter->res = res;
 	}
 
+	/* If FW supports RSS default queue, then skip creating non-RSS
+	 * queue for non-IP traffic.
+	 */
+	adapter->need_def_rxq = (be_if_cap_flags(adapter) &
+				 BE_IF_FLAGS_DEFQ_RSS) ? 0 : 1;
+
 	dev_info(dev, "Max: txqs %d, rxqs %d, rss %d, eqs %d, vfs %d\n",
 		 be_max_txqs(adapter), be_max_rxqs(adapter),
 		 be_max_rss(adapter), be_max_eqs(adapter),
@@ -3649,47 +3884,33 @@ static int be_get_resources(struct be_adapter *adapter)
 		 be_max_uc(adapter), be_max_mc(adapter),
 		 be_max_vlans(adapter));
 
+	/* Sanitize cfg_num_qs based on HW and platform limits */
+	adapter->cfg_num_qs = min_t(u16, netif_get_num_default_rss_queues(),
+				    be_max_qs(adapter));
 	return 0;
 }
 
-static void be_sriov_config(struct be_adapter *adapter)
-{
-	struct device *dev = &adapter->pdev->dev;
-	int status;
-
-	status = be_get_sriov_config(adapter);
-	if (status) {
-		dev_err(dev, "Failed to query SR-IOV configuration\n");
-		dev_err(dev, "SR-IOV cannot be enabled\n");
-		return;
-	}
-
-	/* When the HW is in SRIOV capable configuration, the PF-pool
-	 * resources are equally distributed across the max-number of
-	 * VFs. The user may request only a subset of the max-vfs to be
-	 * enabled. Based on num_vfs, redistribute the resources across
-	 * num_vfs so that each VF will have access to more number of
-	 * resources. This facility is not available in BE3 FW.
-	 * Also, this is done by FW in Lancer chip.
-	 */
-	if (be_max_vfs(adapter) && !pci_num_vf(adapter->pdev)) {
-		status = be_cmd_set_sriov_config(adapter,
-						 adapter->pool_res,
-						 adapter->num_vfs);
-		if (status)
-			dev_err(dev, "Failed to optimize SR-IOV resources\n");
-	}
-}
-
 static int be_get_config(struct be_adapter *adapter)
 {
+	int status, level;
 	u16 profile_id;
-	int status;
+
+	status = be_cmd_get_cntl_attributes(adapter);
+	if (status)
+		return status;
 
 	status = be_cmd_query_fw_cfg(adapter);
 	if (status)
 		return status;
 
+	if (BEx_chip(adapter)) {
+		level = be_cmd_get_fw_log_level(adapter);
+		adapter->msg_enable =
+			level <= FW_LOG_LEVEL_DEFAULT ? NETIF_MSG_HW : 0;
+	}
+
+	be_cmd_get_acpi_wol_cap(adapter);
+
 	be_cmd_query_port_name(adapter);
 
 	if (be_physfn(adapter)) {
@@ -3699,9 +3920,6 @@ static int be_get_config(struct be_adapter *adapter)
 				 "Using profile 0x%x\n", profile_id);
 	}
 
-	if (!BE2_chip(adapter) && be_physfn(adapter))
-		be_sriov_config(adapter);
-
 	status = be_get_resources(adapter);
 	if (status)
 		return status;
@@ -3711,9 +3929,6 @@ static int be_get_config(struct be_adapter *adapter)
 	if (!adapter->pmac_id)
 		return -ENOMEM;
 
-	/* Sanitize cfg_num_qs based on HW and platform limits */
-	adapter->cfg_num_qs = min(adapter->cfg_num_qs, be_max_qs(adapter));
-
 	return 0;
 }
 
@@ -3747,6 +3962,13 @@ static void be_schedule_worker(struct be_adapter *adapter)
 	adapter->flags |= BE_FLAGS_WORKER_SCHEDULED;
 }
 
+static void be_schedule_err_detection(struct be_adapter *adapter)
+{
+	schedule_delayed_work(&adapter->be_err_detection_work,
+			      msecs_to_jiffies(1000));
+	adapter->flags |= BE_FLAGS_ERR_DETECTION_SCHEDULED;
+}
+
 static int be_setup_queues(struct be_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
@@ -3829,16 +4051,61 @@ static inline int fw_major_num(const char *fw_ver)
 	return fw_major;
 }
 
+/* If any VFs are already enabled don't FLR the PF */
+static bool be_reset_required(struct be_adapter *adapter)
+{
+	return pci_num_vf(adapter->pdev) ? false : true;
+}
+
+/* Wait for the FW to be ready and perform the required initialization */
+static int be_func_init(struct be_adapter *adapter)
+{
+	int status;
+
+	status = be_fw_wait_ready(adapter);
+	if (status)
+		return status;
+
+	if (be_reset_required(adapter)) {
+		status = be_cmd_reset_function(adapter);
+		if (status)
+			return status;
+
+		/* Wait for interrupts to quiesce after an FLR */
+		msleep(100);
+
+		/* We can clear all errors when function reset succeeds */
+		be_clear_all_error(adapter);
+	}
+
+	/* Tell FW we're ready to fire cmds */
+	status = be_cmd_fw_init(adapter);
+	if (status)
+		return status;
+
+	/* Allow interrupts for other ULPs running on NIC function */
+	be_intr_set(adapter, true);
+
+	return 0;
+}
+
 static int be_setup(struct be_adapter *adapter)
 {
 	struct device *dev = &adapter->pdev->dev;
 	int status;
 
+	status = be_func_init(adapter);
+	if (status)
+		return status;
+
 	be_setup_init(adapter);
 
 	if (!lancer_chip(adapter))
 		be_cmd_req_native_mode(adapter);
 
+	if (!BE2_chip(adapter) && be_physfn(adapter))
+		be_alloc_sriov_res(adapter);
+
 	status = be_get_config(adapter);
 	if (status)
 		goto err;
@@ -3879,8 +4146,6 @@ static int be_setup(struct be_adapter *adapter)
 
 	be_set_rx_mode(adapter->netdev);
 
-	be_cmd_get_acpi_wol_cap(adapter);
-
 	status = be_cmd_set_flow_control(adapter, adapter->tx_fc,
 					 adapter->rx_fc);
 	if (status)
@@ -4790,6 +5055,142 @@ static void be_netdev_init(struct net_device *netdev)
 	netdev->ethtool_ops = &be_ethtool_ops;
 }
 
+static void be_cleanup(struct be_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+
+	rtnl_lock();
+	netif_device_detach(netdev);
+	if (netif_running(netdev))
+		be_close(netdev);
+	rtnl_unlock();
+
+	be_clear(adapter);
+}
+
+static int be_resume(struct be_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+	int status;
+
+	status = be_setup(adapter);
+	if (status)
+		return status;
+
+	if (netif_running(netdev)) {
+		status = be_open(netdev);
+		if (status)
+			return status;
+	}
+
+	netif_device_attach(netdev);
+
+	return 0;
+}
+
+static int be_err_recover(struct be_adapter *adapter)
+{
+	struct device *dev = &adapter->pdev->dev;
+	int status;
+
+	status = be_resume(adapter);
+	if (status)
+		goto err;
+
+	dev_info(dev, "Adapter recovery successful\n");
+	return 0;
+err:
+	if (be_physfn(adapter))
+		dev_err(dev, "Adapter recovery failed\n");
+	else
+		dev_err(dev, "Re-trying adapter recovery\n");
+
+	return status;
+}
+
+static void be_err_detection_task(struct work_struct *work)
+{
+	struct be_adapter *adapter =
+				container_of(work, struct be_adapter,
+					     be_err_detection_work.work);
+	int status = 0;
+
+	be_detect_error(adapter);
+
+	if (adapter->hw_error) {
+		be_cleanup(adapter);
+
+		/* As of now error recovery support is in Lancer only */
+		if (lancer_chip(adapter))
+			status = be_err_recover(adapter);
+	}
+
+	/* Always attempt recovery on VFs */
+	if (!status || be_virtfn(adapter))
+		be_schedule_err_detection(adapter);
+}
+
+static void be_log_sfp_info(struct be_adapter *adapter)
+{
+	int status;
+
+	status = be_cmd_query_sfp_info(adapter);
+	if (!status) {
+		dev_err(&adapter->pdev->dev,
+			"Unqualified SFP+ detected on %c from %s part no: %s",
+			adapter->port_name, adapter->phy.vendor_name,
+			adapter->phy.vendor_pn);
+	}
+	adapter->flags &= ~BE_FLAGS_EVT_INCOMPATIBLE_SFP;
+}
+
+static void be_worker(struct work_struct *work)
+{
+	struct be_adapter *adapter =
+		container_of(work, struct be_adapter, work.work);
+	struct be_rx_obj *rxo;
+	int i;
+
+	/* when interrupts are not yet enabled, just reap any pending
+	 * mcc completions
+	 */
+	if (!netif_running(adapter->netdev)) {
+		local_bh_disable();
+		be_process_mcc(adapter);
+		local_bh_enable();
+		goto reschedule;
+	}
+
+	if (!adapter->stats_cmd_sent) {
+		if (lancer_chip(adapter))
+			lancer_cmd_get_pport_stats(adapter,
+						   &adapter->stats_cmd);
+		else
+			be_cmd_get_stats(adapter, &adapter->stats_cmd);
+	}
+
+	if (be_physfn(adapter) &&
+	    MODULO(adapter->work_counter, adapter->be_get_temp_freq) == 0)
+		be_cmd_get_die_temperature(adapter);
+
+	for_all_rx_queues(adapter, rxo, i) {
+		/* Replenish RX-queues starved due to memory
+		 * allocation failures.
+		 */
+		if (rxo->rx_post_starved)
+			be_post_rx_frags(rxo, GFP_KERNEL, MAX_RX_POST);
+	}
+
+	be_eqd_update(adapter);
+
+	if (adapter->flags & BE_FLAGS_EVT_INCOMPATIBLE_SFP)
+		be_log_sfp_info(adapter);
+
+reschedule:
+	adapter->work_counter++;
+	schedule_delayed_work(&adapter->work, msecs_to_jiffies(1000));
+}
+
 static void be_unmap_pci_bars(struct be_adapter *adapter)
 {
 	if (adapter->csr)
@@ -4820,131 +5221,134 @@ 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;
+	u32 sli_intf;
+
+	pci_read_config_dword(adapter->pdev, SLI_INTF_REG_OFFSET, &sli_intf);
+	adapter->sli_family = (sli_intf & SLI_INTF_FAMILY_MASK) >>
+				SLI_INTF_FAMILY_SHIFT;
+	adapter->virtfn = (sli_intf & SLI_INTF_FT_MASK) ? 1 : 0;
 
 	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;
 }
 
-static void be_ctrl_cleanup(struct be_adapter *adapter)
+static void be_drv_cleanup(struct be_adapter *adapter)
 {
 	struct be_dma_mem *mem = &adapter->mbox_mem_alloced;
-
-	be_unmap_pci_bars(adapter);
+	struct device *dev = &adapter->pdev->dev;
 
 	if (mem->va)
-		dma_free_coherent(&adapter->pdev->dev, mem->size, mem->va,
-				  mem->dma);
+		dma_free_coherent(dev, mem->size, mem->va, mem->dma);
 
 	mem = &adapter->rx_filter;
 	if (mem->va)
-		dma_free_coherent(&adapter->pdev->dev, mem->size, mem->va,
-				  mem->dma);
+		dma_free_coherent(dev, mem->size, mem->va, mem->dma);
+
+	mem = &adapter->stats_cmd;
+	if (mem->va)
+		dma_free_coherent(dev, mem->size, mem->va, mem->dma);
 }
 
-static int be_ctrl_init(struct be_adapter *adapter)
+/* Allocate and initialize various fields in be_adapter struct */
+static int be_drv_init(struct be_adapter *adapter)
 {
 	struct be_dma_mem *mbox_mem_alloc = &adapter->mbox_mem_alloced;
 	struct be_dma_mem *mbox_mem_align = &adapter->mbox_mem;
 	struct be_dma_mem *rx_filter = &adapter->rx_filter;
-	u32 sli_intf;
-	int status;
-
-	pci_read_config_dword(adapter->pdev, SLI_INTF_REG_OFFSET, &sli_intf);
-	adapter->sli_family = (sli_intf & SLI_INTF_FAMILY_MASK) >>
-				 SLI_INTF_FAMILY_SHIFT;
-	adapter->virtfn = (sli_intf & SLI_INTF_FT_MASK) ? 1 : 0;
-
-	status = be_map_pci_bars(adapter);
-	if (status)
-		goto done;
+	struct be_dma_mem *stats_cmd = &adapter->stats_cmd;
+	struct device *dev = &adapter->pdev->dev;
+	int status = 0;
 
 	mbox_mem_alloc->size = sizeof(struct be_mcc_mailbox) + 16;
-	mbox_mem_alloc->va = dma_alloc_coherent(&adapter->pdev->dev,
-						mbox_mem_alloc->size,
+	mbox_mem_alloc->va = dma_alloc_coherent(dev, mbox_mem_alloc->size,
 						&mbox_mem_alloc->dma,
 						GFP_KERNEL);
-	if (!mbox_mem_alloc->va) {
-		status = -ENOMEM;
-		goto unmap_pci_bars;
-	}
+	if (!mbox_mem_alloc->va)
+		return -ENOMEM;
+
 	mbox_mem_align->size = sizeof(struct be_mcc_mailbox);
 	mbox_mem_align->va = PTR_ALIGN(mbox_mem_alloc->va, 16);
 	mbox_mem_align->dma = PTR_ALIGN(mbox_mem_alloc->dma, 16);
 	memset(mbox_mem_align->va, 0, sizeof(struct be_mcc_mailbox));
 
 	rx_filter->size = sizeof(struct be_cmd_req_rx_filter);
-	rx_filter->va = dma_zalloc_coherent(&adapter->pdev->dev,
-					    rx_filter->size, &rx_filter->dma,
-					    GFP_KERNEL);
+	rx_filter->va = dma_zalloc_coherent(dev, rx_filter->size,
+					    &rx_filter->dma, GFP_KERNEL);
 	if (!rx_filter->va) {
 		status = -ENOMEM;
 		goto free_mbox;
 	}
 
+	if (lancer_chip(adapter))
+		stats_cmd->size = sizeof(struct lancer_cmd_req_pport_stats);
+	else if (BE2_chip(adapter))
+		stats_cmd->size = sizeof(struct be_cmd_req_get_stats_v0);
+	else if (BE3_chip(adapter))
+		stats_cmd->size = sizeof(struct be_cmd_req_get_stats_v1);
+	else
+		stats_cmd->size = sizeof(struct be_cmd_req_get_stats_v2);
+	stats_cmd->va = dma_zalloc_coherent(dev, stats_cmd->size,
+					    &stats_cmd->dma, GFP_KERNEL);
+	if (!stats_cmd->va) {
+		status = -ENOMEM;
+		goto free_rx_filter;
+	}
+
 	mutex_init(&adapter->mbox_lock);
 	spin_lock_init(&adapter->mcc_lock);
 	spin_lock_init(&adapter->mcc_cq_lock);
-
 	init_completion(&adapter->et_cmd_compl);
-	pci_save_state(adapter->pdev);
-	return 0;
 
-free_mbox:
-	dma_free_coherent(&adapter->pdev->dev, mbox_mem_alloc->size,
-			  mbox_mem_alloc->va, mbox_mem_alloc->dma);
-
-unmap_pci_bars:
-	be_unmap_pci_bars(adapter);
-
-done:
-	return status;
-}
-
-static void be_stats_cleanup(struct be_adapter *adapter)
-{
-	struct be_dma_mem *cmd = &adapter->stats_cmd;
+	pci_save_state(adapter->pdev);
 
-	if (cmd->va)
-		dma_free_coherent(&adapter->pdev->dev, cmd->size,
-				  cmd->va, cmd->dma);
-}
+	INIT_DELAYED_WORK(&adapter->work, be_worker);
+	INIT_DELAYED_WORK(&adapter->be_err_detection_work,
+			  be_err_detection_task);
 
-static int be_stats_init(struct be_adapter *adapter)
-{
-	struct be_dma_mem *cmd = &adapter->stats_cmd;
+	adapter->rx_fc = true;
+	adapter->tx_fc = true;
 
-	if (lancer_chip(adapter))
-		cmd->size = sizeof(struct lancer_cmd_req_pport_stats);
-	else if (BE2_chip(adapter))
-		cmd->size = sizeof(struct be_cmd_req_get_stats_v0);
-	else if (BE3_chip(adapter))
-		cmd->size = sizeof(struct be_cmd_req_get_stats_v1);
-	else
-		/* ALL non-BE ASICs */
-		cmd->size = sizeof(struct be_cmd_req_get_stats_v2);
+	/* Must be a power of 2 or else MODULO will BUG_ON */
+	adapter->be_get_temp_freq = 64;
 
-	cmd->va = dma_zalloc_coherent(&adapter->pdev->dev, cmd->size, &cmd->dma,
-				      GFP_KERNEL);
-	if (!cmd->va)
-		return -ENOMEM;
 	return 0;
+
+free_rx_filter:
+	dma_free_coherent(dev, rx_filter->size, rx_filter->va, rx_filter->dma);
+free_mbox:
+	dma_free_coherent(dev, mbox_mem_alloc->size, mbox_mem_alloc->va,
+			  mbox_mem_alloc->dma);
+	return status;
 }
 
 static void be_remove(struct pci_dev *pdev)
@@ -4957,7 +5361,7 @@ static void be_remove(struct pci_dev *pdev)
 	be_roce_dev_remove(adapter);
 	be_intr_set(adapter, false);
 
-	cancel_delayed_work_sync(&adapter->func_recovery_work);
+	be_cancel_err_detection(adapter);
 
 	unregister_netdev(adapter->netdev);
 
@@ -4966,9 +5370,8 @@ static void be_remove(struct pci_dev *pdev)
 	/* tell fw we're done with firing cmds */
 	be_cmd_fw_clean(adapter);
 
-	be_stats_cleanup(adapter);
-
-	be_ctrl_cleanup(adapter);
+	be_unmap_pci_bars(adapter);
+	be_drv_cleanup(adapter);
 
 	pci_disable_pcie_error_reporting(pdev);
 
@@ -4978,156 +5381,6 @@ static void be_remove(struct pci_dev *pdev)
 	free_netdev(adapter->netdev);
 }
 
-static int be_get_initial_config(struct be_adapter *adapter)
-{
-	int status, level;
-
-	status = be_cmd_get_cntl_attributes(adapter);
-	if (status)
-		return status;
-
-	/* Must be a power of 2 or else MODULO will BUG_ON */
-	adapter->be_get_temp_freq = 64;
-
-	if (BEx_chip(adapter)) {
-		level = be_cmd_get_fw_log_level(adapter);
-		adapter->msg_enable =
-			level <= FW_LOG_LEVEL_DEFAULT ? NETIF_MSG_HW : 0;
-	}
-
-	adapter->cfg_num_qs = netif_get_num_default_rss_queues();
-	return 0;
-}
-
-static int lancer_recover_func(struct be_adapter *adapter)
-{
-	struct device *dev = &adapter->pdev->dev;
-	int status;
-
-	status = lancer_test_and_set_rdy_state(adapter);
-	if (status)
-		goto err;
-
-	if (netif_running(adapter->netdev))
-		be_close(adapter->netdev);
-
-	be_clear(adapter);
-
-	be_clear_all_error(adapter);
-
-	status = be_setup(adapter);
-	if (status)
-		goto err;
-
-	if (netif_running(adapter->netdev)) {
-		status = be_open(adapter->netdev);
-		if (status)
-			goto err;
-	}
-
-	dev_err(dev, "Adapter recovery successful\n");
-	return 0;
-err:
-	if (status == -EAGAIN)
-		dev_err(dev, "Waiting for resource provisioning\n");
-	else
-		dev_err(dev, "Adapter recovery failed\n");
-
-	return status;
-}
-
-static void be_func_recovery_task(struct work_struct *work)
-{
-	struct be_adapter *adapter =
-		container_of(work, struct be_adapter,  func_recovery_work.work);
-	int status = 0;
-
-	be_detect_error(adapter);
-
-	if (adapter->hw_error && lancer_chip(adapter)) {
-		rtnl_lock();
-		netif_device_detach(adapter->netdev);
-		rtnl_unlock();
-
-		status = lancer_recover_func(adapter);
-		if (!status)
-			netif_device_attach(adapter->netdev);
-	}
-
-	/* In Lancer, for all errors other than provisioning error (-EAGAIN),
-	 * no need to attempt further recovery.
-	 */
-	if (!status || status == -EAGAIN)
-		schedule_delayed_work(&adapter->func_recovery_work,
-				      msecs_to_jiffies(1000));
-}
-
-static void be_log_sfp_info(struct be_adapter *adapter)
-{
-	int status;
-
-	status = be_cmd_query_sfp_info(adapter);
-	if (!status) {
-		dev_err(&adapter->pdev->dev,
-			"Unqualified SFP+ detected on %c from %s part no: %s",
-			adapter->port_name, adapter->phy.vendor_name,
-			adapter->phy.vendor_pn);
-	}
-	adapter->flags &= ~BE_FLAGS_EVT_INCOMPATIBLE_SFP;
-}
-
-static void be_worker(struct work_struct *work)
-{
-	struct be_adapter *adapter =
-		container_of(work, struct be_adapter, work.work);
-	struct be_rx_obj *rxo;
-	int i;
-
-	/* when interrupts are not yet enabled, just reap any pending
-	* mcc completions */
-	if (!netif_running(adapter->netdev)) {
-		local_bh_disable();
-		be_process_mcc(adapter);
-		local_bh_enable();
-		goto reschedule;
-	}
-
-	if (!adapter->stats_cmd_sent) {
-		if (lancer_chip(adapter))
-			lancer_cmd_get_pport_stats(adapter,
-						   &adapter->stats_cmd);
-		else
-			be_cmd_get_stats(adapter, &adapter->stats_cmd);
-	}
-
-	if (be_physfn(adapter) &&
-	    MODULO(adapter->work_counter, adapter->be_get_temp_freq) == 0)
-		be_cmd_get_die_temperature(adapter);
-
-	for_all_rx_queues(adapter, rxo, i) {
-		/* Replenish RX-queues starved due to memory
-		 * allocation failures.
-		 */
-		if (rxo->rx_post_starved)
-			be_post_rx_frags(rxo, GFP_KERNEL, MAX_RX_POST);
-	}
-
-	be_eqd_update(adapter);
-
-	if (adapter->flags & BE_FLAGS_EVT_INCOMPATIBLE_SFP)
-		be_log_sfp_info(adapter);
-
-reschedule:
-	adapter->work_counter++;
-	schedule_delayed_work(&adapter->work, msecs_to_jiffies(1000));
-}
-
-/* If any VFs are already enabled don't FLR the PF */
-static bool be_reset_required(struct be_adapter *adapter)
-{
-	return pci_num_vf(adapter->pdev) ? false : true;
-}
-
 static char *mc_name(struct be_adapter *adapter)
 {
 	char *str = "";	/* default */
@@ -5226,50 +5479,17 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 	if (!status)
 		dev_info(&pdev->dev, "PCIe error reporting enabled\n");
 
-	status = be_ctrl_init(adapter);
+	status = be_map_pci_bars(adapter);
 	if (status)
 		goto free_netdev;
 
-	/* sync up with fw's ready state */
-	if (be_physfn(adapter)) {
-		status = be_fw_wait_ready(adapter);
-		if (status)
-			goto ctrl_clean;
-	}
-
-	if (be_reset_required(adapter)) {
-		status = be_cmd_reset_function(adapter);
-		if (status)
-			goto ctrl_clean;
-
-		/* Wait for interrupts to quiesce after an FLR */
-		msleep(100);
-	}
-
-	/* Allow interrupts for other ULPs running on NIC function */
-	be_intr_set(adapter, true);
-
-	/* tell fw we're ready to fire cmds */
-	status = be_cmd_fw_init(adapter);
+	status = be_drv_init(adapter);
 	if (status)
-		goto ctrl_clean;
-
-	status = be_stats_init(adapter);
-	if (status)
-		goto ctrl_clean;
-
-	status = be_get_initial_config(adapter);
-	if (status)
-		goto stats_clean;
-
-	INIT_DELAYED_WORK(&adapter->work, be_worker);
-	INIT_DELAYED_WORK(&adapter->func_recovery_work, be_func_recovery_task);
-	adapter->rx_fc = true;
-	adapter->tx_fc = true;
+		goto unmap_bars;
 
 	status = be_setup(adapter);
 	if (status)
-		goto stats_clean;
+		goto drv_cleanup;
 
 	be_netdev_init(netdev);
 	status = register_netdev(netdev);
@@ -5278,8 +5498,7 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 
 	be_roce_dev_add(adapter);
 
-	schedule_delayed_work(&adapter->func_recovery_work,
-			      msecs_to_jiffies(1000));
+	be_schedule_err_detection(adapter);
 
 	dev_info(&pdev->dev, "%s: %s %s port %c\n", nic_name(pdev),
 		 func_name(adapter), mc_name(adapter), adapter->port_name);
@@ -5288,10 +5507,10 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 
 unsetup:
 	be_clear(adapter);
-stats_clean:
-	be_stats_cleanup(adapter);
-ctrl_clean:
-	be_ctrl_cleanup(adapter);
+drv_cleanup:
+	be_drv_cleanup(adapter);
+unmap_bars:
+	be_unmap_pci_bars(adapter);
 free_netdev:
 	free_netdev(netdev);
 rel_reg:
@@ -5306,21 +5525,14 @@ do_none:
 static int be_suspend(struct pci_dev *pdev, pm_message_t state)
 {
 	struct be_adapter *adapter = pci_get_drvdata(pdev);
-	struct net_device *netdev =  adapter->netdev;
 
 	if (adapter->wol_en)
 		be_setup_wol(adapter, true);
 
 	be_intr_set(adapter, false);
-	cancel_delayed_work_sync(&adapter->func_recovery_work);
+	be_cancel_err_detection(adapter);
 
-	netif_device_detach(netdev);
-	if (netif_running(netdev)) {
-		rtnl_lock();
-		be_close(netdev);
-		rtnl_unlock();
-	}
-	be_clear(adapter);
+	be_cleanup(adapter);
 
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
@@ -5328,13 +5540,10 @@ static int be_suspend(struct pci_dev *pdev, pm_message_t state)
 	return 0;
 }
 
-static int be_resume(struct pci_dev *pdev)
+static int be_pci_resume(struct pci_dev *pdev)
 {
-	int status = 0;
 	struct be_adapter *adapter = pci_get_drvdata(pdev);
-	struct net_device *netdev =  adapter->netdev;
-
-	netif_device_detach(netdev);
+	int status = 0;
 
 	status = pci_enable_device(pdev);
 	if (status)
@@ -5343,30 +5552,11 @@ static int be_resume(struct pci_dev *pdev)
 	pci_set_power_state(pdev, PCI_D0);
 	pci_restore_state(pdev);
 
-	status = be_fw_wait_ready(adapter);
+	status = be_resume(adapter);
 	if (status)
 		return status;
 
-	status = be_cmd_reset_function(adapter);
-	if (status)
-		return status;
-
-	be_intr_set(adapter, true);
-	/* tell fw we're ready to fire cmds */
-	status = be_cmd_fw_init(adapter);
-	if (status)
-		return status;
-
-	be_setup(adapter);
-	if (netif_running(netdev)) {
-		rtnl_lock();
-		be_open(netdev);
-		rtnl_unlock();
-	}
-
-	schedule_delayed_work(&adapter->func_recovery_work,
-			      msecs_to_jiffies(1000));
-	netif_device_attach(netdev);
+	be_schedule_err_detection(adapter);
 
 	if (adapter->wol_en)
 		be_setup_wol(adapter, false);
@@ -5386,7 +5576,7 @@ static void be_shutdown(struct pci_dev *pdev)
 
 	be_roce_dev_shutdown(adapter);
 	cancel_delayed_work_sync(&adapter->work);
-	cancel_delayed_work_sync(&adapter->func_recovery_work);
+	be_cancel_err_detection(adapter);
 
 	netif_device_detach(adapter->netdev);
 
@@ -5399,22 +5589,15 @@ static pci_ers_result_t be_eeh_err_detected(struct pci_dev *pdev,
 					    pci_channel_state_t state)
 {
 	struct be_adapter *adapter = pci_get_drvdata(pdev);
-	struct net_device *netdev =  adapter->netdev;
 
 	dev_err(&adapter->pdev->dev, "EEH error detected\n");
 
 	if (!adapter->eeh_error) {
 		adapter->eeh_error = true;
 
-		cancel_delayed_work_sync(&adapter->func_recovery_work);
-
-		rtnl_lock();
-		netif_device_detach(netdev);
-		if (netif_running(netdev))
-			be_close(netdev);
-		rtnl_unlock();
+		be_cancel_err_detection(adapter);
 
-		be_clear(adapter);
+		be_cleanup(adapter);
 	}
 
 	if (state == pci_channel_io_perm_failure)
@@ -5465,43 +5648,73 @@ static void be_eeh_resume(struct pci_dev *pdev)
 {
 	int status = 0;
 	struct be_adapter *adapter = pci_get_drvdata(pdev);
-	struct net_device *netdev =  adapter->netdev;
 
 	dev_info(&adapter->pdev->dev, "EEH resume\n");
 
 	pci_save_state(pdev);
 
-	status = be_cmd_reset_function(adapter);
+	status = be_resume(adapter);
 	if (status)
 		goto err;
 
-	/* On some BE3 FW versions, after a HW reset,
-	 * interrupts will remain disabled for each function.
-	 * So, explicitly enable interrupts
+	be_schedule_err_detection(adapter);
+	return;
+err:
+	dev_err(&adapter->pdev->dev, "EEH resume failed\n");
+}
+
+static int be_pci_sriov_configure(struct pci_dev *pdev, int num_vfs)
+{
+	struct be_adapter *adapter = pci_get_drvdata(pdev);
+	u16 num_vf_qs;
+	int status;
+
+	if (!num_vfs)
+		be_vf_clear(adapter);
+
+	adapter->num_vfs = num_vfs;
+
+	if (adapter->num_vfs == 0 && pci_vfs_assigned(pdev)) {
+		dev_warn(&pdev->dev,
+			 "Cannot disable VFs while they are assigned\n");
+		return -EBUSY;
+	}
+
+	/* When the HW is in SRIOV capable configuration, the PF-pool resources
+	 * are equally distributed across the max-number of VFs. The user may
+	 * request only a subset of the max-vfs to be enabled.
+	 * Based on num_vfs, redistribute the resources across num_vfs so that
+	 * each VF will have access to more number of resources.
+	 * This facility is not available in BE3 FW.
+	 * Also, this is done by FW in Lancer chip.
 	 */
-	be_intr_set(adapter, true);
+	if (skyhawk_chip(adapter) && !pci_num_vf(pdev)) {
+		num_vf_qs = be_calculate_vf_qs(adapter, adapter->num_vfs);
+		status = be_cmd_set_sriov_config(adapter, adapter->pool_res,
+						 adapter->num_vfs, num_vf_qs);
+		if (status)
+			dev_err(&pdev->dev,
+				"Failed to optimize SR-IOV resources\n");
+	}
 
-	/* tell fw we're ready to fire cmds */
-	status = be_cmd_fw_init(adapter);
+	status = be_get_resources(adapter);
 	if (status)
-		goto err;
+		return be_cmd_status(status);
 
-	status = be_setup(adapter);
+	/* Updating real_num_tx/rx_queues() requires rtnl_lock() */
+	rtnl_lock();
+	status = be_update_queues(adapter);
+	rtnl_unlock();
 	if (status)
-		goto err;
+		return be_cmd_status(status);
 
-	if (netif_running(netdev)) {
-		status = be_open(netdev);
-		if (status)
-			goto err;
-	}
+	if (adapter->num_vfs)
+		status = be_vf_setup(adapter);
 
-	schedule_delayed_work(&adapter->func_recovery_work,
-			      msecs_to_jiffies(1000));
-	netif_device_attach(netdev);
-	return;
-err:
-	dev_err(&adapter->pdev->dev, "EEH resume failed\n");
+	if (!status)
+		return adapter->num_vfs;
+
+	return 0;
 }
 
 static const struct pci_error_handlers be_eeh_handlers = {
@@ -5516,8 +5729,9 @@ static struct pci_driver be_driver = {
 	.probe = be_probe,
 	.remove = be_remove,
 	.suspend = be_suspend,
-	.resume = be_resume,
+	.resume = be_pci_resume,
 	.shutdown = be_shutdown,
+	.sriov_configure = be_pci_sriov_configure,
 	.err_handler = &be_eeh_handlers
 };
 
@@ -5531,6 +5745,11 @@ static int __init be_init_module(void)
 		rx_frag_size = 2048;
 	}
 
+	if (num_vfs > 0) {
+		pr_info(DRV_NAME " : Module param num_vfs is obsolete.");
+		pr_info(DRV_NAME " : Use sysfs method to enable VFs\n");
+	}
+
 	return pci_register_driver(&be_driver);
 }
 module_init(be_init_module);
diff --git a/drivers/net/ethernet/ethoc.c b/drivers/net/ethernet/ethoc.c
index f88cfaa359e7..442410cd2ca4 100644
--- a/drivers/net/ethernet/ethoc.c
+++ b/drivers/net/ethernet/ethoc.c
@@ -1299,7 +1299,7 @@ static int ethoc_resume(struct platform_device *pdev)
 # define ethoc_resume  NULL
 #endif
 
-static struct of_device_id ethoc_match[] = {
+static const struct of_device_id ethoc_match[] = {
 	{ .compatible = "opencores,ethoc", },
 	{},
 };
diff --git a/drivers/net/ethernet/freescale/Kconfig b/drivers/net/ethernet/freescale/Kconfig
index ba84c4a9ce32..25e3425729d0 100644
--- a/drivers/net/ethernet/freescale/Kconfig
+++ b/drivers/net/ethernet/freescale/Kconfig
@@ -58,14 +58,12 @@ source "drivers/net/ethernet/freescale/fs_enet/Kconfig"
 
 config FSL_PQ_MDIO
 	tristate "Freescale PQ MDIO"
-	depends on FSL_SOC
 	select PHYLIB
 	---help---
 	  This driver supports the MDIO bus used by the gianfar and UCC drivers.
 
 config FSL_XGMAC_MDIO
 	tristate "Freescale XGMAC MDIO"
-	depends on FSL_SOC
 	select PHYLIB
 	select OF_MDIO
 	---help---
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 99492b7e3713..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);
 		}
 
@@ -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();
diff --git a/drivers/net/ethernet/freescale/fec_mpc52xx.c b/drivers/net/ethernet/freescale/fec_mpc52xx.c
index f495796248db..afe7f39cdd7c 100644
--- a/drivers/net/ethernet/freescale/fec_mpc52xx.c
+++ b/drivers/net/ethernet/freescale/fec_mpc52xx.c
@@ -1057,7 +1057,7 @@ static int mpc52xx_fec_of_resume(struct platform_device *op)
 }
 #endif
 
-static struct of_device_id mpc52xx_fec_match[] = {
+static const struct of_device_id mpc52xx_fec_match[] = {
 	{ .compatible = "fsl,mpc5200b-fec", },
 	{ .compatible = "fsl,mpc5200-fec", },
 	{ .compatible = "mpc5200-fec", },
diff --git a/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c b/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c
index e0528900db02..1e647beaf989 100644
--- a/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c
+++ b/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c
@@ -134,7 +134,7 @@ static int mpc52xx_fec_mdio_remove(struct platform_device *of)
 	return 0;
 }
 
-static struct of_device_id mpc52xx_fec_mdio_match[] = {
+static const struct of_device_id mpc52xx_fec_mdio_match[] = {
 	{ .compatible = "fsl,mpc5200b-mdio", },
 	{ .compatible = "fsl,mpc5200-mdio", },
 	{ .compatible = "mpc5200b-fec-phy", },
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 1f9cf2345266..a583d89b13c4 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -136,7 +136,7 @@ static int fec_ptp_enable_pps(struct fec_enet_private *fep, uint enable)
 		 */
 		writel(FEC_T_TF_MASK, fep->hwp + FEC_TCSR(fep->pps_channel));
 
-		/* It is recommended to doulbe check the TMODE field in the
+		/* It is recommended to double check the TMODE field in the
 		 * TCSR register to be cleared before the first compare counter
 		 * is written into TCCR register. Just add a double check.
 		 */
@@ -390,20 +390,18 @@ static int fec_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
  * read the timecounter and return the correct value on ns,
  * after converting it into a struct timespec.
  */
-static int fec_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int fec_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct fec_enet_private *adapter =
 	    container_of(ptp, struct fec_enet_private, ptp_caps);
 	u64 ns;
-	u32 remainder;
 	unsigned long flags;
 
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
 	ns = timecounter_read(&adapter->tc);
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
@@ -417,7 +415,7 @@ static int fec_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
  * wall timer value.
  */
 static int fec_ptp_settime(struct ptp_clock_info *ptp,
-			   const struct timespec *ts)
+			   const struct timespec64 *ts)
 {
 	struct fec_enet_private *fep =
 	    container_of(ptp, struct fec_enet_private, ptp_caps);
@@ -433,8 +431,7 @@ static int fec_ptp_settime(struct ptp_clock_info *ptp,
 		return -EINVAL;
 	}
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 	/* Get the timer value based on timestamp.
 	 * Update the counter with the masked value.
 	 */
@@ -584,8 +581,8 @@ void fec_ptp_init(struct platform_device *pdev)
 	fep->ptp_caps.pps = 1;
 	fep->ptp_caps.adjfreq = fec_ptp_adjfreq;
 	fep->ptp_caps.adjtime = fec_ptp_adjtime;
-	fep->ptp_caps.gettime = fec_ptp_gettime;
-	fep->ptp_caps.settime = fec_ptp_settime;
+	fep->ptp_caps.gettime64 = fec_ptp_gettime;
+	fep->ptp_caps.settime64 = fec_ptp_settime;
 	fep->ptp_caps.enable = fec_ptp_enable;
 
 	fep->cycle_speed = clk_get_rate(fep->clk_ptp);
diff --git a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
index a17628769a1f..9b3639eae676 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -916,7 +916,7 @@ static const struct net_device_ops fs_enet_netdev_ops = {
 #endif
 };
 
-static struct of_device_id fs_enet_match[];
+static const struct of_device_id fs_enet_match[];
 static int fs_enet_probe(struct platform_device *ofdev)
 {
 	const struct of_device_id *match;
@@ -1082,7 +1082,7 @@ static int fs_enet_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id fs_enet_match[] = {
+static const struct of_device_id fs_enet_match[] = {
 #ifdef CONFIG_FS_ENET_HAS_SCC
 	{
 		.compatible = "fsl,cpm1-scc-enet",
diff --git a/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c b/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
index 1d5617d2d8bd..68a428de0bc0 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
@@ -213,7 +213,7 @@ static int fs_enet_mdio_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id fs_enet_mdio_bb_match[] = {
+static const struct of_device_id fs_enet_mdio_bb_match[] = {
 	{
 		.compatible = "fsl,cpm2-mdio-bitbang",
 	},
diff --git a/drivers/net/ethernet/freescale/fs_enet/mii-fec.c b/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
index 1648e3582500..2be383e6d258 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
@@ -95,7 +95,7 @@ static int fs_enet_fec_mii_write(struct mii_bus *bus, int phy_id, int location,
 
 }
 
-static struct of_device_id fs_enet_mdio_fec_match[];
+static const struct of_device_id fs_enet_mdio_fec_match[];
 static int fs_enet_mdio_probe(struct platform_device *ofdev)
 {
 	const struct of_device_id *match;
@@ -208,7 +208,7 @@ static int fs_enet_mdio_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id fs_enet_mdio_fec_match[] = {
+static const struct of_device_id fs_enet_mdio_fec_match[] = {
 	{
 		.compatible = "fsl,pq1-fec-mdio",
 	},
diff --git a/drivers/net/ethernet/freescale/fsl_pq_mdio.c b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
index d1a91e344e6b..3c40f6b99224 100644
--- a/drivers/net/ethernet/freescale/fsl_pq_mdio.c
+++ b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
@@ -294,7 +294,7 @@ static void ucc_configure(phys_addr_t start, phys_addr_t end)
 
 #endif
 
-static struct of_device_id fsl_pq_mdio_match[] = {
+static const struct of_device_id fsl_pq_mdio_match[] = {
 #if defined(CONFIG_GIANFAR) || defined(CONFIG_GIANFAR_MODULE)
 	{
 		.compatible = "fsl,gianfar-tbi",
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 7bf3682cdf47..4ee080d49bc0 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -158,7 +158,7 @@ static void gfar_init_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
 {
 	u32 lstatus;
 
-	bdp->bufPtr = buf;
+	bdp->bufPtr = cpu_to_be32(buf);
 
 	lstatus = BD_LFLAG(RXBD_EMPTY | RXBD_INTERRUPT);
 	if (bdp == rx_queue->rx_bd_base + rx_queue->rx_ring_size - 1)
@@ -166,7 +166,7 @@ static void gfar_init_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
 
 	gfar_wmb();
 
-	bdp->lstatus = lstatus;
+	bdp->lstatus = cpu_to_be32(lstatus);
 }
 
 static int gfar_init_bds(struct net_device *ndev)
@@ -200,7 +200,8 @@ static int gfar_init_bds(struct net_device *ndev)
 
 		/* Set the last descriptor in the ring to indicate wrap */
 		txbdp--;
-		txbdp->status |= TXBD_WRAP;
+		txbdp->status = cpu_to_be16(be16_to_cpu(txbdp->status) |
+					    TXBD_WRAP);
 	}
 
 	rfbptr = &regs->rfbptr0;
@@ -214,7 +215,7 @@ static int gfar_init_bds(struct net_device *ndev)
 			struct sk_buff *skb = rx_queue->rx_skbuff[j];
 
 			if (skb) {
-				bufaddr = rxbdp->bufPtr;
+				bufaddr = be32_to_cpu(rxbdp->bufPtr);
 			} else {
 				skb = gfar_new_skb(ndev, &bufaddr);
 				if (!skb) {
@@ -696,19 +697,28 @@ static int gfar_parse_group(struct device_node *np,
 	grp->priv = priv;
 	spin_lock_init(&grp->grplock);
 	if (priv->mode == MQ_MG_MODE) {
-		u32 *rxq_mask, *txq_mask;
-		rxq_mask = (u32 *)of_get_property(np, "fsl,rx-bit-map", NULL);
-		txq_mask = (u32 *)of_get_property(np, "fsl,tx-bit-map", NULL);
+		u32 rxq_mask, txq_mask;
+		int ret;
+
+		grp->rx_bit_map = (DEFAULT_MAPPING >> priv->num_grps);
+		grp->tx_bit_map = (DEFAULT_MAPPING >> priv->num_grps);
+
+		ret = of_property_read_u32(np, "fsl,rx-bit-map", &rxq_mask);
+		if (!ret) {
+			grp->rx_bit_map = rxq_mask ?
+			rxq_mask : (DEFAULT_MAPPING >> priv->num_grps);
+		}
+
+		ret = of_property_read_u32(np, "fsl,tx-bit-map", &txq_mask);
+		if (!ret) {
+			grp->tx_bit_map = txq_mask ?
+			txq_mask : (DEFAULT_MAPPING >> priv->num_grps);
+		}
 
 		if (priv->poll_mode == GFAR_SQ_POLLING) {
 			/* One Q per interrupt group: Q0 to G0, Q1 to G1 */
 			grp->rx_bit_map = (DEFAULT_MAPPING >> priv->num_grps);
 			grp->tx_bit_map = (DEFAULT_MAPPING >> priv->num_grps);
-		} else { /* GFAR_MQ_POLLING */
-			grp->rx_bit_map = rxq_mask ?
-			*rxq_mask : (DEFAULT_MAPPING >> priv->num_grps);
-			grp->tx_bit_map = txq_mask ?
-			*txq_mask : (DEFAULT_MAPPING >> priv->num_grps);
 		}
 	} else {
 		grp->rx_bit_map = 0xFF;
@@ -769,11 +779,10 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 	struct gfar_private *priv = NULL;
 	struct device_node *np = ofdev->dev.of_node;
 	struct device_node *child = NULL;
-	const u32 *stash;
-	const u32 *stash_len;
-	const u32 *stash_idx;
+	struct property *stash;
+	u32 stash_len = 0;
+	u32 stash_idx = 0;
 	unsigned int num_tx_qs, num_rx_qs;
-	u32 *tx_queues, *rx_queues;
 	unsigned short mode, poll_mode;
 
 	if (!np)
@@ -787,10 +796,6 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 		poll_mode = GFAR_SQ_POLLING;
 	}
 
-	/* parse the num of HW tx and rx queues */
-	tx_queues = (u32 *)of_get_property(np, "fsl,num_tx_queues", NULL);
-	rx_queues = (u32 *)of_get_property(np, "fsl,num_rx_queues", NULL);
-
 	if (mode == SQ_SG_MODE) {
 		num_tx_qs = 1;
 		num_rx_qs = 1;
@@ -809,8 +814,17 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 			num_tx_qs = num_grps; /* one txq per int group */
 			num_rx_qs = num_grps; /* one rxq per int group */
 		} else { /* GFAR_MQ_POLLING */
-			num_tx_qs = tx_queues ? *tx_queues : 1;
-			num_rx_qs = rx_queues ? *rx_queues : 1;
+			u32 tx_queues, rx_queues;
+			int ret;
+
+			/* parse the num of HW tx and rx queues */
+			ret = of_property_read_u32(np, "fsl,num_tx_queues",
+						   &tx_queues);
+			num_tx_qs = ret ? 1 : tx_queues;
+
+			ret = of_property_read_u32(np, "fsl,num_rx_queues",
+						   &rx_queues);
+			num_rx_qs = ret ? 1 : rx_queues;
 		}
 	}
 
@@ -851,13 +865,17 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 	if (err)
 		goto rx_alloc_failed;
 
+	err = of_property_read_string(np, "model", &model);
+	if (err) {
+		pr_err("Device model property missing, aborting\n");
+		goto rx_alloc_failed;
+	}
+
 	/* Init Rx queue filer rule set linked list */
 	INIT_LIST_HEAD(&priv->rx_list.list);
 	priv->rx_list.count = 0;
 	mutex_init(&priv->rx_queue_access);
 
-	model = of_get_property(np, "model", NULL);
-
 	for (i = 0; i < MAXGROUPS; i++)
 		priv->gfargrp[i].regs = NULL;
 
@@ -877,22 +895,22 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 			goto err_grp_init;
 	}
 
-	stash = of_get_property(np, "bd-stash", NULL);
+	stash = of_find_property(np, "bd-stash", NULL);
 
 	if (stash) {
 		priv->device_flags |= FSL_GIANFAR_DEV_HAS_BD_STASHING;
 		priv->bd_stash_en = 1;
 	}
 
-	stash_len = of_get_property(np, "rx-stash-len", NULL);
+	err = of_property_read_u32(np, "rx-stash-len", &stash_len);
 
-	if (stash_len)
-		priv->rx_stash_size = *stash_len;
+	if (err == 0)
+		priv->rx_stash_size = stash_len;
 
-	stash_idx = of_get_property(np, "rx-stash-idx", NULL);
+	err = of_property_read_u32(np, "rx-stash-idx", &stash_idx);
 
-	if (stash_idx)
-		priv->rx_stash_index = *stash_idx;
+	if (err == 0)
+		priv->rx_stash_index = stash_idx;
 
 	if (stash_len || stash_idx)
 		priv->device_flags |= FSL_GIANFAR_DEV_HAS_BUF_STASHING;
@@ -919,15 +937,15 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 				     FSL_GIANFAR_DEV_HAS_EXTENDED_HASH |
 				     FSL_GIANFAR_DEV_HAS_TIMER;
 
-	ctype = of_get_property(np, "phy-connection-type", NULL);
+	err = of_property_read_string(np, "phy-connection-type", &ctype);
 
 	/* We only care about rgmii-id.  The rest are autodetected */
-	if (ctype && !strcmp(ctype, "rgmii-id"))
+	if (err == 0 && !strcmp(ctype, "rgmii-id"))
 		priv->interface = PHY_INTERFACE_MODE_RGMII_ID;
 	else
 		priv->interface = PHY_INTERFACE_MODE_MII;
 
-	if (of_get_property(np, "fsl,magic-packet", NULL))
+	if (of_find_property(np, "fsl,magic-packet", NULL))
 		priv->device_flags |= FSL_GIANFAR_DEV_HAS_MAGIC_PACKET;
 
 	priv->phy_node = of_parse_phandle(np, "phy-handle", 0);
@@ -1884,14 +1902,15 @@ static void free_skb_tx_queue(struct gfar_priv_tx_q *tx_queue)
 		if (!tx_queue->tx_skbuff[i])
 			continue;
 
-		dma_unmap_single(priv->dev, txbdp->bufPtr,
-				 txbdp->length, DMA_TO_DEVICE);
+		dma_unmap_single(priv->dev, be32_to_cpu(txbdp->bufPtr),
+				 be16_to_cpu(txbdp->length), DMA_TO_DEVICE);
 		txbdp->lstatus = 0;
 		for (j = 0; j < skb_shinfo(tx_queue->tx_skbuff[i])->nr_frags;
 		     j++) {
 			txbdp++;
-			dma_unmap_page(priv->dev, txbdp->bufPtr,
-				       txbdp->length, DMA_TO_DEVICE);
+			dma_unmap_page(priv->dev, be32_to_cpu(txbdp->bufPtr),
+				       be16_to_cpu(txbdp->length),
+				       DMA_TO_DEVICE);
 		}
 		txbdp++;
 		dev_kfree_skb_any(tx_queue->tx_skbuff[i]);
@@ -1911,7 +1930,7 @@ static void free_skb_rx_queue(struct gfar_priv_rx_q *rx_queue)
 
 	for (i = 0; i < rx_queue->rx_ring_size; i++) {
 		if (rx_queue->rx_skbuff[i]) {
-			dma_unmap_single(priv->dev, rxbdp->bufPtr,
+			dma_unmap_single(priv->dev, be32_to_cpu(rxbdp->bufPtr),
 					 priv->rx_buffer_size,
 					 DMA_FROM_DEVICE);
 			dev_kfree_skb_any(rx_queue->rx_skbuff[i]);
@@ -2167,16 +2186,16 @@ static inline void gfar_tx_checksum(struct sk_buff *skb, struct txfcb *fcb,
 	 */
 	if (ip_hdr(skb)->protocol == IPPROTO_UDP) {
 		flags |= TXFCB_UDP;
-		fcb->phcs = udp_hdr(skb)->check;
+		fcb->phcs = (__force __be16)(udp_hdr(skb)->check);
 	} else
-		fcb->phcs = tcp_hdr(skb)->check;
+		fcb->phcs = (__force __be16)(tcp_hdr(skb)->check);
 
 	/* l3os is the distance between the start of the
 	 * frame (skb->data) and the start of the IP hdr.
 	 * l4os is the distance between the start of the
 	 * l3 hdr and the l4 hdr
 	 */
-	fcb->l3os = (u16)(skb_network_offset(skb) - fcb_length);
+	fcb->l3os = (u8)(skb_network_offset(skb) - fcb_length);
 	fcb->l4os = skb_network_header_len(skb);
 
 	fcb->flags = flags;
@@ -2185,7 +2204,7 @@ static inline void gfar_tx_checksum(struct sk_buff *skb, struct txfcb *fcb,
 void inline gfar_tx_vlan(struct sk_buff *skb, struct txfcb *fcb)
 {
 	fcb->flags |= TXFCB_VLN;
-	fcb->vlctl = skb_vlan_tag_get(skb);
+	fcb->vlctl = cpu_to_be16(skb_vlan_tag_get(skb));
 }
 
 static inline struct txbd8 *skip_txbd(struct txbd8 *bdp, int stride,
@@ -2298,7 +2317,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	tx_queue->stats.tx_packets++;
 
 	txbdp = txbdp_start = tx_queue->cur_tx;
-	lstatus = txbdp->lstatus;
+	lstatus = be32_to_cpu(txbdp->lstatus);
 
 	/* Time stamp insertion requires one additional TxBD */
 	if (unlikely(do_tstamp))
@@ -2306,11 +2325,14 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 						 tx_queue->tx_ring_size);
 
 	if (nr_frags == 0) {
-		if (unlikely(do_tstamp))
-			txbdp_tstamp->lstatus |= BD_LFLAG(TXBD_LAST |
-							  TXBD_INTERRUPT);
-		else
+		if (unlikely(do_tstamp)) {
+			u32 lstatus_ts = be32_to_cpu(txbdp_tstamp->lstatus);
+
+			lstatus_ts |= BD_LFLAG(TXBD_LAST | TXBD_INTERRUPT);
+			txbdp_tstamp->lstatus = cpu_to_be32(lstatus_ts);
+		} else {
 			lstatus |= BD_LFLAG(TXBD_LAST | TXBD_INTERRUPT);
+		}
 	} else {
 		/* Place the fragment addresses and lengths into the TxBDs */
 		for (i = 0; i < nr_frags; i++) {
@@ -2320,7 +2342,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 			frag_len = skb_shinfo(skb)->frags[i].size;
 
-			lstatus = txbdp->lstatus | frag_len |
+			lstatus = be32_to_cpu(txbdp->lstatus) | frag_len |
 				  BD_LFLAG(TXBD_READY);
 
 			/* Handle the last BD specially */
@@ -2336,11 +2358,11 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 				goto dma_map_err;
 
 			/* set the TxBD length and buffer pointer */
-			txbdp->bufPtr = bufaddr;
-			txbdp->lstatus = lstatus;
+			txbdp->bufPtr = cpu_to_be32(bufaddr);
+			txbdp->lstatus = cpu_to_be32(lstatus);
 		}
 
-		lstatus = txbdp_start->lstatus;
+		lstatus = be32_to_cpu(txbdp_start->lstatus);
 	}
 
 	/* Add TxPAL between FCB and frame if required */
@@ -2388,7 +2410,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (unlikely(dma_mapping_error(priv->dev, bufaddr)))
 		goto dma_map_err;
 
-	txbdp_start->bufPtr = bufaddr;
+	txbdp_start->bufPtr = cpu_to_be32(bufaddr);
 
 	/* If time stamping is requested one additional TxBD must be set up. The
 	 * first TxBD points to the FCB and must have a data length of
@@ -2396,9 +2418,15 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 * the full frame length.
 	 */
 	if (unlikely(do_tstamp)) {
-		txbdp_tstamp->bufPtr = txbdp_start->bufPtr + fcb_len;
-		txbdp_tstamp->lstatus |= BD_LFLAG(TXBD_READY) |
-					 (skb_headlen(skb) - fcb_len);
+		u32 lstatus_ts = be32_to_cpu(txbdp_tstamp->lstatus);
+
+		bufaddr = be32_to_cpu(txbdp_start->bufPtr);
+		bufaddr += fcb_len;
+		lstatus_ts |= BD_LFLAG(TXBD_READY) |
+			      (skb_headlen(skb) - fcb_len);
+
+		txbdp_tstamp->bufPtr = cpu_to_be32(bufaddr);
+		txbdp_tstamp->lstatus = cpu_to_be32(lstatus_ts);
 		lstatus |= BD_LFLAG(TXBD_CRC | TXBD_READY) | GMAC_FCB_LEN;
 	} else {
 		lstatus |= BD_LFLAG(TXBD_CRC | TXBD_READY) | skb_headlen(skb);
@@ -2421,7 +2449,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	gfar_wmb();
 
-	txbdp_start->lstatus = lstatus;
+	txbdp_start->lstatus = cpu_to_be32(lstatus);
 
 	gfar_wmb(); /* force lstatus write before tx_skbuff */
 
@@ -2460,13 +2488,14 @@ dma_map_err:
 	if (do_tstamp)
 		txbdp = next_txbd(txbdp, base, tx_queue->tx_ring_size);
 	for (i = 0; i < nr_frags; i++) {
-		lstatus = txbdp->lstatus;
+		lstatus = be32_to_cpu(txbdp->lstatus);
 		if (!(lstatus & BD_LFLAG(TXBD_READY)))
 			break;
 
-		txbdp->lstatus = lstatus & ~BD_LFLAG(TXBD_READY);
-		bufaddr = txbdp->bufPtr;
-		dma_unmap_page(priv->dev, bufaddr, txbdp->length,
+		lstatus &= ~BD_LFLAG(TXBD_READY);
+		txbdp->lstatus = cpu_to_be32(lstatus);
+		bufaddr = be32_to_cpu(txbdp->bufPtr);
+		dma_unmap_page(priv->dev, bufaddr, be16_to_cpu(txbdp->length),
 			       DMA_TO_DEVICE);
 		txbdp = next_txbd(txbdp, base, tx_queue->tx_ring_size);
 	}
@@ -2607,7 +2636,7 @@ static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 
 		lbdp = skip_txbd(bdp, nr_txbds - 1, base, tx_ring_size);
 
-		lstatus = lbdp->lstatus;
+		lstatus = be32_to_cpu(lbdp->lstatus);
 
 		/* Only clean completed frames */
 		if ((lstatus & BD_LFLAG(TXBD_READY)) &&
@@ -2616,11 +2645,12 @@ static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 
 		if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) {
 			next = next_txbd(bdp, base, tx_ring_size);
-			buflen = next->length + GMAC_FCB_LEN + GMAC_TXPAL_LEN;
+			buflen = be16_to_cpu(next->length) +
+				 GMAC_FCB_LEN + GMAC_TXPAL_LEN;
 		} else
-			buflen = bdp->length;
+			buflen = be16_to_cpu(bdp->length);
 
-		dma_unmap_single(priv->dev, bdp->bufPtr,
+		dma_unmap_single(priv->dev, be32_to_cpu(bdp->bufPtr),
 				 buflen, DMA_TO_DEVICE);
 
 		if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) {
@@ -2631,17 +2661,18 @@ static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 			shhwtstamps.hwtstamp = ns_to_ktime(*ns);
 			skb_pull(skb, GMAC_FCB_LEN + GMAC_TXPAL_LEN);
 			skb_tstamp_tx(skb, &shhwtstamps);
-			bdp->lstatus &= BD_LFLAG(TXBD_WRAP);
+			gfar_clear_txbd_status(bdp);
 			bdp = next;
 		}
 
-		bdp->lstatus &= BD_LFLAG(TXBD_WRAP);
+		gfar_clear_txbd_status(bdp);
 		bdp = next_txbd(bdp, base, tx_ring_size);
 
 		for (i = 0; i < frags; i++) {
-			dma_unmap_page(priv->dev, bdp->bufPtr,
-				       bdp->length, DMA_TO_DEVICE);
-			bdp->lstatus &= BD_LFLAG(TXBD_WRAP);
+			dma_unmap_page(priv->dev, be32_to_cpu(bdp->bufPtr),
+				       be16_to_cpu(bdp->length),
+				       DMA_TO_DEVICE);
+			gfar_clear_txbd_status(bdp);
 			bdp = next_txbd(bdp, base, tx_ring_size);
 		}
 
@@ -2798,13 +2829,13 @@ static inline void gfar_rx_checksum(struct sk_buff *skb, struct rxfcb *fcb)
 	 * were verified, then we tell the kernel that no
 	 * checksumming is necessary.  Otherwise, it is [FIXME]
 	 */
-	if ((fcb->flags & RXFCB_CSUM_MASK) == (RXFCB_CIP | RXFCB_CTU))
+	if ((be16_to_cpu(fcb->flags) & RXFCB_CSUM_MASK) ==
+	    (RXFCB_CIP | RXFCB_CTU))
 		skb->ip_summed = CHECKSUM_UNNECESSARY;
 	else
 		skb_checksum_none_assert(skb);
 }
 
-
 /* gfar_process_frame() -- handle one incoming packet if skb isn't NULL. */
 static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 			       int amount_pull, struct napi_struct *napi)
@@ -2846,8 +2877,9 @@ static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	 * RXFCB_VLN is pseudo randomly set.
 	 */
 	if (dev->features & NETIF_F_HW_VLAN_CTAG_RX &&
-	    fcb->flags & RXFCB_VLN)
-		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), fcb->vlctl);
+	    be16_to_cpu(fcb->flags) & RXFCB_VLN)
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
+				       be16_to_cpu(fcb->vlctl));
 
 	/* Send the packet up the stack */
 	napi_gro_receive(napi, skb);
@@ -2874,7 +2906,7 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 
 	amount_pull = priv->uses_rxfcb ? GMAC_FCB_LEN : 0;
 
-	while (!((bdp->status & RXBD_EMPTY) || (--rx_work_limit < 0))) {
+	while (!(be16_to_cpu(bdp->status) & RXBD_EMPTY) && rx_work_limit--) {
 		struct sk_buff *newskb;
 		dma_addr_t bufaddr;
 
@@ -2885,21 +2917,22 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 
 		skb = rx_queue->rx_skbuff[rx_queue->skb_currx];
 
-		dma_unmap_single(priv->dev, bdp->bufPtr,
+		dma_unmap_single(priv->dev, be32_to_cpu(bdp->bufPtr),
 				 priv->rx_buffer_size, DMA_FROM_DEVICE);
 
-		if (unlikely(!(bdp->status & RXBD_ERR) &&
-			     bdp->length > priv->rx_buffer_size))
-			bdp->status = RXBD_LARGE;
+		if (unlikely(!(be16_to_cpu(bdp->status) & RXBD_ERR) &&
+			     be16_to_cpu(bdp->length) > priv->rx_buffer_size))
+			bdp->status = cpu_to_be16(RXBD_LARGE);
 
 		/* We drop the frame if we failed to allocate a new buffer */
-		if (unlikely(!newskb || !(bdp->status & RXBD_LAST) ||
-			     bdp->status & RXBD_ERR)) {
-			count_errors(bdp->status, dev);
+		if (unlikely(!newskb ||
+			     !(be16_to_cpu(bdp->status) & RXBD_LAST) ||
+			     be16_to_cpu(bdp->status) & RXBD_ERR)) {
+			count_errors(be16_to_cpu(bdp->status), dev);
 
 			if (unlikely(!newskb)) {
 				newskb = skb;
-				bufaddr = bdp->bufPtr;
+				bufaddr = be32_to_cpu(bdp->bufPtr);
 			} else if (skb)
 				dev_kfree_skb(skb);
 		} else {
@@ -2908,7 +2941,8 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 			howmany++;
 
 			if (likely(skb)) {
-				pkt_len = bdp->length - ETH_FCS_LEN;
+				pkt_len = be16_to_cpu(bdp->length) -
+					  ETH_FCS_LEN;
 				/* Remove the FCS from the packet length */
 				skb_put(skb, pkt_len);
 				rx_queue->stats.rx_bytes += pkt_len;
@@ -3560,7 +3594,7 @@ static noinline void gfar_update_link_state(struct gfar_private *priv)
 		phy_print_status(phydev);
 }
 
-static struct of_device_id gfar_match[] =
+static const struct of_device_id gfar_match[] =
 {
 	{
 		.type = "network",
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index 9e1802400c23..daa1d37de642 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -544,12 +544,12 @@ struct txbd8
 {
 	union {
 		struct {
-			u16	status;	/* Status Fields */
-			u16	length;	/* Buffer length */
+			__be16	status;	/* Status Fields */
+			__be16	length;	/* Buffer length */
 		};
-		u32 lstatus;
+		__be32 lstatus;
 	};
-	u32	bufPtr;	/* Buffer Pointer */
+	__be32	bufPtr;	/* Buffer Pointer */
 };
 
 struct txfcb {
@@ -557,28 +557,28 @@ struct txfcb {
 	u8	ptp;    /* Flag to enable tx timestamping */
 	u8	l4os;	/* Level 4 Header Offset */
 	u8	l3os; 	/* Level 3 Header Offset */
-	u16	phcs;	/* Pseudo-header Checksum */
-	u16	vlctl;	/* VLAN control word */
+	__be16	phcs;	/* Pseudo-header Checksum */
+	__be16	vlctl;	/* VLAN control word */
 };
 
 struct rxbd8
 {
 	union {
 		struct {
-			u16	status;	/* Status Fields */
-			u16	length;	/* Buffer Length */
+			__be16	status;	/* Status Fields */
+			__be16	length;	/* Buffer Length */
 		};
-		u32 lstatus;
+		__be32 lstatus;
 	};
-	u32	bufPtr;	/* Buffer Pointer */
+	__be32	bufPtr;	/* Buffer Pointer */
 };
 
 struct rxfcb {
-	u16	flags;
+	__be16	flags;
 	u8	rq;	/* Receive Queue index */
 	u8	pro;	/* Layer 4 Protocol */
 	u16	reserved;
-	u16	vlctl;	/* VLAN control word */
+	__be16	vlctl;	/* VLAN control word */
 };
 
 struct gianfar_skb_cb {
@@ -1287,6 +1287,14 @@ static inline void gfar_wmb(void)
 #endif
 }
 
+static inline void gfar_clear_txbd_status(struct txbd8 *bdp)
+{
+	u32 lstatus = be32_to_cpu(bdp->lstatus);
+
+	lstatus &= BD_LFLAG(TXBD_WRAP);
+	bdp->lstatus = cpu_to_be32(lstatus);
+}
+
 irqreturn_t gfar_receive(int irq, void *dev_id);
 int startup_gfar(struct net_device *dev);
 void stop_gfar(struct net_device *dev);
diff --git a/drivers/net/ethernet/freescale/gianfar_ptp.c b/drivers/net/ethernet/freescale/gianfar_ptp.c
index 16826341a4c9..8e3cd77aa347 100644
--- a/drivers/net/ethernet/freescale/gianfar_ptp.c
+++ b/drivers/net/ethernet/freescale/gianfar_ptp.c
@@ -322,10 +322,10 @@ static int ptp_gianfar_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int ptp_gianfar_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int ptp_gianfar_gettime(struct ptp_clock_info *ptp,
+			       struct timespec64 *ts)
 {
 	u64 ns;
-	u32 remainder;
 	unsigned long flags;
 	struct etsects *etsects = container_of(ptp, struct etsects, caps);
 
@@ -335,20 +335,19 @@ static int ptp_gianfar_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 
 	spin_unlock_irqrestore(&etsects->lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
+
 	return 0;
 }
 
 static int ptp_gianfar_settime(struct ptp_clock_info *ptp,
-			       const struct timespec *ts)
+			       const struct timespec64 *ts)
 {
 	u64 ns;
 	unsigned long flags;
 	struct etsects *etsects = container_of(ptp, struct etsects, caps);
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 
 	spin_lock_irqsave(&etsects->lock, flags);
 
@@ -418,8 +417,8 @@ static struct ptp_clock_info ptp_gianfar_caps = {
 	.pps		= 1,
 	.adjfreq	= ptp_gianfar_adjfreq,
 	.adjtime	= ptp_gianfar_adjtime,
-	.gettime	= ptp_gianfar_gettime,
-	.settime	= ptp_gianfar_settime,
+	.gettime64	= ptp_gianfar_gettime,
+	.settime64	= ptp_gianfar_settime,
 	.enable		= ptp_gianfar_enable,
 };
 
@@ -440,7 +439,7 @@ static int gianfar_ptp_probe(struct platform_device *dev)
 {
 	struct device_node *node = dev->dev.of_node;
 	struct etsects *etsects;
-	struct timespec now;
+	struct timespec64 now;
 	int err = -ENOMEM;
 	u32 tmr_ctrl;
 	unsigned long flags;
@@ -495,7 +494,7 @@ static int gianfar_ptp_probe(struct platform_device *dev)
 		pr_err("ioremap ptp registers failed\n");
 		goto no_ioremap;
 	}
-	getnstimeofday(&now);
+	getnstimeofday64(&now);
 	ptp_gianfar_settime(&etsects->caps, &now);
 
 	tmr_ctrl =
@@ -554,7 +553,7 @@ static int gianfar_ptp_remove(struct platform_device *dev)
 	return 0;
 }
 
-static struct of_device_id match_table[] = {
+static const struct of_device_id match_table[] = {
 	{ .compatible = "fsl,etsec-ptp" },
 	{},
 };
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 357e8b576905..4dd40e057f40 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))
@@ -3930,7 +3933,7 @@ static int ucc_geth_remove(struct platform_device* ofdev)
 	return 0;
 }
 
-static struct of_device_id ucc_geth_match[] = {
+static const struct of_device_id ucc_geth_match[] = {
 	{
 		.type = "network",
 		.compatible = "ucc_geth",
diff --git a/drivers/net/ethernet/freescale/xgmac_mdio.c b/drivers/net/ethernet/freescale/xgmac_mdio.c
index 3a83bc2c613c..7b8fe866f603 100644
--- a/drivers/net/ethernet/freescale/xgmac_mdio.c
+++ b/drivers/net/ethernet/freescale/xgmac_mdio.c
@@ -46,17 +46,43 @@ struct tgec_mdio_controller {
 #define MDIO_DATA(x)		(x & 0xffff)
 #define MDIO_DATA_BSY		BIT(31)
 
+struct mdio_fsl_priv {
+	struct	tgec_mdio_controller __iomem *mdio_base;
+	bool	is_little_endian;
+};
+
+static u32 xgmac_read32(void __iomem *regs,
+			bool is_little_endian)
+{
+	if (is_little_endian)
+		return ioread32(regs);
+	else
+		return ioread32be(regs);
+}
+
+static void xgmac_write32(u32 value,
+			  void __iomem *regs,
+			  bool is_little_endian)
+{
+	if (is_little_endian)
+		iowrite32(value, regs);
+	else
+		iowrite32be(value, regs);
+}
+
 /*
  * Wait until the MDIO bus is free
  */
 static int xgmac_wait_until_free(struct device *dev,
-				 struct tgec_mdio_controller __iomem *regs)
+				 struct tgec_mdio_controller __iomem *regs,
+				 bool is_little_endian)
 {
 	unsigned int timeout;
 
 	/* Wait till the bus is free */
 	timeout = TIMEOUT;
-	while ((ioread32be(&regs->mdio_stat) & MDIO_STAT_BSY) && timeout) {
+	while ((xgmac_read32(&regs->mdio_stat, is_little_endian) &
+		MDIO_STAT_BSY) && timeout) {
 		cpu_relax();
 		timeout--;
 	}
@@ -73,13 +99,15 @@ static int xgmac_wait_until_free(struct device *dev,
  * Wait till the MDIO read or write operation is complete
  */
 static int xgmac_wait_until_done(struct device *dev,
-				 struct tgec_mdio_controller __iomem *regs)
+				 struct tgec_mdio_controller __iomem *regs,
+				 bool is_little_endian)
 {
 	unsigned int timeout;
 
 	/* Wait till the MDIO write is complete */
 	timeout = TIMEOUT;
-	while ((ioread32be(&regs->mdio_data) & MDIO_DATA_BSY) && timeout) {
+	while ((xgmac_read32(&regs->mdio_stat, is_little_endian) &
+		MDIO_STAT_BSY) && timeout) {
 		cpu_relax();
 		timeout--;
 	}
@@ -99,12 +127,14 @@ static int xgmac_wait_until_done(struct device *dev,
  */
 static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 value)
 {
-	struct tgec_mdio_controller __iomem *regs = bus->priv;
+	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
+	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
 	uint16_t dev_addr;
 	u32 mdio_ctl, mdio_stat;
 	int ret;
+	bool endian = priv->is_little_endian;
 
-	mdio_stat = ioread32be(&regs->mdio_stat);
+	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
 	if (regnum & MII_ADDR_C45) {
 		/* Clause 45 (ie 10G) */
 		dev_addr = (regnum >> 16) & 0x1f;
@@ -115,29 +145,29 @@ static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 val
 		mdio_stat &= ~MDIO_STAT_ENC;
 	}
 
-	iowrite32be(mdio_stat, &regs->mdio_stat);
+	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
 
-	ret = xgmac_wait_until_free(&bus->dev, regs);
+	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
 	if (ret)
 		return ret;
 
 	/* Set the port and dev addr */
 	mdio_ctl = MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr);
-	iowrite32be(mdio_ctl, &regs->mdio_ctl);
+	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
 
 	/* Set the register address */
 	if (regnum & MII_ADDR_C45) {
-		iowrite32be(regnum & 0xffff, &regs->mdio_addr);
+		xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
 
-		ret = xgmac_wait_until_free(&bus->dev, regs);
+		ret = xgmac_wait_until_free(&bus->dev, regs, endian);
 		if (ret)
 			return ret;
 	}
 
 	/* Write the value to the register */
-	iowrite32be(MDIO_DATA(value), &regs->mdio_data);
+	xgmac_write32(MDIO_DATA(value), &regs->mdio_data, endian);
 
-	ret = xgmac_wait_until_done(&bus->dev, regs);
+	ret = xgmac_wait_until_done(&bus->dev, regs, endian);
 	if (ret)
 		return ret;
 
@@ -151,14 +181,16 @@ static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 val
  */
 static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
 {
-	struct tgec_mdio_controller __iomem *regs = bus->priv;
+	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
+	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
 	uint16_t dev_addr;
 	uint32_t mdio_stat;
 	uint32_t mdio_ctl;
 	uint16_t value;
 	int ret;
+	bool endian = priv->is_little_endian;
 
-	mdio_stat = ioread32be(&regs->mdio_stat);
+	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
 	if (regnum & MII_ADDR_C45) {
 		dev_addr = (regnum >> 16) & 0x1f;
 		mdio_stat |= MDIO_STAT_ENC;
@@ -167,41 +199,41 @@ static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
 		mdio_stat &= ~MDIO_STAT_ENC;
 	}
 
-	iowrite32be(mdio_stat, &regs->mdio_stat);
+	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
 
-	ret = xgmac_wait_until_free(&bus->dev, regs);
+	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
 	if (ret)
 		return ret;
 
 	/* Set the Port and Device Addrs */
 	mdio_ctl = MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr);
-	iowrite32be(mdio_ctl, &regs->mdio_ctl);
+	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
 
 	/* Set the register address */
 	if (regnum & MII_ADDR_C45) {
-		iowrite32be(regnum & 0xffff, &regs->mdio_addr);
+		xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
 
-		ret = xgmac_wait_until_free(&bus->dev, regs);
+		ret = xgmac_wait_until_free(&bus->dev, regs, endian);
 		if (ret)
 			return ret;
 	}
 
 	/* Initiate the read */
-	iowrite32be(mdio_ctl | MDIO_CTL_READ, &regs->mdio_ctl);
+	xgmac_write32(mdio_ctl | MDIO_CTL_READ, &regs->mdio_ctl, endian);
 
-	ret = xgmac_wait_until_done(&bus->dev, regs);
+	ret = xgmac_wait_until_done(&bus->dev, regs, endian);
 	if (ret)
 		return ret;
 
 	/* Return all Fs if nothing was there */
-	if (ioread32be(&regs->mdio_stat) & MDIO_STAT_RD_ER) {
+	if (xgmac_read32(&regs->mdio_stat, endian) & MDIO_STAT_RD_ER) {
 		dev_err(&bus->dev,
 			"Error while reading PHY%d reg at %d.%hhu\n",
 			phy_id, dev_addr, regnum);
 		return 0xffff;
 	}
 
-	value = ioread32be(&regs->mdio_data) & 0xffff;
+	value = xgmac_read32(&regs->mdio_data, endian) & 0xffff;
 	dev_dbg(&bus->dev, "read %04x\n", value);
 
 	return value;
@@ -212,6 +244,7 @@ static int xgmac_mdio_probe(struct platform_device *pdev)
 	struct device_node *np = pdev->dev.of_node;
 	struct mii_bus *bus;
 	struct resource res;
+	struct mdio_fsl_priv *priv;
 	int ret;
 
 	ret = of_address_to_resource(np, 0, &res);
@@ -220,7 +253,7 @@ static int xgmac_mdio_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	bus = mdiobus_alloc();
+	bus = mdiobus_alloc_size(sizeof(struct mdio_fsl_priv));
 	if (!bus)
 		return -ENOMEM;
 
@@ -231,12 +264,19 @@ static int xgmac_mdio_probe(struct platform_device *pdev)
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%llx", (unsigned long long)res.start);
 
 	/* Set the PHY base address */
-	bus->priv = of_iomap(np, 0);
-	if (!bus->priv) {
+	priv = bus->priv;
+	priv->mdio_base = of_iomap(np, 0);
+	if (!priv->mdio_base) {
 		ret = -ENOMEM;
 		goto err_ioremap;
 	}
 
+	if (of_get_property(pdev->dev.of_node,
+			    "little-endian", NULL))
+		priv->is_little_endian = true;
+	else
+		priv->is_little_endian = false;
+
 	ret = of_mdiobus_register(bus, np);
 	if (ret) {
 		dev_err(&pdev->dev, "cannot register MDIO bus\n");
@@ -248,7 +288,7 @@ static int xgmac_mdio_probe(struct platform_device *pdev)
 	return 0;
 
 err_registration:
-	iounmap(bus->priv);
+	iounmap(priv->mdio_base);
 
 err_ioremap:
 	mdiobus_free(bus);
@@ -267,7 +307,7 @@ static int xgmac_mdio_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id xgmac_mdio_match[] = {
+static const struct of_device_id xgmac_mdio_match[] = {
 	{
 		.compatible = "fsl,fman-xmdio",
 	},
diff --git a/drivers/net/ethernet/ibm/ehea/ehea_main.c b/drivers/net/ethernet/ibm/ehea/ehea_main.c
index c05e50759621..291c87036e17 100644
--- a/drivers/net/ethernet/ibm/ehea/ehea_main.c
+++ b/drivers/net/ethernet/ibm/ehea/ehea_main.c
@@ -103,7 +103,7 @@ static int ehea_probe_adapter(struct platform_device *dev);
 
 static int ehea_remove(struct platform_device *dev);
 
-static struct of_device_id ehea_module_device_table[] = {
+static const struct of_device_id ehea_module_device_table[] = {
 	{
 		.name = "lhea",
 		.compatible = "IBM,lhea",
@@ -116,7 +116,7 @@ static struct of_device_id ehea_module_device_table[] = {
 };
 MODULE_DEVICE_TABLE(of, ehea_module_device_table);
 
-static struct of_device_id ehea_device_table[] = {
+static const struct of_device_id ehea_device_table[] = {
 	{
 		.name = "lhea",
 		.compatible = "IBM,lhea",
diff --git a/drivers/net/ethernet/ibm/emac/core.c b/drivers/net/ethernet/ibm/emac/core.c
index 220bae6f5e43..de7919322190 100644
--- a/drivers/net/ethernet/ibm/emac/core.c
+++ b/drivers/net/ethernet/ibm/emac/core.c
@@ -2973,7 +2973,7 @@ static int emac_remove(struct platform_device *ofdev)
 }
 
 /* XXX Features in here should be replaced by properties... */
-static struct of_device_id emac_match[] =
+static const struct of_device_id emac_match[] =
 {
 	{
 		.type		= "network",
diff --git a/drivers/net/ethernet/ibm/emac/mal.c b/drivers/net/ethernet/ibm/emac/mal.c
index dddaab11a4c7..fdb5cdb3cd15 100644
--- a/drivers/net/ethernet/ibm/emac/mal.c
+++ b/drivers/net/ethernet/ibm/emac/mal.c
@@ -753,7 +753,7 @@ static int mal_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id mal_platform_match[] =
+static const struct of_device_id mal_platform_match[] =
 {
 	{
 		.compatible	= "ibm,mcmal",
diff --git a/drivers/net/ethernet/ibm/emac/rgmii.c b/drivers/net/ethernet/ibm/emac/rgmii.c
index 457088fc5b06..206ccbbae7bb 100644
--- a/drivers/net/ethernet/ibm/emac/rgmii.c
+++ b/drivers/net/ethernet/ibm/emac/rgmii.c
@@ -305,7 +305,7 @@ static int rgmii_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id rgmii_match[] =
+static const struct of_device_id rgmii_match[] =
 {
 	{
 		.compatible	= "ibm,rgmii",
diff --git a/drivers/net/ethernet/ibm/emac/tah.c b/drivers/net/ethernet/ibm/emac/tah.c
index cb18e7f917c6..32cb6c9007c5 100644
--- a/drivers/net/ethernet/ibm/emac/tah.c
+++ b/drivers/net/ethernet/ibm/emac/tah.c
@@ -148,7 +148,7 @@ static int tah_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id tah_match[] =
+static const struct of_device_id tah_match[] =
 {
 	{
 		.compatible	= "ibm,tah",
diff --git a/drivers/net/ethernet/ibm/emac/zmii.c b/drivers/net/ethernet/ibm/emac/zmii.c
index 36409ccb75ea..8727b865ea02 100644
--- a/drivers/net/ethernet/ibm/emac/zmii.c
+++ b/drivers/net/ethernet/ibm/emac/zmii.c
@@ -295,7 +295,7 @@ static int zmii_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static struct of_device_id zmii_match[] =
+static const struct of_device_id zmii_match[] =
 {
 	{
 		.compatible	= "ibm,zmii",
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index 072426a72745..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,
diff --git a/drivers/net/ethernet/intel/e100.c b/drivers/net/ethernet/intel/e100.c
index e9c3a87e5b11..1a450f4b6b12 100644
--- a/drivers/net/ethernet/intel/e100.c
+++ b/drivers/net/ethernet/intel/e100.c
@@ -414,7 +414,7 @@ enum cb_status {
 
 /**
  * cb_command - Command Block flags
- * @cb_tx_nc:  0: controler does CRC (normal),  1: CRC from skb memory
+ * @cb_tx_nc:  0: controller does CRC (normal),  1: CRC from skb memory
  */
 enum cb_command {
 	cb_nop    = 0x0000,
@@ -899,7 +899,7 @@ static int e100_exec_cb(struct nic *nic, struct sk_buff *skb,
 	/* Order is important otherwise we'll be in a race with h/w:
 	 * set S-bit in current first, then clear S-bit in previous. */
 	cb->command |= cpu_to_le16(cb_s);
-	wmb();
+	dma_wmb();
 	cb->prev->command &= cpu_to_le16(~cb_s);
 
 	while (nic->cb_to_send != nic->cb_to_use) {
@@ -1843,7 +1843,7 @@ static int e100_tx_clean(struct nic *nic)
 	for (cb = nic->cb_to_clean;
 	    cb->status & cpu_to_le16(cb_complete);
 	    cb = nic->cb_to_clean = cb->next) {
-		rmb(); /* read skb after status */
+		dma_rmb(); /* read skb after status */
 		netif_printk(nic, tx_done, KERN_DEBUG, nic->netdev,
 			     "cb[%d]->status = 0x%04X\n",
 			     (int)(((void*)cb - (void*)nic->cbs)/sizeof(struct cb)),
@@ -1993,7 +1993,7 @@ static int e100_rx_indicate(struct nic *nic, struct rx *rx,
 
 	netif_printk(nic, rx_status, KERN_DEBUG, nic->netdev,
 		     "status=0x%04X\n", rfd_status);
-	rmb(); /* read size after status bit */
+	dma_rmb(); /* read size after status bit */
 
 	/* If data isn't ready, nothing to indicate */
 	if (unlikely(!(rfd_status & cb_complete))) {
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index 7f997d36948f..983eb4e6f7aa 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -144,6 +144,11 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 				     struct e1000_rx_ring *rx_ring,
 				     int *work_done, int work_to_do);
+static void e1000_alloc_dummy_rx_buffers(struct e1000_adapter *adapter,
+					 struct e1000_rx_ring *rx_ring,
+					 int cleaned_count)
+{
+}
 static void e1000_alloc_rx_buffers(struct e1000_adapter *adapter,
 				   struct e1000_rx_ring *rx_ring,
 				   int cleaned_count);
@@ -516,6 +521,7 @@ void e1000_down(struct e1000_adapter *adapter)
 	struct net_device *netdev = adapter->netdev;
 	u32 rctl, tctl;
 
+	netif_carrier_off(netdev);
 
 	/* disable receives in the hardware */
 	rctl = er32(RCTL);
@@ -544,7 +550,6 @@ void e1000_down(struct e1000_adapter *adapter)
 
 	adapter->link_speed = 0;
 	adapter->link_duplex = 0;
-	netif_carrier_off(netdev);
 
 	e1000_reset(adapter);
 	e1000_clean_all_tx_rings(adapter);
@@ -1111,7 +1116,7 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		if (e1000_read_mac_addr(hw))
 			e_err(probe, "EEPROM Read Error\n");
 	}
-	/* don't block initalization here due to bad MAC address */
+	/* don't block initialization here due to bad MAC address */
 	memcpy(netdev->dev_addr, hw->mac_addr, netdev->addr_len);
 
 	if (!is_valid_ether_addr(netdev->dev_addr))
@@ -3552,8 +3557,11 @@ static int e1000_change_mtu(struct net_device *netdev, int new_mtu)
 		msleep(1);
 	/* e1000_down has a dependency on max_frame_size */
 	hw->max_frame_size = max_frame;
-	if (netif_running(netdev))
+	if (netif_running(netdev)) {
+		/* prevent buffers from being reallocated */
+		adapter->alloc_rx_buf = e1000_alloc_dummy_rx_buffers;
 		e1000_down(adapter);
+	}
 
 	/* NOTE: netdev_alloc_skb reserves 16 bytes, and typically NET_IP_ALIGN
 	 * means we reserve 2 more, this pushes us to allocate from the next
@@ -3848,7 +3856,7 @@ static bool e1000_clean_tx_irq(struct e1000_adapter *adapter,
 	while ((eop_desc->upper.data & cpu_to_le32(E1000_TXD_STAT_DD)) &&
 	       (count < tx_ring->count)) {
 		bool cleaned = false;
-		rmb();	/* read buffer_info after eop_desc */
+		dma_rmb();	/* read buffer_info after eop_desc */
 		for ( ; !cleaned; count++) {
 			tx_desc = E1000_TX_DESC(*tx_ring, i);
 			buffer_info = &tx_ring->buffer_info[i];
@@ -4146,7 +4154,7 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 		if (*work_done >= work_to_do)
 			break;
 		(*work_done)++;
-		rmb(); /* read descriptor and rx_buffer_info after status DD */
+		dma_rmb(); /* read descriptor and rx_buffer_info after status DD */
 
 		status = rx_desc->status;
 
@@ -4367,7 +4375,7 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 		if (*work_done >= work_to_do)
 			break;
 		(*work_done)++;
-		rmb(); /* read descriptor and rx_buffer_info after status DD */
+		dma_rmb(); /* read descriptor and rx_buffer_info after status DD */
 
 		status = rx_desc->status;
 		length = le16_to_cpu(rx_desc->length);
diff --git a/drivers/net/ethernet/intel/e1000e/defines.h b/drivers/net/ethernet/intel/e1000e/defines.h
index bb7ab3c321d6..0570c668ec3d 100644
--- a/drivers/net/ethernet/intel/e1000e/defines.h
+++ b/drivers/net/ethernet/intel/e1000e/defines.h
@@ -141,6 +141,7 @@
 #define E1000_RCTL_LBM_TCVR       0x000000C0    /* tcvr loopback mode */
 #define E1000_RCTL_DTYP_PS        0x00000400    /* Packet Split descriptor */
 #define E1000_RCTL_RDMTS_HALF     0x00000000    /* Rx desc min threshold size */
+#define E1000_RCTL_RDMTS_HEX      0x00010000
 #define E1000_RCTL_MO_SHIFT       12            /* multicast offset shift */
 #define E1000_RCTL_MO_3           0x00003000    /* multicast offset 15:4 */
 #define E1000_RCTL_BAM            0x00008000    /* broadcast enable */
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index 9416e5a7e0c8..5d9ceb17b4cb 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -132,6 +132,7 @@ enum e1000_boards {
 	board_pchlan,
 	board_pch2lan,
 	board_pch_lpt,
+	board_pch_spt
 };
 
 struct e1000_ps_page {
@@ -342,6 +343,7 @@ struct e1000_adapter {
 	struct timecounter tc;
 	struct ptp_clock *ptp_clock;
 	struct ptp_clock_info ptp_clock_info;
+	struct pm_qos_request pm_qos_req;
 
 	u16 eee_advert;
 };
@@ -501,6 +503,7 @@ extern const struct e1000_info e1000_ich10_info;
 extern const struct e1000_info e1000_pch_info;
 extern const struct e1000_info e1000_pch2_info;
 extern const struct e1000_info e1000_pch_lpt_info;
+extern const struct e1000_info e1000_pch_spt_info;
 extern const struct e1000_info e1000_es2_info;
 
 void e1000e_ptp_init(struct e1000_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/e1000e/ethtool.c b/drivers/net/ethernet/intel/e1000e/ethtool.c
index 865ce45f9ec3..11f486e4ff7b 100644
--- a/drivers/net/ethernet/intel/e1000e/ethtool.c
+++ b/drivers/net/ethernet/intel/e1000e/ethtool.c
@@ -896,18 +896,20 @@ static int e1000_reg_test(struct e1000_adapter *adapter, u64 *data)
 	case e1000_pchlan:
 	case e1000_pch2lan:
 	case e1000_pch_lpt:
+	case e1000_pch_spt:
 		mask |= (1 << 18);
 		break;
 	default:
 		break;
 	}
 
-	if (mac->type == e1000_pch_lpt)
+	if ((mac->type == e1000_pch_lpt) || (mac->type == e1000_pch_spt))
 		wlock_mac = (er32(FWSM) & E1000_FWSM_WLOCK_MAC_MASK) >>
 		    E1000_FWSM_WLOCK_MAC_SHIFT;
 
 	for (i = 0; i < mac->rar_entry_count; i++) {
-		if (mac->type == e1000_pch_lpt) {
+		if ((mac->type == e1000_pch_lpt) ||
+		    (mac->type == e1000_pch_spt)) {
 			/* Cannot test write-protected SHRAL[n] registers */
 			if ((wlock_mac == 1) || (wlock_mac && (i > wlock_mac)))
 				continue;
diff --git a/drivers/net/ethernet/intel/e1000e/hw.h b/drivers/net/ethernet/intel/e1000e/hw.h
index 72f5475c4b90..19e8c487db06 100644
--- a/drivers/net/ethernet/intel/e1000e/hw.h
+++ b/drivers/net/ethernet/intel/e1000e/hw.h
@@ -87,6 +87,10 @@ struct e1000_hw;
 #define E1000_DEV_ID_PCH_I218_V2		0x15A1
 #define E1000_DEV_ID_PCH_I218_LM3		0x15A2	/* Wildcat Point PCH */
 #define E1000_DEV_ID_PCH_I218_V3		0x15A3	/* Wildcat Point PCH */
+#define E1000_DEV_ID_PCH_SPT_I219_LM		0x156F	/* SPT PCH */
+#define E1000_DEV_ID_PCH_SPT_I219_V		0x1570	/* SPT PCH */
+#define E1000_DEV_ID_PCH_SPT_I219_LM2		0x15B7	/* SPT-H PCH */
+#define E1000_DEV_ID_PCH_SPT_I219_V2		0x15B8	/* SPT-H PCH */
 
 #define E1000_REVISION_4	4
 
@@ -108,6 +112,7 @@ enum e1000_mac_type {
 	e1000_pchlan,
 	e1000_pch2lan,
 	e1000_pch_lpt,
+	e1000_pch_spt,
 };
 
 enum e1000_media_type {
@@ -153,6 +158,7 @@ enum e1000_bus_width {
 	e1000_bus_width_pcie_x1,
 	e1000_bus_width_pcie_x2,
 	e1000_bus_width_pcie_x4 = 4,
+	e1000_bus_width_pcie_x8 = 8,
 	e1000_bus_width_32,
 	e1000_bus_width_64,
 	e1000_bus_width_reserved
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.c b/drivers/net/ethernet/intel/e1000e/ich8lan.c
index 48b74a549155..9d81c0317433 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.c
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.c
@@ -123,6 +123,14 @@ static s32 e1000_read_flash_word_ich8lan(struct e1000_hw *hw, u32 offset,
 					 u16 *data);
 static s32 e1000_read_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 					 u8 size, u16 *data);
+static s32 e1000_read_flash_data32_ich8lan(struct e1000_hw *hw, u32 offset,
+					   u32 *data);
+static s32 e1000_read_flash_dword_ich8lan(struct e1000_hw *hw,
+					  u32 offset, u32 *data);
+static s32 e1000_write_flash_data32_ich8lan(struct e1000_hw *hw,
+					    u32 offset, u32 data);
+static s32 e1000_retry_write_flash_dword_ich8lan(struct e1000_hw *hw,
+						 u32 offset, u32 dword);
 static s32 e1000_kmrn_lock_loss_workaround_ich8lan(struct e1000_hw *hw);
 static s32 e1000_cleanup_led_ich8lan(struct e1000_hw *hw);
 static s32 e1000_led_on_ich8lan(struct e1000_hw *hw);
@@ -229,7 +237,8 @@ static bool e1000_phy_is_accessible_pchlan(struct e1000_hw *hw)
 	if (ret_val)
 		return false;
 out:
-	if (hw->mac.type == e1000_pch_lpt) {
+	if ((hw->mac.type == e1000_pch_lpt) ||
+	    (hw->mac.type == e1000_pch_spt)) {
 		/* Unforce SMBus mode in PHY */
 		e1e_rphy_locked(hw, CV_SMB_CTRL, &phy_reg);
 		phy_reg &= ~CV_SMB_CTRL_FORCE_SMBUS;
@@ -321,6 +330,7 @@ static s32 e1000_init_phy_workarounds_pchlan(struct e1000_hw *hw)
 	 */
 	switch (hw->mac.type) {
 	case e1000_pch_lpt:
+	case e1000_pch_spt:
 		if (e1000_phy_is_accessible_pchlan(hw))
 			break;
 
@@ -461,6 +471,7 @@ static s32 e1000_init_phy_params_pchlan(struct e1000_hw *hw)
 			/* fall-through */
 		case e1000_pch2lan:
 		case e1000_pch_lpt:
+		case e1000_pch_spt:
 			/* In case the PHY needs to be in mdio slow mode,
 			 * set slow mode and try to get the PHY id again.
 			 */
@@ -590,35 +601,54 @@ static s32 e1000_init_nvm_params_ich8lan(struct e1000_hw *hw)
 	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
 	u32 gfpreg, sector_base_addr, sector_end_addr;
 	u16 i;
-
-	/* Can't read flash registers if the register set isn't mapped. */
-	if (!hw->flash_address) {
-		e_dbg("ERROR: Flash registers not mapped\n");
-		return -E1000_ERR_CONFIG;
-	}
+	u32 nvm_size;
 
 	nvm->type = e1000_nvm_flash_sw;
 
-	gfpreg = er32flash(ICH_FLASH_GFPREG);
+	if (hw->mac.type == e1000_pch_spt) {
+		/* in SPT, gfpreg doesn't exist. NVM size is taken from the
+		 * STRAP register. This is because in SPT the GbE Flash region
+		 * is no longer accessed through the flash registers. Instead,
+		 * the mechanism has changed, and the Flash region access
+		 * registers are now implemented in GbE memory space.
+		 */
+		nvm->flash_base_addr = 0;
+		nvm_size = (((er32(STRAP) >> 1) & 0x1F) + 1)
+		    * NVM_SIZE_MULTIPLIER;
+		nvm->flash_bank_size = nvm_size / 2;
+		/* Adjust to word count */
+		nvm->flash_bank_size /= sizeof(u16);
+		/* Set the base address for flash register access */
+		hw->flash_address = hw->hw_addr + E1000_FLASH_BASE_ADDR;
+	} else {
+		/* Can't read flash registers if register set isn't mapped. */
+		if (!hw->flash_address) {
+			e_dbg("ERROR: Flash registers not mapped\n");
+			return -E1000_ERR_CONFIG;
+		}
 
-	/* sector_X_addr is a "sector"-aligned address (4096 bytes)
-	 * Add 1 to sector_end_addr since this sector is included in
-	 * the overall size.
-	 */
-	sector_base_addr = gfpreg & FLASH_GFPREG_BASE_MASK;
-	sector_end_addr = ((gfpreg >> 16) & FLASH_GFPREG_BASE_MASK) + 1;
+		gfpreg = er32flash(ICH_FLASH_GFPREG);
 
-	/* flash_base_addr is byte-aligned */
-	nvm->flash_base_addr = sector_base_addr << FLASH_SECTOR_ADDR_SHIFT;
+		/* sector_X_addr is a "sector"-aligned address (4096 bytes)
+		 * Add 1 to sector_end_addr since this sector is included in
+		 * the overall size.
+		 */
+		sector_base_addr = gfpreg & FLASH_GFPREG_BASE_MASK;
+		sector_end_addr = ((gfpreg >> 16) & FLASH_GFPREG_BASE_MASK) + 1;
 
-	/* find total size of the NVM, then cut in half since the total
-	 * size represents two separate NVM banks.
-	 */
-	nvm->flash_bank_size = ((sector_end_addr - sector_base_addr)
-				<< FLASH_SECTOR_ADDR_SHIFT);
-	nvm->flash_bank_size /= 2;
-	/* Adjust to word count */
-	nvm->flash_bank_size /= sizeof(u16);
+		/* flash_base_addr is byte-aligned */
+		nvm->flash_base_addr = sector_base_addr
+		    << FLASH_SECTOR_ADDR_SHIFT;
+
+		/* find total size of the NVM, then cut in half since the total
+		 * size represents two separate NVM banks.
+		 */
+		nvm->flash_bank_size = ((sector_end_addr - sector_base_addr)
+					<< FLASH_SECTOR_ADDR_SHIFT);
+		nvm->flash_bank_size /= 2;
+		/* Adjust to word count */
+		nvm->flash_bank_size /= sizeof(u16);
+	}
 
 	nvm->word_size = E1000_ICH8_SHADOW_RAM_WORDS;
 
@@ -682,6 +712,7 @@ static s32 e1000_init_mac_params_ich8lan(struct e1000_hw *hw)
 		mac->ops.rar_set = e1000_rar_set_pch2lan;
 		/* fall-through */
 	case e1000_pch_lpt:
+	case e1000_pch_spt:
 	case e1000_pchlan:
 		/* check management mode */
 		mac->ops.check_mng_mode = e1000_check_mng_mode_pchlan;
@@ -699,7 +730,7 @@ static s32 e1000_init_mac_params_ich8lan(struct e1000_hw *hw)
 		break;
 	}
 
-	if (mac->type == e1000_pch_lpt) {
+	if ((mac->type == e1000_pch_lpt) || (mac->type == e1000_pch_spt)) {
 		mac->rar_entry_count = E1000_PCH_LPT_RAR_ENTRIES;
 		mac->ops.rar_set = e1000_rar_set_pch_lpt;
 		mac->ops.setup_physical_interface =
@@ -919,8 +950,9 @@ release:
 		/* clear FEXTNVM6 bit 8 on link down or 10/100 */
 		fextnvm6 &= ~E1000_FEXTNVM6_REQ_PLL_CLK;
 
-		if (!link || ((status & E1000_STATUS_SPEED_100) &&
-			      (status & E1000_STATUS_FD)))
+		if ((hw->phy.revision > 5) || !link ||
+		    ((status & E1000_STATUS_SPEED_100) &&
+		     (status & E1000_STATUS_FD)))
 			goto update_fextnvm6;
 
 		ret_val = e1e_rphy(hw, I217_INBAND_CTRL, &reg);
@@ -1100,6 +1132,21 @@ s32 e1000_enable_ulp_lpt_lp(struct e1000_hw *hw, bool to_sx)
 	if (ret_val)
 		goto out;
 
+	/* Si workaround for ULP entry flow on i127/rev6 h/w.  Enable
+	 * LPLU and disable Gig speed when entering ULP
+	 */
+	if ((hw->phy.type == e1000_phy_i217) && (hw->phy.revision == 6)) {
+		ret_val = e1000_read_phy_reg_hv_locked(hw, HV_OEM_BITS,
+						       &phy_reg);
+		if (ret_val)
+			goto release;
+		phy_reg |= HV_OEM_BITS_LPLU | HV_OEM_BITS_GBE_DIS;
+		ret_val = e1000_write_phy_reg_hv_locked(hw, HV_OEM_BITS,
+							phy_reg);
+		if (ret_val)
+			goto release;
+	}
+
 	/* Force SMBus mode in PHY */
 	ret_val = e1000_read_phy_reg_hv_locked(hw, CV_SMB_CTRL, &phy_reg);
 	if (ret_val)
@@ -1302,7 +1349,8 @@ out:
 static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 {
 	struct e1000_mac_info *mac = &hw->mac;
-	s32 ret_val;
+	s32 ret_val, tipg_reg = 0;
+	u16 emi_addr, emi_val = 0;
 	bool link;
 	u16 phy_reg;
 
@@ -1333,48 +1381,55 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 	 * the IPG and reduce Rx latency in the PHY.
 	 */
 	if (((hw->mac.type == e1000_pch2lan) ||
-	     (hw->mac.type == e1000_pch_lpt)) && link) {
+	     (hw->mac.type == e1000_pch_lpt) ||
+	     (hw->mac.type == e1000_pch_spt)) && link) {
 		u32 reg;
 
 		reg = er32(STATUS);
+		tipg_reg = er32(TIPG);
+		tipg_reg &= ~E1000_TIPG_IPGT_MASK;
+
 		if (!(reg & (E1000_STATUS_FD | E1000_STATUS_SPEED_MASK))) {
-			u16 emi_addr;
+			tipg_reg |= 0xFF;
+			/* Reduce Rx latency in analog PHY */
+			emi_val = 0;
+		} else {
 
-			reg = er32(TIPG);
-			reg &= ~E1000_TIPG_IPGT_MASK;
-			reg |= 0xFF;
-			ew32(TIPG, reg);
+			/* Roll back the default values */
+			tipg_reg |= 0x08;
+			emi_val = 1;
+		}
 
-			/* Reduce Rx latency in analog PHY */
-			ret_val = hw->phy.ops.acquire(hw);
-			if (ret_val)
-				return ret_val;
+		ew32(TIPG, tipg_reg);
 
-			if (hw->mac.type == e1000_pch2lan)
-				emi_addr = I82579_RX_CONFIG;
-			else
-				emi_addr = I217_RX_CONFIG;
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val)
+			return ret_val;
 
-			ret_val = e1000_write_emi_reg_locked(hw, emi_addr, 0);
+		if (hw->mac.type == e1000_pch2lan)
+			emi_addr = I82579_RX_CONFIG;
+		else
+			emi_addr = I217_RX_CONFIG;
+		ret_val = e1000_write_emi_reg_locked(hw, emi_addr, emi_val);
 
-			hw->phy.ops.release(hw);
+		hw->phy.ops.release(hw);
 
-			if (ret_val)
-				return ret_val;
-		}
+		if (ret_val)
+			return ret_val;
 	}
 
 	/* Work-around I218 hang issue */
 	if ((hw->adapter->pdev->device == E1000_DEV_ID_PCH_LPTLP_I218_LM) ||
 	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_LPTLP_I218_V) ||
 	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_I218_LM3) ||
-	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_I218_V3)) {
+	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_I218_V3) ||
+	    (hw->mac.type == e1000_pch_spt)) {
 		ret_val = e1000_k1_workaround_lpt_lp(hw, link);
 		if (ret_val)
 			return ret_val;
 	}
-
-	if (hw->mac.type == e1000_pch_lpt) {
+	if ((hw->mac.type == e1000_pch_lpt) ||
+	    (hw->mac.type == e1000_pch_spt)) {
 		/* Set platform power management values for
 		 * Latency Tolerance Reporting (LTR)
 		 */
@@ -1386,6 +1441,19 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 	/* Clear link partner's EEE ability */
 	hw->dev_spec.ich8lan.eee_lp_ability = 0;
 
+	/* FEXTNVM6 K1-off workaround */
+	if (hw->mac.type == e1000_pch_spt) {
+		u32 pcieanacfg = er32(PCIEANACFG);
+		u32 fextnvm6 = er32(FEXTNVM6);
+
+		if (pcieanacfg & E1000_FEXTNVM6_K1_OFF_ENABLE)
+			fextnvm6 |= E1000_FEXTNVM6_K1_OFF_ENABLE;
+		else
+			fextnvm6 &= ~E1000_FEXTNVM6_K1_OFF_ENABLE;
+
+		ew32(FEXTNVM6, fextnvm6);
+	}
+
 	if (!link)
 		return 0;	/* No link detected */
 
@@ -1479,6 +1547,7 @@ static s32 e1000_get_variants_ich8lan(struct e1000_adapter *adapter)
 	case e1000_pchlan:
 	case e1000_pch2lan:
 	case e1000_pch_lpt:
+	case e1000_pch_spt:
 		rc = e1000_init_phy_params_pchlan(hw);
 		break;
 	default:
@@ -1929,6 +1998,7 @@ static s32 e1000_sw_lcd_config_ich8lan(struct e1000_hw *hw)
 	case e1000_pchlan:
 	case e1000_pch2lan:
 	case e1000_pch_lpt:
+	case e1000_pch_spt:
 		sw_cfg_mask = E1000_FEXTNVM_SW_CONFIG_ICH8M;
 		break;
 	default:
@@ -2961,6 +3031,20 @@ static s32 e1000_valid_nvm_bank_detect_ich8lan(struct e1000_hw *hw, u32 *bank)
 	s32 ret_val;
 
 	switch (hw->mac.type) {
+		/* In SPT, read from the CTRL_EXT reg instead of
+		 * accessing the sector valid bits from the nvm
+		 */
+	case e1000_pch_spt:
+		*bank = er32(CTRL_EXT)
+		    & E1000_CTRL_EXT_NVMVS;
+		if ((*bank == 0) || (*bank == 1)) {
+			e_dbg("ERROR: No valid NVM bank present\n");
+			return -E1000_ERR_NVM;
+		} else {
+			*bank = *bank - 2;
+			return 0;
+		}
+		break;
 	case e1000_ich8lan:
 	case e1000_ich9lan:
 		eecd = er32(EECD);
@@ -3008,6 +3092,99 @@ static s32 e1000_valid_nvm_bank_detect_ich8lan(struct e1000_hw *hw, u32 *bank)
 }
 
 /**
+ *  e1000_read_nvm_spt - NVM access for SPT
+ *  @hw: pointer to the HW structure
+ *  @offset: The offset (in bytes) of the word(s) to read.
+ *  @words: Size of data to read in words.
+ *  @data: pointer to the word(s) to read at offset.
+ *
+ *  Reads a word(s) from the NVM
+ **/
+static s32 e1000_read_nvm_spt(struct e1000_hw *hw, u16 offset, u16 words,
+			      u16 *data)
+{
+	struct e1000_nvm_info *nvm = &hw->nvm;
+	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
+	u32 act_offset;
+	s32 ret_val = 0;
+	u32 bank = 0;
+	u32 dword = 0;
+	u16 offset_to_read;
+	u16 i;
+
+	if ((offset >= nvm->word_size) || (words > nvm->word_size - offset) ||
+	    (words == 0)) {
+		e_dbg("nvm parameter(s) out of bounds\n");
+		ret_val = -E1000_ERR_NVM;
+		goto out;
+	}
+
+	nvm->ops.acquire(hw);
+
+	ret_val = e1000_valid_nvm_bank_detect_ich8lan(hw, &bank);
+	if (ret_val) {
+		e_dbg("Could not detect valid bank, assuming bank 0\n");
+		bank = 0;
+	}
+
+	act_offset = (bank) ? nvm->flash_bank_size : 0;
+	act_offset += offset;
+
+	ret_val = 0;
+
+	for (i = 0; i < words; i += 2) {
+		if (words - i == 1) {
+			if (dev_spec->shadow_ram[offset + i].modified) {
+				data[i] =
+				    dev_spec->shadow_ram[offset + i].value;
+			} else {
+				offset_to_read = act_offset + i -
+				    ((act_offset + i) % 2);
+				ret_val =
+				  e1000_read_flash_dword_ich8lan(hw,
+								 offset_to_read,
+								 &dword);
+				if (ret_val)
+					break;
+				if ((act_offset + i) % 2 == 0)
+					data[i] = (u16)(dword & 0xFFFF);
+				else
+					data[i] = (u16)((dword >> 16) & 0xFFFF);
+			}
+		} else {
+			offset_to_read = act_offset + i;
+			if (!(dev_spec->shadow_ram[offset + i].modified) ||
+			    !(dev_spec->shadow_ram[offset + i + 1].modified)) {
+				ret_val =
+				  e1000_read_flash_dword_ich8lan(hw,
+								 offset_to_read,
+								 &dword);
+				if (ret_val)
+					break;
+			}
+			if (dev_spec->shadow_ram[offset + i].modified)
+				data[i] =
+				    dev_spec->shadow_ram[offset + i].value;
+			else
+				data[i] = (u16)(dword & 0xFFFF);
+			if (dev_spec->shadow_ram[offset + i].modified)
+				data[i + 1] =
+				    dev_spec->shadow_ram[offset + i + 1].value;
+			else
+				data[i + 1] = (u16)(dword >> 16 & 0xFFFF);
+		}
+	}
+
+	nvm->ops.release(hw);
+
+out:
+	if (ret_val)
+		e_dbg("NVM read error: %d\n", ret_val);
+
+	return ret_val;
+}
+
+/**
  *  e1000_read_nvm_ich8lan - Read word(s) from the NVM
  *  @hw: pointer to the HW structure
  *  @offset: The offset (in bytes) of the word(s) to read.
@@ -3090,8 +3267,10 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 	/* Clear FCERR and DAEL in hw status by writing 1 */
 	hsfsts.hsf_status.flcerr = 1;
 	hsfsts.hsf_status.dael = 1;
-
-	ew16flash(ICH_FLASH_HSFSTS, hsfsts.regval);
+	if (hw->mac.type == e1000_pch_spt)
+		ew32flash(ICH_FLASH_HSFSTS, hsfsts.regval & 0xFFFF);
+	else
+		ew16flash(ICH_FLASH_HSFSTS, hsfsts.regval);
 
 	/* Either we should have a hardware SPI cycle in progress
 	 * bit to check against, in order to start a new cycle or
@@ -3107,7 +3286,10 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 		 * Begin by setting Flash Cycle Done.
 		 */
 		hsfsts.hsf_status.flcdone = 1;
-		ew16flash(ICH_FLASH_HSFSTS, hsfsts.regval);
+		if (hw->mac.type == e1000_pch_spt)
+			ew32flash(ICH_FLASH_HSFSTS, hsfsts.regval & 0xFFFF);
+		else
+			ew16flash(ICH_FLASH_HSFSTS, hsfsts.regval);
 		ret_val = 0;
 	} else {
 		s32 i;
@@ -3128,7 +3310,11 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 			 * now set the Flash Cycle Done.
 			 */
 			hsfsts.hsf_status.flcdone = 1;
-			ew16flash(ICH_FLASH_HSFSTS, hsfsts.regval);
+			if (hw->mac.type == e1000_pch_spt)
+				ew32flash(ICH_FLASH_HSFSTS,
+					  hsfsts.regval & 0xFFFF);
+			else
+				ew16flash(ICH_FLASH_HSFSTS, hsfsts.regval);
 		} else {
 			e_dbg("Flash controller busy, cannot get access\n");
 		}
@@ -3151,9 +3337,16 @@ static s32 e1000_flash_cycle_ich8lan(struct e1000_hw *hw, u32 timeout)
 	u32 i = 0;
 
 	/* Start a cycle by writing 1 in Flash Cycle Go in Hw Flash Control */
-	hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
+	if (hw->mac.type == e1000_pch_spt)
+		hsflctl.regval = er32flash(ICH_FLASH_HSFSTS) >> 16;
+	else
+		hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
 	hsflctl.hsf_ctrl.flcgo = 1;
-	ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
+
+	if (hw->mac.type == e1000_pch_spt)
+		ew32flash(ICH_FLASH_HSFSTS, hsflctl.regval << 16);
+	else
+		ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
 
 	/* wait till FDONE bit is set to 1 */
 	do {
@@ -3170,6 +3363,23 @@ static s32 e1000_flash_cycle_ich8lan(struct e1000_hw *hw, u32 timeout)
 }
 
 /**
+ *  e1000_read_flash_dword_ich8lan - Read dword from flash
+ *  @hw: pointer to the HW structure
+ *  @offset: offset to data location
+ *  @data: pointer to the location for storing the data
+ *
+ *  Reads the flash dword at offset into data.  Offset is converted
+ *  to bytes before read.
+ **/
+static s32 e1000_read_flash_dword_ich8lan(struct e1000_hw *hw, u32 offset,
+					  u32 *data)
+{
+	/* Must convert word offset into bytes. */
+	offset <<= 1;
+	return e1000_read_flash_data32_ich8lan(hw, offset, data);
+}
+
+/**
  *  e1000_read_flash_word_ich8lan - Read word from flash
  *  @hw: pointer to the HW structure
  *  @offset: offset to data location
@@ -3201,7 +3411,14 @@ static s32 e1000_read_flash_byte_ich8lan(struct e1000_hw *hw, u32 offset,
 	s32 ret_val;
 	u16 word = 0;
 
-	ret_val = e1000_read_flash_data_ich8lan(hw, offset, 1, &word);
+	/* In SPT, only 32 bits access is supported,
+	 * so this function should not be called.
+	 */
+	if (hw->mac.type == e1000_pch_spt)
+		return -E1000_ERR_NVM;
+	else
+		ret_val = e1000_read_flash_data_ich8lan(hw, offset, 1, &word);
+
 	if (ret_val)
 		return ret_val;
 
@@ -3287,6 +3504,82 @@ static s32 e1000_read_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 }
 
 /**
+ *  e1000_read_flash_data32_ich8lan - Read dword from NVM
+ *  @hw: pointer to the HW structure
+ *  @offset: The offset (in bytes) of the dword to read.
+ *  @data: Pointer to the dword to store the value read.
+ *
+ *  Reads a byte or word from the NVM using the flash access registers.
+ **/
+
+static s32 e1000_read_flash_data32_ich8lan(struct e1000_hw *hw, u32 offset,
+					   u32 *data)
+{
+	union ich8_hws_flash_status hsfsts;
+	union ich8_hws_flash_ctrl hsflctl;
+	u32 flash_linear_addr;
+	s32 ret_val = -E1000_ERR_NVM;
+	u8 count = 0;
+
+	if (offset > ICH_FLASH_LINEAR_ADDR_MASK ||
+	    hw->mac.type != e1000_pch_spt)
+		return -E1000_ERR_NVM;
+	flash_linear_addr = ((ICH_FLASH_LINEAR_ADDR_MASK & offset) +
+			     hw->nvm.flash_base_addr);
+
+	do {
+		udelay(1);
+		/* Steps */
+		ret_val = e1000_flash_cycle_init_ich8lan(hw);
+		if (ret_val)
+			break;
+		/* In SPT, This register is in Lan memory space, not flash.
+		 * Therefore, only 32 bit access is supported
+		 */
+		hsflctl.regval = er32flash(ICH_FLASH_HSFSTS) >> 16;
+
+		/* 0b/1b corresponds to 1 or 2 byte size, respectively. */
+		hsflctl.hsf_ctrl.fldbcount = sizeof(u32) - 1;
+		hsflctl.hsf_ctrl.flcycle = ICH_CYCLE_READ;
+		/* In SPT, This register is in Lan memory space, not flash.
+		 * Therefore, only 32 bit access is supported
+		 */
+		ew32flash(ICH_FLASH_HSFSTS, (u32)hsflctl.regval << 16);
+		ew32flash(ICH_FLASH_FADDR, flash_linear_addr);
+
+		ret_val =
+		   e1000_flash_cycle_ich8lan(hw,
+					     ICH_FLASH_READ_COMMAND_TIMEOUT);
+
+		/* Check if FCERR is set to 1, if set to 1, clear it
+		 * and try the whole sequence a few more times, else
+		 * read in (shift in) the Flash Data0, the order is
+		 * least significant byte first msb to lsb
+		 */
+		if (!ret_val) {
+			*data = er32flash(ICH_FLASH_FDATA0);
+			break;
+		} else {
+			/* If we've gotten here, then things are probably
+			 * completely hosed, but if the error condition is
+			 * detected, it won't hurt to give it another try...
+			 * ICH_FLASH_CYCLE_REPEAT_COUNT times.
+			 */
+			hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
+			if (hsfsts.hsf_status.flcerr) {
+				/* Repeat for some time before giving up. */
+				continue;
+			} else if (!hsfsts.hsf_status.flcdone) {
+				e_dbg("Timeout error - flash cycle did not complete.\n");
+				break;
+			}
+		}
+	} while (count++ < ICH_FLASH_CYCLE_REPEAT_COUNT);
+
+	return ret_val;
+}
+
+/**
  *  e1000_write_nvm_ich8lan - Write word(s) to the NVM
  *  @hw: pointer to the HW structure
  *  @offset: The offset (in bytes) of the word(s) to write.
@@ -3321,7 +3614,7 @@ static s32 e1000_write_nvm_ich8lan(struct e1000_hw *hw, u16 offset, u16 words,
 }
 
 /**
- *  e1000_update_nvm_checksum_ich8lan - Update the checksum for NVM
+ *  e1000_update_nvm_checksum_spt - Update the checksum for NVM
  *  @hw: pointer to the HW structure
  *
  *  The NVM checksum is updated by calling the generic update_nvm_checksum,
@@ -3331,13 +3624,13 @@ static s32 e1000_write_nvm_ich8lan(struct e1000_hw *hw, u16 offset, u16 words,
  *  After a successful commit, the shadow ram is cleared and is ready for
  *  future writes.
  **/
-static s32 e1000_update_nvm_checksum_ich8lan(struct e1000_hw *hw)
+static s32 e1000_update_nvm_checksum_spt(struct e1000_hw *hw)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
 	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
 	u32 i, act_offset, new_bank_offset, old_bank_offset, bank;
 	s32 ret_val;
-	u16 data;
+	u32 dword = 0;
 
 	ret_val = e1000e_update_nvm_checksum_generic(hw);
 	if (ret_val)
@@ -3371,12 +3664,175 @@ static s32 e1000_update_nvm_checksum_ich8lan(struct e1000_hw *hw)
 		if (ret_val)
 			goto release;
 	}
-
-	for (i = 0; i < E1000_ICH8_SHADOW_RAM_WORDS; i++) {
+	for (i = 0; i < E1000_ICH8_SHADOW_RAM_WORDS; i += 2) {
 		/* Determine whether to write the value stored
 		 * in the other NVM bank or a modified value stored
 		 * in the shadow RAM
 		 */
+		ret_val = e1000_read_flash_dword_ich8lan(hw,
+							 i + old_bank_offset,
+							 &dword);
+
+		if (dev_spec->shadow_ram[i].modified) {
+			dword &= 0xffff0000;
+			dword |= (dev_spec->shadow_ram[i].value & 0xffff);
+		}
+		if (dev_spec->shadow_ram[i + 1].modified) {
+			dword &= 0x0000ffff;
+			dword |= ((dev_spec->shadow_ram[i + 1].value & 0xffff)
+				  << 16);
+		}
+		if (ret_val)
+			break;
+
+		/* If the word is 0x13, then make sure the signature bits
+		 * (15:14) are 11b until the commit has completed.
+		 * This will allow us to write 10b which indicates the
+		 * signature is valid.  We want to do this after the write
+		 * has completed so that we don't mark the segment valid
+		 * while the write is still in progress
+		 */
+		if (i == E1000_ICH_NVM_SIG_WORD - 1)
+			dword |= E1000_ICH_NVM_SIG_MASK << 16;
+
+		/* Convert offset to bytes. */
+		act_offset = (i + new_bank_offset) << 1;
+
+		usleep_range(100, 200);
+
+		/* Write the data to the new bank. Offset in words */
+		act_offset = i + new_bank_offset;
+		ret_val = e1000_retry_write_flash_dword_ich8lan(hw, act_offset,
+								dword);
+		if (ret_val)
+			break;
+	}
+
+	/* Don't bother writing the segment valid bits if sector
+	 * programming failed.
+	 */
+	if (ret_val) {
+		/* Possibly read-only, see e1000e_write_protect_nvm_ich8lan() */
+		e_dbg("Flash commit failed.\n");
+		goto release;
+	}
+
+	/* Finally validate the new segment by setting bit 15:14
+	 * to 10b in word 0x13 , this can be done without an
+	 * erase as well since these bits are 11 to start with
+	 * and we need to change bit 14 to 0b
+	 */
+	act_offset = new_bank_offset + E1000_ICH_NVM_SIG_WORD;
+
+	/*offset in words but we read dword */
+	--act_offset;
+	ret_val = e1000_read_flash_dword_ich8lan(hw, act_offset, &dword);
+
+	if (ret_val)
+		goto release;
+
+	dword &= 0xBFFFFFFF;
+	ret_val = e1000_retry_write_flash_dword_ich8lan(hw, act_offset, dword);
+
+	if (ret_val)
+		goto release;
+
+	/* And invalidate the previously valid segment by setting
+	 * its signature word (0x13) high_byte to 0b. This can be
+	 * done without an erase because flash erase sets all bits
+	 * to 1's. We can write 1's to 0's without an erase
+	 */
+	act_offset = (old_bank_offset + E1000_ICH_NVM_SIG_WORD) * 2 + 1;
+
+	/* offset in words but we read dword */
+	act_offset = old_bank_offset + E1000_ICH_NVM_SIG_WORD - 1;
+	ret_val = e1000_read_flash_dword_ich8lan(hw, act_offset, &dword);
+
+	if (ret_val)
+		goto release;
+
+	dword &= 0x00FFFFFF;
+	ret_val = e1000_retry_write_flash_dword_ich8lan(hw, act_offset, dword);
+
+	if (ret_val)
+		goto release;
+
+	/* Great!  Everything worked, we can now clear the cached entries. */
+	for (i = 0; i < E1000_ICH8_SHADOW_RAM_WORDS; i++) {
+		dev_spec->shadow_ram[i].modified = false;
+		dev_spec->shadow_ram[i].value = 0xFFFF;
+	}
+
+release:
+	nvm->ops.release(hw);
+
+	/* Reload the EEPROM, or else modifications will not appear
+	 * until after the next adapter reset.
+	 */
+	if (!ret_val) {
+		nvm->ops.reload(hw);
+		usleep_range(10000, 20000);
+	}
+
+out:
+	if (ret_val)
+		e_dbg("NVM update error: %d\n", ret_val);
+
+	return ret_val;
+}
+
+/**
+ *  e1000_update_nvm_checksum_ich8lan - Update the checksum for NVM
+ *  @hw: pointer to the HW structure
+ *
+ *  The NVM checksum is updated by calling the generic update_nvm_checksum,
+ *  which writes the checksum to the shadow ram.  The changes in the shadow
+ *  ram are then committed to the EEPROM by processing each bank at a time
+ *  checking for the modified bit and writing only the pending changes.
+ *  After a successful commit, the shadow ram is cleared and is ready for
+ *  future writes.
+ **/
+static s32 e1000_update_nvm_checksum_ich8lan(struct e1000_hw *hw)
+{
+	struct e1000_nvm_info *nvm = &hw->nvm;
+	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
+	u32 i, act_offset, new_bank_offset, old_bank_offset, bank;
+	s32 ret_val;
+	u16 data = 0;
+
+	ret_val = e1000e_update_nvm_checksum_generic(hw);
+	if (ret_val)
+		goto out;
+
+	if (nvm->type != e1000_nvm_flash_sw)
+		goto out;
+
+	nvm->ops.acquire(hw);
+
+	/* We're writing to the opposite bank so if we're on bank 1,
+	 * write to bank 0 etc.  We also need to erase the segment that
+	 * is going to be written
+	 */
+	ret_val = e1000_valid_nvm_bank_detect_ich8lan(hw, &bank);
+	if (ret_val) {
+		e_dbg("Could not detect valid bank, assuming bank 0\n");
+		bank = 0;
+	}
+
+	if (bank == 0) {
+		new_bank_offset = nvm->flash_bank_size;
+		old_bank_offset = 0;
+		ret_val = e1000_erase_flash_bank_ich8lan(hw, 1);
+		if (ret_val)
+			goto release;
+	} else {
+		old_bank_offset = nvm->flash_bank_size;
+		new_bank_offset = 0;
+		ret_val = e1000_erase_flash_bank_ich8lan(hw, 0);
+		if (ret_val)
+			goto release;
+	}
+	for (i = 0; i < E1000_ICH8_SHADOW_RAM_WORDS; i++) {
 		if (dev_spec->shadow_ram[i].modified) {
 			data = dev_spec->shadow_ram[i].value;
 		} else {
@@ -3498,6 +3954,7 @@ static s32 e1000_validate_nvm_checksum_ich8lan(struct e1000_hw *hw)
 	 */
 	switch (hw->mac.type) {
 	case e1000_pch_lpt:
+	case e1000_pch_spt:
 		word = NVM_COMPAT;
 		valid_csum_mask = NVM_COMPAT_VALID_CSUM;
 		break;
@@ -3583,9 +4040,13 @@ static s32 e1000_write_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 	s32 ret_val;
 	u8 count = 0;
 
-	if (size < 1 || size > 2 || data > size * 0xff ||
-	    offset > ICH_FLASH_LINEAR_ADDR_MASK)
-		return -E1000_ERR_NVM;
+	if (hw->mac.type == e1000_pch_spt) {
+		if (size != 4 || offset > ICH_FLASH_LINEAR_ADDR_MASK)
+			return -E1000_ERR_NVM;
+	} else {
+		if (size < 1 || size > 2 || offset > ICH_FLASH_LINEAR_ADDR_MASK)
+			return -E1000_ERR_NVM;
+	}
 
 	flash_linear_addr = ((ICH_FLASH_LINEAR_ADDR_MASK & offset) +
 			     hw->nvm.flash_base_addr);
@@ -3596,12 +4057,25 @@ static s32 e1000_write_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 		ret_val = e1000_flash_cycle_init_ich8lan(hw);
 		if (ret_val)
 			break;
+		/* In SPT, This register is in Lan memory space, not
+		 * flash.  Therefore, only 32 bit access is supported
+		 */
+		if (hw->mac.type == e1000_pch_spt)
+			hsflctl.regval = er32flash(ICH_FLASH_HSFSTS) >> 16;
+		else
+			hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
 
-		hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
 		/* 0b/1b corresponds to 1 or 2 byte size, respectively. */
 		hsflctl.hsf_ctrl.fldbcount = size - 1;
 		hsflctl.hsf_ctrl.flcycle = ICH_CYCLE_WRITE;
-		ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
+		/* In SPT, This register is in Lan memory space,
+		 * not flash.  Therefore, only 32 bit access is
+		 * supported
+		 */
+		if (hw->mac.type == e1000_pch_spt)
+			ew32flash(ICH_FLASH_HSFSTS, hsflctl.regval << 16);
+		else
+			ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
 
 		ew32flash(ICH_FLASH_FADDR, flash_linear_addr);
 
@@ -3640,6 +4114,90 @@ static s32 e1000_write_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 }
 
 /**
+*  e1000_write_flash_data32_ich8lan - Writes 4 bytes to the NVM
+*  @hw: pointer to the HW structure
+*  @offset: The offset (in bytes) of the dwords to read.
+*  @data: The 4 bytes to write to the NVM.
+*
+*  Writes one/two/four bytes to the NVM using the flash access registers.
+**/
+static s32 e1000_write_flash_data32_ich8lan(struct e1000_hw *hw, u32 offset,
+					    u32 data)
+{
+	union ich8_hws_flash_status hsfsts;
+	union ich8_hws_flash_ctrl hsflctl;
+	u32 flash_linear_addr;
+	s32 ret_val;
+	u8 count = 0;
+
+	if (hw->mac.type == e1000_pch_spt) {
+		if (offset > ICH_FLASH_LINEAR_ADDR_MASK)
+			return -E1000_ERR_NVM;
+	}
+	flash_linear_addr = ((ICH_FLASH_LINEAR_ADDR_MASK & offset) +
+			     hw->nvm.flash_base_addr);
+	do {
+		udelay(1);
+		/* Steps */
+		ret_val = e1000_flash_cycle_init_ich8lan(hw);
+		if (ret_val)
+			break;
+
+		/* In SPT, This register is in Lan memory space, not
+		 * flash.  Therefore, only 32 bit access is supported
+		 */
+		if (hw->mac.type == e1000_pch_spt)
+			hsflctl.regval = er32flash(ICH_FLASH_HSFSTS)
+			    >> 16;
+		else
+			hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
+
+		hsflctl.hsf_ctrl.fldbcount = sizeof(u32) - 1;
+		hsflctl.hsf_ctrl.flcycle = ICH_CYCLE_WRITE;
+
+		/* In SPT, This register is in Lan memory space,
+		 * not flash.  Therefore, only 32 bit access is
+		 * supported
+		 */
+		if (hw->mac.type == e1000_pch_spt)
+			ew32flash(ICH_FLASH_HSFSTS, hsflctl.regval << 16);
+		else
+			ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
+
+		ew32flash(ICH_FLASH_FADDR, flash_linear_addr);
+
+		ew32flash(ICH_FLASH_FDATA0, data);
+
+		/* check if FCERR is set to 1 , if set to 1, clear it
+		 * and try the whole sequence a few more times else done
+		 */
+		ret_val =
+		   e1000_flash_cycle_ich8lan(hw,
+					     ICH_FLASH_WRITE_COMMAND_TIMEOUT);
+
+		if (!ret_val)
+			break;
+
+		/* If we're here, then things are most likely
+		 * completely hosed, but if the error condition
+		 * is detected, it won't hurt to give it another
+		 * try...ICH_FLASH_CYCLE_REPEAT_COUNT times.
+		 */
+		hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
+
+		if (hsfsts.hsf_status.flcerr)
+			/* Repeat for some time before giving up. */
+			continue;
+		if (!hsfsts.hsf_status.flcdone) {
+			e_dbg("Timeout error - flash cycle did not complete.\n");
+			break;
+		}
+	} while (count++ < ICH_FLASH_CYCLE_REPEAT_COUNT);
+
+	return ret_val;
+}
+
+/**
  *  e1000_write_flash_byte_ich8lan - Write a single byte to NVM
  *  @hw: pointer to the HW structure
  *  @offset: The index of the byte to read.
@@ -3656,6 +4214,40 @@ static s32 e1000_write_flash_byte_ich8lan(struct e1000_hw *hw, u32 offset,
 }
 
 /**
+*  e1000_retry_write_flash_dword_ich8lan - Writes a dword to NVM
+*  @hw: pointer to the HW structure
+*  @offset: The offset of the word to write.
+*  @dword: The dword to write to the NVM.
+*
+*  Writes a single dword to the NVM using the flash access registers.
+*  Goes through a retry algorithm before giving up.
+**/
+static s32 e1000_retry_write_flash_dword_ich8lan(struct e1000_hw *hw,
+						 u32 offset, u32 dword)
+{
+	s32 ret_val;
+	u16 program_retries;
+
+	/* Must convert word offset into bytes. */
+	offset <<= 1;
+	ret_val = e1000_write_flash_data32_ich8lan(hw, offset, dword);
+
+	if (!ret_val)
+		return ret_val;
+	for (program_retries = 0; program_retries < 100; program_retries++) {
+		e_dbg("Retrying Byte %8.8X at offset %u\n", dword, offset);
+		usleep_range(100, 200);
+		ret_val = e1000_write_flash_data32_ich8lan(hw, offset, dword);
+		if (!ret_val)
+			break;
+	}
+	if (program_retries == 100)
+		return -E1000_ERR_NVM;
+
+	return 0;
+}
+
+/**
  *  e1000_retry_write_flash_byte_ich8lan - Writes a single byte to NVM
  *  @hw: pointer to the HW structure
  *  @offset: The offset of the byte to write.
@@ -3759,9 +4351,18 @@ static s32 e1000_erase_flash_bank_ich8lan(struct e1000_hw *hw, u32 bank)
 			/* Write a value 11 (block Erase) in Flash
 			 * Cycle field in hw flash control
 			 */
-			hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
+			if (hw->mac.type == e1000_pch_spt)
+				hsflctl.regval =
+				    er32flash(ICH_FLASH_HSFSTS) >> 16;
+			else
+				hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
+
 			hsflctl.hsf_ctrl.flcycle = ICH_CYCLE_ERASE;
-			ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
+			if (hw->mac.type == e1000_pch_spt)
+				ew32flash(ICH_FLASH_HSFSTS,
+					  hsflctl.regval << 16);
+			else
+				ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
 
 			/* Write the last 24 bits of an index within the
 			 * block into Flash Linear address field in Flash
@@ -4180,7 +4781,8 @@ static void e1000_initialize_hw_bits_ich8lan(struct e1000_hw *hw)
 	ew32(RFCTL, reg);
 
 	/* Enable ECC on Lynxpoint */
-	if (hw->mac.type == e1000_pch_lpt) {
+	if ((hw->mac.type == e1000_pch_lpt) ||
+	    (hw->mac.type == e1000_pch_spt)) {
 		reg = er32(PBECCSTS);
 		reg |= E1000_PBECCSTS_ECC_ENABLE;
 		ew32(PBECCSTS, reg);
@@ -4583,7 +5185,8 @@ void e1000_suspend_workarounds_ich8lan(struct e1000_hw *hw)
 		if ((device_id == E1000_DEV_ID_PCH_LPTLP_I218_LM) ||
 		    (device_id == E1000_DEV_ID_PCH_LPTLP_I218_V) ||
 		    (device_id == E1000_DEV_ID_PCH_I218_LM3) ||
-		    (device_id == E1000_DEV_ID_PCH_I218_V3)) {
+		    (device_id == E1000_DEV_ID_PCH_I218_V3) ||
+		    (hw->mac.type == e1000_pch_spt)) {
 			u32 fextnvm6 = er32(FEXTNVM6);
 
 			ew32(FEXTNVM6, fextnvm6 & ~E1000_FEXTNVM6_REQ_PLL_CLK);
@@ -5058,6 +5661,17 @@ static const struct e1000_nvm_operations ich8_nvm_ops = {
 	.write			= e1000_write_nvm_ich8lan,
 };
 
+static const struct e1000_nvm_operations spt_nvm_ops = {
+	.acquire		= e1000_acquire_nvm_ich8lan,
+	.release		= e1000_release_nvm_ich8lan,
+	.read			= e1000_read_nvm_spt,
+	.update			= e1000_update_nvm_checksum_spt,
+	.reload			= e1000e_reload_nvm_generic,
+	.valid_led_default	= e1000_valid_led_default_ich8lan,
+	.validate		= e1000_validate_nvm_checksum_ich8lan,
+	.write			= e1000_write_nvm_ich8lan,
+};
+
 const struct e1000_info e1000_ich8_info = {
 	.mac			= e1000_ich8lan,
 	.flags			= FLAG_HAS_WOL
@@ -5166,3 +5780,23 @@ const struct e1000_info e1000_pch_lpt_info = {
 	.phy_ops		= &ich8_phy_ops,
 	.nvm_ops		= &ich8_nvm_ops,
 };
+
+const struct e1000_info e1000_pch_spt_info = {
+	.mac			= e1000_pch_spt,
+	.flags			= FLAG_IS_ICH
+				  | FLAG_HAS_WOL
+				  | FLAG_HAS_HW_TIMESTAMP
+				  | FLAG_HAS_CTRLEXT_ON_LOAD
+				  | FLAG_HAS_AMT
+				  | FLAG_HAS_FLASH
+				  | FLAG_HAS_JUMBO_FRAMES
+				  | FLAG_APME_IN_WUC,
+	.flags2			= FLAG2_HAS_PHY_STATS
+				  | FLAG2_HAS_EEE,
+	.pba			= 26,
+	.max_hw_frame_size	= 9018,
+	.get_variants		= e1000_get_variants_ich8lan,
+	.mac_ops		= &ich8_mac_ops,
+	.phy_ops		= &ich8_phy_ops,
+	.nvm_ops		= &spt_nvm_ops,
+};
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.h b/drivers/net/ethernet/intel/e1000e/ich8lan.h
index 8066a498eaac..770a573b9eea 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.h
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.h
@@ -95,9 +95,18 @@
 
 #define E1000_FEXTNVM6_REQ_PLL_CLK	0x00000100
 #define E1000_FEXTNVM6_ENABLE_K1_ENTRY_CONDITION	0x00000200
+#define E1000_FEXTNVM6_K1_OFF_ENABLE	0x80000000
+/* bit for disabling packet buffer read */
+#define E1000_FEXTNVM7_DISABLE_PB_READ	0x00040000
 
 #define E1000_FEXTNVM7_DISABLE_SMB_PERST	0x00000020
 
+#define K1_ENTRY_LATENCY	0
+#define K1_MIN_TIME		1
+#define NVM_SIZE_MULTIPLIER 4096	/*multiplier for NVMS field */
+#define E1000_FLASH_BASE_ADDR 0xE000	/*offset of NVM access regs */
+#define E1000_CTRL_EXT_NVMVS 0x3	/*NVM valid sector */
+
 #define PCIE_ICH8_SNOOP_ALL	PCIE_NO_SNOOP_ALL
 
 #define E1000_ICH_RAR_ENTRIES	7
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index 1e8c40fd5c3d..c509a5c900f5 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -70,6 +70,7 @@ static const struct e1000_info *e1000_info_tbl[] = {
 	[board_pchlan]		= &e1000_pch_info,
 	[board_pch2lan]		= &e1000_pch2_info,
 	[board_pch_lpt]		= &e1000_pch_lpt_info,
+	[board_pch_spt]		= &e1000_pch_spt_info,
 };
 
 struct e1000_reg_info {
@@ -946,7 +947,7 @@ static bool e1000_clean_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 		if (*work_done >= work_to_do)
 			break;
 		(*work_done)++;
-		rmb();	/* read descriptor and rx_buffer_info after status DD */
+		dma_rmb();	/* read descriptor and rx_buffer_info after status DD */
 
 		skb = buffer_info->skb;
 		buffer_info->skb = NULL;
@@ -1231,7 +1232,7 @@ static bool e1000_clean_tx_irq(struct e1000_ring *tx_ring)
 	       (count < tx_ring->count)) {
 		bool cleaned = false;
 
-		rmb();		/* read buffer_info after eop_desc */
+		dma_rmb();		/* read buffer_info after eop_desc */
 		for (; !cleaned; count++) {
 			tx_desc = E1000_TX_DESC(*tx_ring, i);
 			buffer_info = &tx_ring->buffer_info[i];
@@ -1331,7 +1332,7 @@ static bool e1000_clean_rx_irq_ps(struct e1000_ring *rx_ring, int *work_done,
 			break;
 		(*work_done)++;
 		skb = buffer_info->skb;
-		rmb();	/* read descriptor and rx_buffer_info after status DD */
+		dma_rmb();	/* read descriptor and rx_buffer_info after status DD */
 
 		/* in the packet split case this is header only */
 		prefetch(skb->data - NET_IP_ALIGN);
@@ -1535,7 +1536,7 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 		if (*work_done >= work_to_do)
 			break;
 		(*work_done)++;
-		rmb();	/* read descriptor and rx_buffer_info after status DD */
+		dma_rmb();	/* read descriptor and rx_buffer_info after status DD */
 
 		skb = buffer_info->skb;
 		buffer_info->skb = NULL;
@@ -1796,7 +1797,8 @@ static irqreturn_t e1000_intr_msi(int __always_unused irq, void *data)
 	}
 
 	/* Reset on uncorrectable ECC error */
-	if ((icr & E1000_ICR_ECCER) && (hw->mac.type == e1000_pch_lpt)) {
+	if ((icr & E1000_ICR_ECCER) && ((hw->mac.type == e1000_pch_lpt) ||
+					(hw->mac.type == e1000_pch_spt))) {
 		u32 pbeccsts = er32(PBECCSTS);
 
 		adapter->corr_errors +=
@@ -1876,7 +1878,8 @@ static irqreturn_t e1000_intr(int __always_unused irq, void *data)
 	}
 
 	/* Reset on uncorrectable ECC error */
-	if ((icr & E1000_ICR_ECCER) && (hw->mac.type == e1000_pch_lpt)) {
+	if ((icr & E1000_ICR_ECCER) && ((hw->mac.type == e1000_pch_lpt) ||
+					(hw->mac.type == e1000_pch_spt))) {
 		u32 pbeccsts = er32(PBECCSTS);
 
 		adapter->corr_errors +=
@@ -2257,7 +2260,8 @@ static void e1000_irq_enable(struct e1000_adapter *adapter)
 	if (adapter->msix_entries) {
 		ew32(EIAC_82574, adapter->eiac_mask & E1000_EIAC_MASK_82574);
 		ew32(IMS, adapter->eiac_mask | E1000_IMS_OTHER | E1000_IMS_LSC);
-	} else if (hw->mac.type == e1000_pch_lpt) {
+	} else if ((hw->mac.type == e1000_pch_lpt) ||
+		   (hw->mac.type == e1000_pch_spt)) {
 		ew32(IMS, IMS_ENABLE_MASK | E1000_IMS_ECCER);
 	} else {
 		ew32(IMS, IMS_ENABLE_MASK);
@@ -3014,6 +3018,19 @@ static void e1000_configure_tx(struct e1000_adapter *adapter)
 	ew32(TCTL, tctl);
 
 	hw->mac.ops.config_collision_dist(hw);
+
+	/* SPT Si errata workaround to avoid data corruption */
+	if (hw->mac.type == e1000_pch_spt) {
+		u32 reg_val;
+
+		reg_val = er32(IOSFPC);
+		reg_val |= E1000_RCTL_RDMTS_HEX;
+		ew32(IOSFPC, reg_val);
+
+		reg_val = er32(TARC(0));
+		reg_val |= E1000_TARC0_CB_MULTIQ_3_REQ;
+		ew32(TARC(0), reg_val);
+	}
 }
 
 /**
@@ -3280,9 +3297,9 @@ static void e1000_configure_rx(struct e1000_adapter *adapter)
 			ew32(RXDCTL(0), rxdctl | 0x3);
 		}
 
-		pm_qos_update_request(&adapter->netdev->pm_qos_req, lat);
+		pm_qos_update_request(&adapter->pm_qos_req, lat);
 	} else {
-		pm_qos_update_request(&adapter->netdev->pm_qos_req,
+		pm_qos_update_request(&adapter->pm_qos_req,
 				      PM_QOS_DEFAULT_VALUE);
 	}
 
@@ -3490,8 +3507,11 @@ s32 e1000e_get_base_timinca(struct e1000_adapter *adapter, u32 *timinca)
 	struct e1000_hw *hw = &adapter->hw;
 	u32 incvalue, incperiod, shift;
 
-	/* Make sure clock is enabled on I217 before checking the frequency */
-	if ((hw->mac.type == e1000_pch_lpt) &&
+	/* Make sure clock is enabled on I217/I218/I219  before checking
+	 * the frequency
+	 */
+	if (((hw->mac.type == e1000_pch_lpt) ||
+	     (hw->mac.type == e1000_pch_spt)) &&
 	    !(er32(TSYNCTXCTL) & E1000_TSYNCTXCTL_ENABLED) &&
 	    !(er32(TSYNCRXCTL) & E1000_TSYNCRXCTL_ENABLED)) {
 		u32 fextnvm7 = er32(FEXTNVM7);
@@ -3505,10 +3525,13 @@ s32 e1000e_get_base_timinca(struct e1000_adapter *adapter, u32 *timinca)
 	switch (hw->mac.type) {
 	case e1000_pch2lan:
 	case e1000_pch_lpt:
-		/* On I217, the clock frequency is 25MHz or 96MHz as
-		 * indicated by the System Clock Frequency Indication
+	case e1000_pch_spt:
+		/* On I217, I218 and I219, the clock frequency is 25MHz
+		 * or 96MHz as indicated by the System Clock Frequency
+		 * Indication
 		 */
-		if ((hw->mac.type != e1000_pch_lpt) ||
+		if (((hw->mac.type != e1000_pch_lpt) &&
+		     (hw->mac.type != e1000_pch_spt)) ||
 		    (er32(TSYNCRXCTL) & E1000_TSYNCRXCTL_SYSCFI)) {
 			/* Stable 96MHz frequency */
 			incperiod = INCPERIOD_96MHz;
@@ -3875,6 +3898,7 @@ void e1000e_reset(struct e1000_adapter *adapter)
 		break;
 	case e1000_pch2lan:
 	case e1000_pch_lpt:
+	case e1000_pch_spt:
 		fc->refresh_time = 0x0400;
 
 		if (adapter->netdev->mtu <= ETH_DATA_LEN) {
@@ -4060,6 +4084,8 @@ void e1000e_down(struct e1000_adapter *adapter, bool reset)
 	 */
 	set_bit(__E1000_DOWN, &adapter->state);
 
+	netif_carrier_off(netdev);
+
 	/* disable receives in the hardware */
 	rctl = er32(RCTL);
 	if (!(adapter->flags2 & FLAG2_NO_DISABLE_RX))
@@ -4084,8 +4110,6 @@ void e1000e_down(struct e1000_adapter *adapter, bool reset)
 	del_timer_sync(&adapter->watchdog_timer);
 	del_timer_sync(&adapter->phy_info_timer);
 
-	netif_carrier_off(netdev);
-
 	spin_lock(&adapter->stats64_lock);
 	e1000e_update_stats(adapter);
 	spin_unlock(&adapter->stats64_lock);
@@ -4379,7 +4403,7 @@ static int e1000_open(struct net_device *netdev)
 		e1000_update_mng_vlan(adapter);
 
 	/* DMA latency requirement to workaround jumbo issue */
-	pm_qos_add_request(&adapter->netdev->pm_qos_req, PM_QOS_CPU_DMA_LATENCY,
+	pm_qos_add_request(&adapter->pm_qos_req, PM_QOS_CPU_DMA_LATENCY,
 			   PM_QOS_DEFAULT_VALUE);
 
 	/* before we allocate an interrupt, we must be ready to handle it.
@@ -4490,7 +4514,7 @@ static int e1000_close(struct net_device *netdev)
 	    !test_bit(__E1000_TESTING, &adapter->state))
 		e1000e_release_hw_control(adapter);
 
-	pm_qos_remove_request(&adapter->netdev->pm_qos_req);
+	pm_qos_remove_request(&adapter->pm_qos_req);
 
 	pm_runtime_put_sync(&pdev->dev);
 
@@ -4759,7 +4783,8 @@ static void e1000e_update_stats(struct e1000_adapter *adapter)
 	adapter->stats.mgpdc += er32(MGTPDC);
 
 	/* Correctable ECC Errors */
-	if (hw->mac.type == e1000_pch_lpt) {
+	if ((hw->mac.type == e1000_pch_lpt) ||
+	    (hw->mac.type == e1000_pch_spt)) {
 		u32 pbeccsts = er32(PBECCSTS);
 
 		adapter->corr_errors +=
@@ -6144,7 +6169,8 @@ static int __e1000_shutdown(struct pci_dev *pdev, bool runtime)
 
 	if (adapter->hw.phy.type == e1000_phy_igp_3) {
 		e1000e_igp3_phy_powerdown_workaround_ich8lan(&adapter->hw);
-	} else if (hw->mac.type == e1000_pch_lpt) {
+	} else if ((hw->mac.type == e1000_pch_lpt) ||
+		   (hw->mac.type == e1000_pch_spt)) {
 		if (!(wufc & (E1000_WUFC_EX | E1000_WUFC_MC | E1000_WUFC_BC)))
 			/* ULP does not support wake from unicast, multicast
 			 * or broadcast.
@@ -6807,7 +6833,8 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_ioremap;
 
 	if ((adapter->flags & FLAG_HAS_FLASH) &&
-	    (pci_resource_flags(pdev, 1) & IORESOURCE_MEM)) {
+	    (pci_resource_flags(pdev, 1) & IORESOURCE_MEM) &&
+	    (hw->mac.type < e1000_pch_spt)) {
 		flash_start = pci_resource_start(pdev, 1);
 		flash_len = pci_resource_len(pdev, 1);
 		adapter->hw.flash_address = ioremap(flash_start, flash_len);
@@ -6847,7 +6874,8 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_hw_init;
 
 	if ((adapter->flags & FLAG_IS_ICH) &&
-	    (adapter->flags & FLAG_READ_ONLY_NVM))
+	    (adapter->flags & FLAG_READ_ONLY_NVM) &&
+	    (hw->mac.type < e1000_pch_spt))
 		e1000e_write_protect_nvm_ich8lan(&adapter->hw);
 
 	hw->mac.ops.get_bus_info(&adapter->hw);
@@ -7043,7 +7071,7 @@ err_hw_init:
 	kfree(adapter->tx_ring);
 	kfree(adapter->rx_ring);
 err_sw_init:
-	if (adapter->hw.flash_address)
+	if ((adapter->hw.flash_address) && (hw->mac.type < e1000_pch_spt))
 		iounmap(adapter->hw.flash_address);
 	e1000e_reset_interrupt_capability(adapter);
 err_flashmap:
@@ -7116,7 +7144,8 @@ static void e1000_remove(struct pci_dev *pdev)
 	kfree(adapter->rx_ring);
 
 	iounmap(adapter->hw.hw_addr);
-	if (adapter->hw.flash_address)
+	if ((adapter->hw.flash_address) &&
+	    (adapter->hw.mac.type < e1000_pch_spt))
 		iounmap(adapter->hw.flash_address);
 	pci_release_selected_regions(pdev,
 				     pci_select_bars(pdev, IORESOURCE_MEM));
@@ -7213,6 +7242,10 @@ static const struct pci_device_id e1000_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_I218_V2), board_pch_lpt },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_I218_LM3), board_pch_lpt },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_I218_V3), board_pch_lpt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_SPT_I219_LM), board_pch_spt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_SPT_I219_V), board_pch_spt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_SPT_I219_LM2), board_pch_spt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_SPT_I219_V2), board_pch_spt },
 
 	{ 0, 0, 0, 0, 0, 0, 0 }	/* terminate list */
 };
diff --git a/drivers/net/ethernet/intel/e1000e/ptp.c b/drivers/net/ethernet/intel/e1000e/ptp.c
index 978ef9c4a043..8d7b21dc7e19 100644
--- a/drivers/net/ethernet/intel/e1000e/ptp.c
+++ b/drivers/net/ethernet/intel/e1000e/ptp.c
@@ -106,20 +106,18 @@ static int e1000e_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)
  * Read the timecounter and return the correct value in ns after converting
  * it into a struct timespec.
  **/
-static int e1000e_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int e1000e_phc_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct e1000_adapter *adapter = container_of(ptp, struct e1000_adapter,
 						     ptp_clock_info);
 	unsigned long flags;
-	u32 remainder;
 	u64 ns;
 
 	spin_lock_irqsave(&adapter->systim_lock, flags);
 	ns = timecounter_read(&adapter->tc);
 	spin_unlock_irqrestore(&adapter->systim_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, NSEC_PER_SEC, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
@@ -133,14 +131,14 @@ static int e1000e_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
  * wall timer value.
  **/
 static int e1000e_phc_settime(struct ptp_clock_info *ptp,
-			      const struct timespec *ts)
+			      const struct timespec64 *ts)
 {
 	struct e1000_adapter *adapter = container_of(ptp, struct e1000_adapter,
 						     ptp_clock_info);
 	unsigned long flags;
 	u64 ns;
 
-	ns = timespec_to_ns(ts);
+	ns = timespec64_to_ns(ts);
 
 	/* reset the timecounter */
 	spin_lock_irqsave(&adapter->systim_lock, flags);
@@ -171,11 +169,12 @@ static void e1000e_systim_overflow_work(struct work_struct *work)
 	struct e1000_adapter *adapter = container_of(work, struct e1000_adapter,
 						     systim_overflow_work.work);
 	struct e1000_hw *hw = &adapter->hw;
-	struct timespec ts;
+	struct timespec64 ts;
 
-	adapter->ptp_clock_info.gettime(&adapter->ptp_clock_info, &ts);
+	adapter->ptp_clock_info.gettime64(&adapter->ptp_clock_info, &ts);
 
-	e_dbg("SYSTIM overflow check at %ld.%09lu\n", ts.tv_sec, ts.tv_nsec);
+	e_dbg("SYSTIM overflow check at %lld.%09lu\n",
+	      (long long) ts.tv_sec, ts.tv_nsec);
 
 	schedule_delayed_work(&adapter->systim_overflow_work,
 			      E1000_SYSTIM_OVERFLOW_PERIOD);
@@ -190,8 +189,8 @@ static const struct ptp_clock_info e1000e_ptp_clock_info = {
 	.pps		= 0,
 	.adjfreq	= e1000e_phc_adjfreq,
 	.adjtime	= e1000e_phc_adjtime,
-	.gettime	= e1000e_phc_gettime,
-	.settime	= e1000e_phc_settime,
+	.gettime64	= e1000e_phc_gettime,
+	.settime64	= e1000e_phc_settime,
 	.enable		= e1000e_phc_enable,
 };
 
@@ -221,7 +220,9 @@ void e1000e_ptp_init(struct e1000_adapter *adapter)
 	switch (hw->mac.type) {
 	case e1000_pch2lan:
 	case e1000_pch_lpt:
-		if ((hw->mac.type != e1000_pch_lpt) ||
+	case e1000_pch_spt:
+		if (((hw->mac.type != e1000_pch_lpt) &&
+		     (hw->mac.type != e1000_pch_spt)) ||
 		    (er32(TSYNCRXCTL) & E1000_TSYNCRXCTL_SYSCFI)) {
 			adapter->ptp_clock_info.max_adj = 24000000 - 1;
 			break;
diff --git a/drivers/net/ethernet/intel/e1000e/regs.h b/drivers/net/ethernet/intel/e1000e/regs.h
index ea235bbe50d3..85eefc4832ba 100644
--- a/drivers/net/ethernet/intel/e1000e/regs.h
+++ b/drivers/net/ethernet/intel/e1000e/regs.h
@@ -38,6 +38,7 @@
 #define E1000_FEXTNVM4	0x00024	/* Future Extended NVM 4 - RW */
 #define E1000_FEXTNVM6	0x00010	/* Future Extended NVM 6 - RW */
 #define E1000_FEXTNVM7	0x000E4	/* Future Extended NVM 7 - RW */
+#define E1000_PCIEANACFG	0x00F18	/* PCIE Analog Config */
 #define E1000_FCT	0x00030	/* Flow Control Type - RW */
 #define E1000_VET	0x00038	/* VLAN Ether Type - RW */
 #define E1000_ICR	0x000C0	/* Interrupt Cause Read - R/clr */
@@ -67,6 +68,7 @@
 #define E1000_PBA	0x01000	/* Packet Buffer Allocation - RW */
 #define E1000_PBS	0x01008	/* Packet Buffer Size */
 #define E1000_PBECCSTS	0x0100C	/* Packet Buffer ECC Status - RW */
+#define E1000_IOSFPC	0x00F28	/* TX corrupted data  */
 #define E1000_EEMNGCTL	0x01010	/* MNG EEprom Control */
 #define E1000_EEWR	0x0102C	/* EEPROM Write Register - RW */
 #define E1000_FLOP	0x0103C	/* FLASH Opcode Register */
@@ -121,6 +123,7 @@
 				 (0x054E4 + ((_i - 16) * 8)))
 #define E1000_SHRAL(_i)		(0x05438 + ((_i) * 8))
 #define E1000_SHRAH(_i)		(0x0543C + ((_i) * 8))
+#define E1000_TARC0_CB_MULTIQ_3_REQ	(1 << 28 | 1 << 29)
 #define E1000_TDFH		0x03410	/* Tx Data FIFO Head - RW */
 #define E1000_TDFT		0x03418	/* Tx Data FIFO Tail - RW */
 #define E1000_TDFHS		0x03420	/* Tx Data FIFO Head Saved - RW */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k.h b/drivers/net/ethernet/intel/fm10k/fm10k.h
index 42eb4344a9dc..c8c8c5baefda 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k.h
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -235,6 +235,9 @@ struct fm10k_vxlan_port {
 	__be16			port;
 };
 
+/* one work queue for entire driver */
+extern struct workqueue_struct *fm10k_workqueue;
+
 struct fm10k_intfc {
 	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
 	struct net_device *netdev;
@@ -266,7 +269,6 @@ struct fm10k_intfc {
 	u64 tx_csum_errors;
 	u64 alloc_failed;
 	u64 rx_csum_errors;
-	u64 rx_errors;
 
 	u64 tx_bytes_nic;
 	u64 tx_packets_nic;
@@ -439,6 +441,7 @@ extern char fm10k_driver_name[];
 extern const char fm10k_driver_version[];
 int fm10k_init_queueing_scheme(struct fm10k_intfc *interface);
 void fm10k_clear_queueing_scheme(struct fm10k_intfc *interface);
+__be16 fm10k_tx_encap_offload(struct sk_buff *skb);
 netdev_tx_t fm10k_xmit_frame_ring(struct sk_buff *skb,
 				  struct fm10k_ring *tx_ring);
 void fm10k_tx_timeout_reset(struct fm10k_intfc *interface);
@@ -457,6 +460,9 @@ void fm10k_down(struct fm10k_intfc *interface);
 void fm10k_update_stats(struct fm10k_intfc *interface);
 void fm10k_service_event_schedule(struct fm10k_intfc *interface);
 void fm10k_update_rx_drop_en(struct fm10k_intfc *interface);
+#ifdef CONFIG_NET_POLL_CONTROLLER
+void fm10k_netpoll(struct net_device *netdev);
+#endif
 
 /* Netdev */
 struct net_device *fm10k_alloc_netdev(void);
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_common.c b/drivers/net/ethernet/intel/fm10k/fm10k_common.c
index bf19dccd4288..6cfae6ac04ea 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_common.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_common.c
@@ -398,7 +398,7 @@ static void fm10k_update_hw_stats_rx_q(struct fm10k_hw *hw,
 	/* Retrieve RX Owner Data */
 	id_rx = fm10k_read_reg(hw, FM10K_RXQCTL(idx));
 
-	/* Process RX Ring*/
+	/* Process RX Ring */
 	do {
 		rx_drops = fm10k_read_hw_stats_32b(hw, FM10K_QPRDC(idx),
 						   &q->rx_drops);
@@ -466,7 +466,6 @@ void fm10k_update_hw_stats_q(struct fm10k_hw *hw, struct fm10k_hw_stats_q *q,
  *  Function invalidates the index values for the queues so any updates that
  *  may have happened are ignored and the base for the queue stats is reset.
  **/
-
 void fm10k_unbind_hw_stats_q(struct fm10k_hw_stats_q *q, u32 idx, u32 count)
 {
 	u32 i;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c b/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
index 212a92dad222..5c7a4d7662d8 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -128,7 +128,7 @@ static int fm10k_dcbnl_ieee_setpfc(struct net_device *dev, struct ieee_pfc *pfc)
  *
  * Returns that we support only IEEE DCB for this interface
  **/
-static u8 fm10k_dcbnl_getdcbx(struct net_device *dev)
+static u8 fm10k_dcbnl_getdcbx(struct net_device __always_unused *dev)
 {
 	return DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE;
 }
@@ -140,7 +140,7 @@ static u8 fm10k_dcbnl_getdcbx(struct net_device *dev)
  *
  * Returns error on attempt to enable anything but IEEE DCB for this interface
  **/
-static u8 fm10k_dcbnl_setdcbx(struct net_device *dev, u8 mode)
+static u8 fm10k_dcbnl_setdcbx(struct net_device __always_unused *dev, u8 mode)
 {
 	return (mode != (DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE)) ? 1 : 0;
 }
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c b/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
index 4327f86218b9..f45b4d71adb8 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -36,14 +36,16 @@ static void *fm10k_dbg_desc_seq_start(struct seq_file *s, loff_t *pos)
 	return (*pos < ring->count) ? pos : NULL;
 }
 
-static void *fm10k_dbg_desc_seq_next(struct seq_file *s, void *v, loff_t *pos)
+static void *fm10k_dbg_desc_seq_next(struct seq_file *s,
+				     void __always_unused *v, loff_t *pos)
 {
 	struct fm10k_ring *ring = s->private;
 
 	return (++(*pos) < ring->count) ? pos : NULL;
 }
 
-static void fm10k_dbg_desc_seq_stop(struct seq_file *s, void *v)
+static void fm10k_dbg_desc_seq_stop(struct seq_file __always_unused *s,
+				    __always_unused void *v)
 {
 	/* Do nothing. */
 }
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
index 651f53bc7376..4b9d9f88af70 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -57,13 +57,12 @@ static const struct fm10k_stats fm10k_gstrings_net_stats[] = {
 	.stat_offset = offsetof(struct fm10k_intfc, _stat) \
 }
 
-static const struct fm10k_stats fm10k_gstrings_stats[] = {
+static const struct fm10k_stats fm10k_gstrings_global_stats[] = {
 	FM10K_STAT("tx_restart_queue", restart_queue),
 	FM10K_STAT("tx_busy", tx_busy),
 	FM10K_STAT("tx_csum_errors", tx_csum_errors),
 	FM10K_STAT("rx_alloc_failed", alloc_failed),
 	FM10K_STAT("rx_csum_errors", rx_csum_errors),
-	FM10K_STAT("rx_errors", rx_errors),
 
 	FM10K_STAT("tx_packets_nic", tx_packets_nic),
 	FM10K_STAT("tx_bytes_nic", tx_bytes_nic),
@@ -73,38 +72,42 @@ static const struct fm10k_stats fm10k_gstrings_stats[] = {
 	FM10K_STAT("rx_overrun_pf", rx_overrun_pf),
 	FM10K_STAT("rx_overrun_vf", rx_overrun_vf),
 
-	FM10K_STAT("timeout", stats.timeout.count),
-	FM10K_STAT("ur", stats.ur.count),
-	FM10K_STAT("ca", stats.ca.count),
-	FM10K_STAT("um", stats.um.count),
-	FM10K_STAT("xec", stats.xec.count),
-	FM10K_STAT("vlan_drop", stats.vlan_drop.count),
-	FM10K_STAT("loopback_drop", stats.loopback_drop.count),
-	FM10K_STAT("nodesc_drop", stats.nodesc_drop.count),
-
 	FM10K_STAT("swapi_status", hw.swapi.status),
 	FM10K_STAT("mac_rules_used", hw.swapi.mac.used),
 	FM10K_STAT("mac_rules_avail", hw.swapi.mac.avail),
 
 	FM10K_STAT("mbx_tx_busy", hw.mbx.tx_busy),
-	FM10K_STAT("mbx_tx_dropped", hw.mbx.tx_dropped),
+	FM10K_STAT("mbx_tx_oversized", hw.mbx.tx_dropped),
 	FM10K_STAT("mbx_tx_messages", hw.mbx.tx_messages),
 	FM10K_STAT("mbx_tx_dwords", hw.mbx.tx_dwords),
 	FM10K_STAT("mbx_rx_messages", hw.mbx.rx_messages),
 	FM10K_STAT("mbx_rx_dwords", hw.mbx.rx_dwords),
 	FM10K_STAT("mbx_rx_parse_err", hw.mbx.rx_parse_err),
 
+	FM10K_STAT("tx_hang_count", tx_timeout_count),
+
 	FM10K_STAT("tx_hwtstamp_timeouts", tx_hwtstamp_timeouts),
 };
 
-#define FM10K_GLOBAL_STATS_LEN ARRAY_SIZE(fm10k_gstrings_stats)
+static const struct fm10k_stats fm10k_gstrings_pf_stats[] = {
+	FM10K_STAT("timeout", stats.timeout.count),
+	FM10K_STAT("ur", stats.ur.count),
+	FM10K_STAT("ca", stats.ca.count),
+	FM10K_STAT("um", stats.um.count),
+	FM10K_STAT("xec", stats.xec.count),
+	FM10K_STAT("vlan_drop", stats.vlan_drop.count),
+	FM10K_STAT("loopback_drop", stats.loopback_drop.count),
+	FM10K_STAT("nodesc_drop", stats.nodesc_drop.count),
+};
+
+#define FM10K_GLOBAL_STATS_LEN ARRAY_SIZE(fm10k_gstrings_global_stats)
+#define FM10K_PF_STATS_LEN ARRAY_SIZE(fm10k_gstrings_pf_stats)
 
-#define FM10K_QUEUE_STATS_LEN \
-	  (MAX_QUEUES * 2 * (sizeof(struct fm10k_queue_stats) / sizeof(u64)))
+#define FM10K_QUEUE_STATS_LEN(_n) \
+	( (_n) * 2 * (sizeof(struct fm10k_queue_stats) / sizeof(u64)))
 
-#define FM10K_STATS_LEN (FM10K_GLOBAL_STATS_LEN + \
-			 FM10K_NETDEV_STATS_LEN + \
-			 FM10K_QUEUE_STATS_LEN)
+#define FM10K_STATIC_STATS_LEN (FM10K_GLOBAL_STATS_LEN + \
+				FM10K_NETDEV_STATS_LEN)
 
 static const char fm10k_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Mailbox test (on/offline)"
@@ -117,9 +120,9 @@ enum fm10k_self_test_types {
 	FM10K_TEST_MAX = FM10K_TEST_LEN
 };
 
-static void fm10k_get_strings(struct net_device *dev, u32 stringset,
-			      u8 *data)
+static void fm10k_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 {
+	struct fm10k_intfc *interface = netdev_priv(dev);
 	char *p = (char *)data;
 	int i;
 
@@ -135,12 +138,19 @@ static void fm10k_get_strings(struct net_device *dev, u32 stringset,
 			p += ETH_GSTRING_LEN;
 		}
 		for (i = 0; i < FM10K_GLOBAL_STATS_LEN; i++) {
-			memcpy(p, fm10k_gstrings_stats[i].stat_string,
+			memcpy(p, fm10k_gstrings_global_stats[i].stat_string,
 			       ETH_GSTRING_LEN);
 			p += ETH_GSTRING_LEN;
 		}
 
-		for (i = 0; i < MAX_QUEUES; i++) {
+		if (interface->hw.mac.type != fm10k_mac_vf)
+			for (i = 0; i < FM10K_PF_STATS_LEN; i++) {
+				memcpy(p, fm10k_gstrings_pf_stats[i].stat_string,
+				       ETH_GSTRING_LEN);
+				p += ETH_GSTRING_LEN;
+			}
+
+		for (i = 0; i < interface->hw.mac.max_queues; i++) {
 			sprintf(p, "tx_queue_%u_packets", i);
 			p += ETH_GSTRING_LEN;
 			sprintf(p, "tx_queue_%u_bytes", i);
@@ -156,18 +166,28 @@ static void fm10k_get_strings(struct net_device *dev, u32 stringset,
 
 static int fm10k_get_sset_count(struct net_device *dev, int sset)
 {
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+	int stats_len = FM10K_STATIC_STATS_LEN;
+
 	switch (sset) {
 	case ETH_SS_TEST:
 		return FM10K_TEST_LEN;
 	case ETH_SS_STATS:
-		return FM10K_STATS_LEN;
+		stats_len += FM10K_QUEUE_STATS_LEN(hw->mac.max_queues);
+
+		if (hw->mac.type != fm10k_mac_vf)
+			stats_len += FM10K_PF_STATS_LEN;
+
+		return stats_len;
 	default:
 		return -EOPNOTSUPP;
 	}
 }
 
 static void fm10k_get_ethtool_stats(struct net_device *netdev,
-				    struct ethtool_stats *stats, u64 *data)
+				    struct ethtool_stats __always_unused *stats,
+				    u64 *data)
 {
 	const int stat_count = sizeof(struct fm10k_queue_stats) / sizeof(u64);
 	struct fm10k_intfc *interface = netdev_priv(netdev);
@@ -184,12 +204,21 @@ static void fm10k_get_ethtool_stats(struct net_device *netdev,
 	}
 
 	for (i = 0; i < FM10K_GLOBAL_STATS_LEN; i++) {
-		p = (char *)interface + fm10k_gstrings_stats[i].stat_offset;
-		*(data++) = (fm10k_gstrings_stats[i].sizeof_stat ==
+		p = (char *)interface +
+		    fm10k_gstrings_global_stats[i].stat_offset;
+		*(data++) = (fm10k_gstrings_global_stats[i].sizeof_stat ==
 			sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
 	}
 
-	for (i = 0; i < MAX_QUEUES; i++) {
+	if (interface->hw.mac.type != fm10k_mac_vf)
+		for (i = 0; i < FM10K_PF_STATS_LEN; i++) {
+			p = (char *)interface +
+			    fm10k_gstrings_pf_stats[i].stat_offset;
+			*(data++) = (fm10k_gstrings_pf_stats[i].sizeof_stat ==
+				     sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
+		}
+
+	for (i = 0; i < interface->hw.mac.max_queues; i++) {
 		struct fm10k_ring *ring;
 		u64 *queue_stat;
 
@@ -369,7 +398,7 @@ static void fm10k_get_drvinfo(struct net_device *dev,
 	strncpy(info->bus_info, pci_name(interface->pdev),
 		sizeof(info->bus_info) - 1);
 
-	info->n_stats = FM10K_STATS_LEN;
+	info->n_stats = fm10k_get_sset_count(dev, ETH_SS_STATS);
 
 	info->regdump_len = fm10k_get_regs_len(dev);
 }
@@ -645,7 +674,7 @@ static int fm10k_get_rss_hash_opts(struct fm10k_intfc *interface,
 }
 
 static int fm10k_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
-			   u32 *rule_locs)
+			   u32 __always_unused *rule_locs)
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
 	int ret = -EOPNOTSUPP;
@@ -851,7 +880,7 @@ static void fm10k_self_test(struct net_device *dev,
 		eth_test->flags |= ETH_TEST_FL_FAILED;
 }
 
-static u32 fm10k_get_reta_size(struct net_device *netdev)
+static u32 fm10k_get_reta_size(struct net_device __always_unused *netdev)
 {
 	return FM10K_RETA_SIZE * FM10K_RETA_ENTRIES_PER_REG;
 }
@@ -911,7 +940,7 @@ static int fm10k_set_reta(struct net_device *netdev, const u32 *indir)
 	return 0;
 }
 
-static u32 fm10k_get_rssrk_size(struct net_device *netdev)
+static u32 fm10k_get_rssrk_size(struct net_device __always_unused *netdev)
 {
 	return FM10K_RSSRK_SIZE * FM10K_RSSRK_ENTRIES_PER_REG;
 }
@@ -1019,7 +1048,7 @@ static int fm10k_set_channels(struct net_device *dev,
 }
 
 static int fm10k_get_ts_info(struct net_device *dev,
-			   struct ethtool_ts_info *info)
+			     struct ethtool_ts_info *info)
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
 
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_iov.c b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
index 060190864238..5b08e6284a3c 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -47,7 +47,7 @@ s32 fm10k_iov_event(struct fm10k_intfc *interface)
 {
 	struct fm10k_hw *hw = &interface->hw;
 	struct fm10k_iov_data *iov_data;
-	s64 mbicr, vflre;
+	s64 vflre;
 	int i;
 
 	/* if there is no iov_data then there is no mailboxes to process */
@@ -63,7 +63,7 @@ s32 fm10k_iov_event(struct fm10k_intfc *interface)
 		goto read_unlock;
 
 	if (!(fm10k_read_reg(hw, FM10K_EICR) & FM10K_EICR_VFLR))
-		goto process_mbx;
+		goto read_unlock;
 
 	/* read VFLRE to determine if any VFs have been reset */
 	do {
@@ -86,32 +86,6 @@ s32 fm10k_iov_event(struct fm10k_intfc *interface)
 		}
 	} while (i != iov_data->num_vfs);
 
-process_mbx:
-	/* read MBICR to determine which VFs require attention */
-	mbicr = fm10k_read_reg(hw, FM10K_MBICR(1));
-	mbicr <<= 32;
-	mbicr |= fm10k_read_reg(hw, FM10K_MBICR(0));
-
-	i = iov_data->next_vf_mbx ? : iov_data->num_vfs;
-
-	for (mbicr <<= 64 - i; i--; mbicr += mbicr) {
-		struct fm10k_mbx_info *mbx = &iov_data->vf_info[i].mbx;
-
-		if (mbicr >= 0)
-			continue;
-
-		if (!hw->mbx.ops.tx_ready(&hw->mbx, FM10K_VFMBX_MSG_MTU))
-			break;
-
-		mbx->ops.process(hw, mbx);
-	}
-
-	if (i >= 0) {
-		iov_data->next_vf_mbx = i + 1;
-	} else if (iov_data->next_vf_mbx) {
-		iov_data->next_vf_mbx = 0;
-		goto process_mbx;
-	}
 read_unlock:
 	rcu_read_unlock();
 
@@ -139,6 +113,13 @@ s32 fm10k_iov_mbx(struct fm10k_intfc *interface)
 	/* lock the mailbox for transmit and receive */
 	fm10k_mbx_lock(interface);
 
+	/* Most VF messages sent to the PF cause the PF to respond by
+	 * requesting from the SM mailbox. This means that too many VF
+	 * messages processed at once could cause a mailbox timeout on the PF.
+	 * To prevent this, store a pointer to the next VF mbx to process. Use
+	 * that as the start of the loop so that we don't starve whichever VF
+	 * got ignored on the previous run.
+	 */
 process_mbx:
 	for (i = iov_data->next_vf_mbx ? : iov_data->num_vfs; i--;) {
 		struct fm10k_vf_info *vf_info = &iov_data->vf_info[i];
@@ -155,10 +136,6 @@ process_mbx:
 			mbx->ops.connect(hw, mbx);
 		}
 
-		/* no work pending, then just continue */
-		if (mbx->ops.tx_complete(mbx) && !mbx->ops.rx_ready(mbx))
-			continue;
-
 		/* guarantee we have free space in the SM mailbox */
 		if (!hw->mbx.ops.tx_ready(&hw->mbx, FM10K_VFMBX_MSG_MTU))
 			break;
@@ -167,6 +144,10 @@ process_mbx:
 		mbx->ops.process(hw, mbx);
 	}
 
+	/* if we stopped processing mailboxes early, update next_vf_mbx.
+	 * Otherwise, reset next_vf_mbx, and restart loop so that we process
+	 * the remaining mailboxes we skipped at the start.
+	 */
 	if (i >= 0) {
 		iov_data->next_vf_mbx = i + 1;
 	} else if (iov_data->next_vf_mbx) {
@@ -275,7 +256,7 @@ s32 fm10k_iov_update_pvid(struct fm10k_intfc *interface, u16 glort, u16 pvid)
 	if (vf_idx >= iov_data->num_vfs)
 		return FM10K_ERR_PARAM;
 
-	/* determine if an update has occured and if so notify the VF */
+	/* determine if an update has occurred and if so notify the VF */
 	vf_info = &iov_data->vf_info[vf_idx];
 	if (vf_info->sw_vid != pvid) {
 		vf_info->sw_vid = pvid;
@@ -488,8 +469,8 @@ int fm10k_ndo_set_vf_vlan(struct net_device *netdev, int vf_idx, u16 vid,
 	return 0;
 }
 
-int fm10k_ndo_set_vf_bw(struct net_device *netdev, int vf_idx, int unused,
-			int rate)
+int fm10k_ndo_set_vf_bw(struct net_device *netdev, int vf_idx,
+			int __always_unused unused, int rate)
 {
 	struct fm10k_intfc *interface = netdev_priv(netdev);
 	struct fm10k_iov_data *iov_data = interface->iov_data;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_main.c b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
index 84ab9eea2768..1b0661e3573b 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_main.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
@@ -28,7 +28,7 @@
 
 #include "fm10k.h"
 
-#define DRV_VERSION	"0.12.2-k"
+#define DRV_VERSION	"0.15.2-k"
 const char fm10k_driver_version[] = DRV_VERSION;
 char fm10k_driver_name[] = "fm10k";
 static const char fm10k_driver_string[] =
@@ -41,6 +41,9 @@ MODULE_DESCRIPTION("Intel(R) Ethernet Switch Host Interface Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 
+/* single workqueue for entire fm10k driver */
+struct workqueue_struct *fm10k_workqueue = NULL;
+
 /**
  * fm10k_init_module - Driver Registration Routine
  *
@@ -52,6 +55,10 @@ static int __init fm10k_init_module(void)
 	pr_info("%s - version %s\n", fm10k_driver_string, fm10k_driver_version);
 	pr_info("%s\n", fm10k_copyright);
 
+	/* create driver workqueue */
+	if (!fm10k_workqueue)
+		fm10k_workqueue = create_workqueue("fm10k");
+
 	fm10k_dbg_init();
 
 	return fm10k_register_pci_driver();
@@ -69,6 +76,11 @@ static void __exit fm10k_exit_module(void)
 	fm10k_unregister_pci_driver();
 
 	fm10k_dbg_exit();
+
+	/* destroy driver workqueue */
+	flush_workqueue(fm10k_workqueue);
+	destroy_workqueue(fm10k_workqueue);
+	fm10k_workqueue = NULL;
 }
 module_exit(fm10k_exit_module);
 
@@ -209,7 +221,7 @@ static inline bool fm10k_page_is_reserved(struct page *page)
 
 static bool fm10k_can_reuse_rx_page(struct fm10k_rx_buffer *rx_buffer,
 				    struct page *page,
-				    unsigned int truesize)
+				    unsigned int __maybe_unused truesize)
 {
 	/* avoid re-using remote pages */
 	if (unlikely(fm10k_page_is_reserved(page)))
@@ -240,7 +252,6 @@ static bool fm10k_can_reuse_rx_page(struct fm10k_rx_buffer *rx_buffer,
 
 /**
  * fm10k_add_rx_frag - Add contents of Rx buffer to sk_buff
- * @rx_ring: rx descriptor ring to transact packets on
  * @rx_buffer: buffer containing page to add
  * @rx_desc: descriptor containing length of buffer written by hardware
  * @skb: sk_buff to place the data into
@@ -253,8 +264,7 @@ static bool fm10k_can_reuse_rx_page(struct fm10k_rx_buffer *rx_buffer,
  * The function will then update the page offset if necessary and return
  * true if the buffer can be reused by the interface.
  **/
-static bool fm10k_add_rx_frag(struct fm10k_ring *rx_ring,
-			      struct fm10k_rx_buffer *rx_buffer,
+static bool fm10k_add_rx_frag(struct fm10k_rx_buffer *rx_buffer,
 			      union fm10k_rx_desc *rx_desc,
 			      struct sk_buff *skb)
 {
@@ -330,7 +340,7 @@ static struct sk_buff *fm10k_fetch_rx_buffer(struct fm10k_ring *rx_ring,
 				      DMA_FROM_DEVICE);
 
 	/* pull page into skb */
-	if (fm10k_add_rx_frag(rx_ring, rx_buffer, rx_desc, skb)) {
+	if (fm10k_add_rx_frag(rx_buffer, rx_desc, skb)) {
 		/* hand second half of page back to the ring */
 		fm10k_reuse_rx_page(rx_ring, rx_buffer);
 	} else {
@@ -412,7 +422,7 @@ static void fm10k_rx_hwtstamp(struct fm10k_ring *rx_ring,
 }
 
 static void fm10k_type_trans(struct fm10k_ring *rx_ring,
-			     union fm10k_rx_desc *rx_desc,
+			     union fm10k_rx_desc __maybe_unused *rx_desc,
 			     struct sk_buff *skb)
 {
 	struct net_device *dev = rx_ring->netdev;
@@ -509,8 +519,6 @@ static bool fm10k_is_non_eop(struct fm10k_ring *rx_ring,
 
 /**
  * fm10k_pull_tail - fm10k specific version of skb_pull_tail
- * @rx_ring: rx descriptor ring packet is being transacted on
- * @rx_desc: pointer to the EOP Rx descriptor
  * @skb: pointer to current skb being adjusted
  *
  * This function is an fm10k specific version of __pskb_pull_tail.  The
@@ -520,9 +528,7 @@ static bool fm10k_is_non_eop(struct fm10k_ring *rx_ring,
  * As a result we can do things like drop a frag and maintain an accurate
  * truesize for the skb.
  */
-static void fm10k_pull_tail(struct fm10k_ring *rx_ring,
-			    union fm10k_rx_desc *rx_desc,
-			    struct sk_buff *skb)
+static void fm10k_pull_tail(struct sk_buff *skb)
 {
 	struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[0];
 	unsigned char *va;
@@ -576,7 +582,7 @@ static bool fm10k_cleanup_headers(struct fm10k_ring *rx_ring,
 
 	/* place header in linear portion of buffer */
 	if (skb_is_nonlinear(skb))
-		fm10k_pull_tail(rx_ring, rx_desc, skb);
+		fm10k_pull_tail(skb);
 
 	/* if eth_skb_pad returns an error the skb was freed */
 	if (eth_skb_pad(skb))
@@ -711,10 +717,6 @@ static struct ethhdr *fm10k_gre_is_nvgre(struct sk_buff *skb)
 	if (nvgre_hdr->flags & FM10K_NVGRE_RESERVED0_FLAGS)
 		return NULL;
 
-	/* verify protocol is transparent Ethernet bridging */
-	if (nvgre_hdr->proto != htons(ETH_P_TEB))
-		return NULL;
-
 	/* report start of ethernet header */
 	if (nvgre_hdr->flags & NVGRE_TNI)
 		return (struct ethhdr *)(nvgre_hdr + 1);
@@ -722,15 +724,13 @@ static struct ethhdr *fm10k_gre_is_nvgre(struct sk_buff *skb)
 	return (struct ethhdr *)(&nvgre_hdr->tni);
 }
 
-static __be16 fm10k_tx_encap_offload(struct sk_buff *skb)
+__be16 fm10k_tx_encap_offload(struct sk_buff *skb)
 {
+	u8 l4_hdr = 0, inner_l4_hdr = 0, inner_l4_hlen;
 	struct ethhdr *eth_hdr;
-	u8 l4_hdr = 0;
 
-/* fm10k supports 184 octets of outer+inner headers. Minus 20 for inner L4. */
-#define FM10K_MAX_ENCAP_TRANSPORT_OFFSET	164
-	if (skb_inner_transport_header(skb) - skb_mac_header(skb) >
-	    FM10K_MAX_ENCAP_TRANSPORT_OFFSET)
+	if (skb->inner_protocol_type != ENCAP_TYPE_ETHER ||
+	    skb->inner_protocol != htons(ETH_P_TEB))
 		return 0;
 
 	switch (vlan_get_protocol(skb)) {
@@ -760,12 +760,33 @@ static __be16 fm10k_tx_encap_offload(struct sk_buff *skb)
 
 	switch (eth_hdr->h_proto) {
 	case htons(ETH_P_IP):
+		inner_l4_hdr = inner_ip_hdr(skb)->protocol;
+		break;
 	case htons(ETH_P_IPV6):
+		inner_l4_hdr = inner_ipv6_hdr(skb)->nexthdr;
+		break;
+	default:
+		return 0;
+	}
+
+	switch (inner_l4_hdr) {
+	case IPPROTO_TCP:
+		inner_l4_hlen = inner_tcp_hdrlen(skb);
+		break;
+	case IPPROTO_UDP:
+		inner_l4_hlen = 8;
 		break;
 	default:
 		return 0;
 	}
 
+	/* The hardware allows tunnel offloads only if the combined inner and
+	 * outer header is 184 bytes or less
+	 */
+	if (skb_inner_transport_header(skb) + inner_l4_hlen -
+	    skb_mac_header(skb) > FM10K_TUNNEL_HEADER_LENGTH)
+		return 0;
+
 	return eth_hdr->h_proto;
 }
 
@@ -934,10 +955,10 @@ static int __fm10k_maybe_stop_tx(struct fm10k_ring *tx_ring, u16 size)
 {
 	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
 
+	/* Memory barrier before checking head and tail */
 	smp_mb();
 
-	/* We need to check again in a case another CPU has just
-	 * made room available. */
+	/* Check again in a case another CPU has just made room available */
 	if (likely(fm10k_desc_unused(tx_ring) < size))
 		return -EBUSY;
 
@@ -1182,7 +1203,6 @@ void fm10k_tx_timeout_reset(struct fm10k_intfc *interface)
 {
 	/* Do the reset outside of interrupt context */
 	if (!test_bit(__FM10K_DOWN, &interface->state)) {
-		netdev_err(interface->netdev, "Reset interface\n");
 		interface->tx_timeout_count++;
 		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
 		fm10k_service_event_schedule(interface);
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
index 9f5457c9e627..1b2738380518 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -72,7 +72,7 @@ static bool fm10k_fifo_empty(struct fm10k_mbx_fifo *fifo)
  *  @fifo: pointer to FIFO
  *  @offset: offset to add to head
  *
- *  This function returns the indicies into the fifo based on head + offset
+ *  This function returns the indices into the fifo based on head + offset
  **/
 static u16 fm10k_fifo_head_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
 {
@@ -84,7 +84,7 @@ static u16 fm10k_fifo_head_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
  *  @fifo: pointer to FIFO
  *  @offset: offset to add to tail
  *
- *  This function returns the indicies into the fifo based on tail + offset
+ *  This function returns the indices into the fifo based on tail + offset
  **/
 static u16 fm10k_fifo_tail_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
 {
@@ -126,6 +126,18 @@ static u16 fm10k_fifo_head_drop(struct fm10k_mbx_fifo *fifo)
 }
 
 /**
+ *  fm10k_fifo_drop_all - Drop all messages in FIFO
+ *  @fifo: pointer to FIFO
+ *
+ *  This function resets the head pointer to drop all messages in the FIFO,
+ *  and ensure the FIFO is empty.
+ **/
+static void fm10k_fifo_drop_all(struct fm10k_mbx_fifo *fifo)
+{
+	fifo->head = fifo->tail;
+}
+
+/**
  *  fm10k_mbx_index_len - Convert a head/tail index into a length value
  *  @mbx: pointer to mailbox
  *  @head: head index
@@ -315,7 +327,7 @@ static u16 fm10k_mbx_validate_msg_size(struct fm10k_mbx_info *mbx, u16 len)
 	} while (total_len < len);
 
 	/* message extends out of pushed section, but fits in FIFO */
-	if ((len < total_len) && (msg_len <= mbx->rx.size))
+	if ((len < total_len) && (msg_len <= mbx->max_size))
 		return 0;
 
 	/* return length of invalid section */
@@ -326,8 +338,7 @@ static u16 fm10k_mbx_validate_msg_size(struct fm10k_mbx_info *mbx, u16 len)
  *  fm10k_mbx_write_copy - pulls data off of Tx FIFO and places it in mbmem
  *  @mbx: pointer to mailbox
  *
- *  This function will take a seciton of the Rx FIFO and copy it into the
-		mbx->tail--;
+ *  This function will take a section of the Tx FIFO and copy it into the
  *  mailbox memory.  The offset in mbmem is based on the lower bits of the
  *  tail and len determines the length to copy.
  **/
@@ -418,7 +429,7 @@ static void fm10k_mbx_pull_head(struct fm10k_hw *hw,
  *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *
- *  This function will take a seciton of the mailbox memory and copy it
+ *  This function will take a section of the mailbox memory and copy it
  *  into the Rx FIFO.  The offset is based on the lower bits of the
  *  head and len determines the length to copy.
  **/
@@ -464,7 +475,7 @@ static void fm10k_mbx_read_copy(struct fm10k_hw *hw,
  *  @tail: tail index of message
  *
  *  This function will first validate the tail index and size for the
- *  incoming message.  It then updates the acknowlegment number and
+ *  incoming message.  It then updates the acknowledgment number and
  *  copies the data into the FIFO.  It will return the number of messages
  *  dequeued on success and a negative value on error.
  **/
@@ -761,7 +772,7 @@ static s32 fm10k_mbx_enqueue_tx(struct fm10k_hw *hw,
 		err = fm10k_fifo_enqueue(&mbx->tx, msg);
 	}
 
-	/* if we failed trhead the error */
+	/* if we failed treat the error */
 	if (err) {
 		mbx->timeout = 0;
 		mbx->tx_busy++;
@@ -815,10 +826,10 @@ static void fm10k_mbx_write(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx)
 {
 	u32 mbmem = mbx->mbmem_reg;
 
-	/* write new msg header to notify recepient of change */
+	/* write new msg header to notify recipient of change */
 	fm10k_write_reg(hw, mbmem, mbx->mbx_hdr);
 
-	/* write mailbox to sent interrupt */
+	/* write mailbox to send interrupt */
 	if (mbx->mbx_lock)
 		fm10k_write_reg(hw, mbx->mbx_reg, mbx->mbx_lock);
 
@@ -1052,8 +1063,11 @@ static void fm10k_mbx_reset_work(struct fm10k_mbx_info *mbx)
  *  @mbx: pointer to mailbox
  *  @size: new value for max_size
  *
- *  This function will update the max_size value and drop any outgoing messages
- *  from the head of the Tx FIFO that are larger than max_size.
+ *  This function updates the max_size value and drops any outgoing messages
+ *  at the head of the Tx FIFO if they are larger than max_size. It does not
+ *  drop all messages, as this is too difficult to parse and remove them from
+ *  the FIFO. Instead, rely on the checking to ensure that messages larger
+ *  than max_size aren't pushed into the memory buffer.
  **/
 static void fm10k_mbx_update_max_size(struct fm10k_mbx_info *mbx, u16 size)
 {
@@ -1251,7 +1265,7 @@ static s32 fm10k_mbx_process_error(struct fm10k_hw *hw,
 	/* we will need to pull all of the fields for verification */
 	head = FM10K_MSG_HDR_FIELD_GET(*hdr, HEAD);
 
-	/* we only have lower 10 bits of error number os add upper bits */
+	/* we only have lower 10 bits of error number so add upper bits */
 	err_no = FM10K_MSG_HDR_FIELD_GET(*hdr, ERR_NO);
 	err_no |= ~FM10K_MSG_HDR_MASK(ERR_NO);
 
@@ -1371,9 +1385,11 @@ static void fm10k_mbx_disconnect(struct fm10k_hw *hw,
 		timeout -= FM10K_MBX_POLL_DELAY;
 	} while ((timeout > 0) && (mbx->state != FM10K_STATE_CLOSED));
 
-	/* in case we didn't close just force the mailbox into shutdown */
+	/* in case we didn't close, just force the mailbox into shutdown and
+	 * drop all left over messages in the FIFO.
+	 */
 	fm10k_mbx_connect_reset(mbx);
-	fm10k_mbx_update_max_size(mbx, 0);
+	fm10k_fifo_drop_all(&mbx->tx);
 
 	fm10k_write_reg(hw, mbx->mbmem_reg, 0);
 }
@@ -1548,7 +1564,7 @@ s32 fm10k_pfvf_mbx_init(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx,
 	mbx->timeout = 0;
 	mbx->udelay = FM10K_MBX_INIT_DELAY;
 
-	/* initalize tail and head */
+	/* initialize tail and head */
 	mbx->tail = 1;
 	mbx->head = 1;
 
@@ -1627,7 +1643,7 @@ static void fm10k_sm_mbx_connect_reset(struct fm10k_mbx_info *mbx)
 	mbx->local = FM10K_SM_MBX_VERSION;
 	mbx->remote = 0;
 
-	/* initalize tail and head */
+	/* initialize tail and head */
 	mbx->tail = 1;
 	mbx->head = 1;
 
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
index cfde8bac1aeb..2f4f41b7eae7 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -356,7 +356,7 @@ static void fm10k_free_all_rx_resources(struct fm10k_intfc *interface)
  * fm10k_request_glort_range - Request GLORTs for use in configuring rules
  * @interface: board private structure
  *
- * This function allocates a range of glorts for this inteface to use.
+ * This function allocates a range of glorts for this interface to use.
  **/
 static void fm10k_request_glort_range(struct fm10k_intfc *interface)
 {
@@ -770,18 +770,18 @@ static int fm10k_update_vid(struct net_device *netdev, u16 vid, bool set)
 	if (hw->mac.vlan_override)
 		return -EACCES;
 
-	/* if default VLAN is already present do nothing */
-	if (vid == hw->mac.default_vid)
-		return -EBUSY;
-
 	/* update active_vlans bitmask */
 	set_bit(vid, interface->active_vlans);
 	if (!set)
 		clear_bit(vid, interface->active_vlans);
 
+	/* if default VLAN is already present do nothing */
+	if (vid == hw->mac.default_vid)
+		return 0;
+
 	fm10k_mbx_lock(interface);
 
-	/* only need to update the VLAN if not in promiscous mode */
+	/* only need to update the VLAN if not in promiscuous mode */
 	if (!(netdev->flags & IFF_PROMISC)) {
 		err = hw->mac.ops.update_vlan(hw, vid, 0, set);
 		if (err)
@@ -970,14 +970,7 @@ static void fm10k_set_rx_mode(struct net_device *dev)
 
 	fm10k_mbx_lock(interface);
 
-	/* syncronize all of the addresses */
-	if (xcast_mode != FM10K_XCAST_MODE_PROMISC) {
-		__dev_uc_sync(dev, fm10k_uc_sync, fm10k_uc_unsync);
-		if (xcast_mode != FM10K_XCAST_MODE_ALLMULTI)
-			__dev_mc_sync(dev, fm10k_mc_sync, fm10k_mc_unsync);
-	}
-
-	/* if we aren't changing modes there is nothing to do */
+	/* update xcast mode first, but only if it changed */
 	if (interface->xcast_mode != xcast_mode) {
 		/* update VLAN table */
 		if (xcast_mode == FM10K_XCAST_MODE_PROMISC)
@@ -992,6 +985,13 @@ static void fm10k_set_rx_mode(struct net_device *dev)
 		interface->xcast_mode = xcast_mode;
 	}
 
+	/* synchronize all of the addresses */
+	if (xcast_mode != FM10K_XCAST_MODE_PROMISC) {
+		__dev_uc_sync(dev, fm10k_uc_sync, fm10k_uc_unsync);
+		if (xcast_mode != FM10K_XCAST_MODE_ALLMULTI)
+			__dev_mc_sync(dev, fm10k_mc_sync, fm10k_mc_unsync);
+	}
+
 	fm10k_mbx_unlock(interface);
 }
 
@@ -1051,16 +1051,16 @@ void fm10k_restore_rx_state(struct fm10k_intfc *interface)
 					   vid, true, 0);
 	}
 
-	/* syncronize all of the addresses */
+	/* update xcast mode before syncronizing addresses */
+	hw->mac.ops.update_xcast_mode(hw, glort, xcast_mode);
+
+	/* synchronize all of the addresses */
 	if (xcast_mode != FM10K_XCAST_MODE_PROMISC) {
 		__dev_uc_sync(netdev, fm10k_uc_sync, fm10k_uc_unsync);
 		if (xcast_mode != FM10K_XCAST_MODE_ALLMULTI)
 			__dev_mc_sync(netdev, fm10k_mc_sync, fm10k_mc_unsync);
 	}
 
-	/* update xcast mode */
-	hw->mac.ops.update_xcast_mode(hw, glort, xcast_mode);
-
 	fm10k_mbx_unlock(interface);
 
 	/* record updated xcast mode state */
@@ -1126,7 +1126,7 @@ static struct rtnl_link_stats64 *fm10k_get_stats64(struct net_device *netdev,
 	}
 
 	for (i = 0; i < interface->num_tx_queues; i++) {
-		ring = ACCESS_ONCE(interface->rx_ring[i]);
+		ring = ACCESS_ONCE(interface->tx_ring[i]);
 
 		if (!ring)
 			continue;
@@ -1350,6 +1350,16 @@ static void fm10k_dfwd_del_station(struct net_device *dev, void *priv)
 	}
 }
 
+static netdev_features_t fm10k_features_check(struct sk_buff *skb,
+					      struct net_device *dev,
+					      netdev_features_t features)
+{
+	if (!skb->encapsulation || fm10k_tx_encap_offload(skb))
+		return features;
+
+	return features & ~(NETIF_F_ALL_CSUM | NETIF_F_GSO_MASK);
+}
+
 static const struct net_device_ops fm10k_netdev_ops = {
 	.ndo_open		= fm10k_open,
 	.ndo_stop		= fm10k_close,
@@ -1372,6 +1382,10 @@ static const struct net_device_ops fm10k_netdev_ops = {
 	.ndo_do_ioctl		= fm10k_ioctl,
 	.ndo_dfwd_add_station	= fm10k_dfwd_add_station,
 	.ndo_dfwd_del_station	= fm10k_dfwd_del_station,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= fm10k_netpoll,
+#endif
+	.ndo_features_check	= fm10k_features_check,
 };
 
 #define DEFAULT_DEBUG_LEVEL_SHIFT 3
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
index 4f5892cc32d7..df9fda38bdd1 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -94,7 +94,7 @@ void fm10k_service_event_schedule(struct fm10k_intfc *interface)
 {
 	if (!test_bit(__FM10K_SERVICE_DISABLE, &interface->state) &&
 	    !test_and_set_bit(__FM10K_SERVICE_SCHED, &interface->state))
-		schedule_work(&interface->service_task);
+		queue_work(fm10k_workqueue, &interface->service_task);
 }
 
 static void fm10k_service_event_complete(struct fm10k_intfc *interface)
@@ -191,7 +191,6 @@ static void fm10k_reset_subtask(struct fm10k_intfc *interface)
 	interface->flags &= ~FM10K_FLAG_RESET_REQUESTED;
 
 	netdev_err(interface->netdev, "Reset interface\n");
-	interface->tx_timeout_count++;
 
 	fm10k_reinit(interface);
 }
@@ -357,11 +356,10 @@ void fm10k_update_stats(struct fm10k_intfc *interface)
 	net_stats->rx_packets = pkts;
 	interface->alloc_failed = alloc_failed;
 	interface->rx_csum_errors = rx_csum_errors;
-	interface->rx_errors = rx_errors;
 
 	hw->mac.ops.update_hw_stats(hw, &interface->stats);
 
-	for (i = 0; i < FM10K_MAX_QUEUES_PF; i++) {
+	for (i = 0; i < hw->mac.max_queues; i++) {
 		struct fm10k_hw_stats_q *q = &interface->stats.q[i];
 
 		tx_bytes_nic += q->tx_bytes.count;
@@ -378,7 +376,7 @@ void fm10k_update_stats(struct fm10k_intfc *interface)
 	interface->rx_drops_nic = rx_drops_nic;
 
 	/* Fill out the OS statistics structure */
-	net_stats->rx_errors = interface->stats.xec.count;
+	net_stats->rx_errors = rx_errors;
 	net_stats->rx_dropped = interface->stats.nodesc_drop.count;
 }
 
@@ -648,7 +646,7 @@ static void fm10k_configure_rx_ring(struct fm10k_intfc *interface,
 	/* Configure the Rx buffer size for one buff without split */
 	srrctl |= FM10K_RX_BUFSZ >> FM10K_SRRCTL_BSIZEPKT_SHIFT;
 
-	/* Configure the Rx ring to supress loopback packets */
+	/* Configure the Rx ring to suppress loopback packets */
 	srrctl |= FM10K_SRRCTL_LOOPBACK_SUPPRESS;
 	fm10k_write_reg(hw, FM10K_SRRCTL(reg_idx), srrctl);
 
@@ -808,7 +806,7 @@ static void fm10k_napi_enable_all(struct fm10k_intfc *interface)
 	}
 }
 
-static irqreturn_t fm10k_msix_clean_rings(int irq, void *data)
+static irqreturn_t fm10k_msix_clean_rings(int __always_unused irq, void *data)
 {
 	struct fm10k_q_vector *q_vector = data;
 
@@ -818,7 +816,7 @@ static irqreturn_t fm10k_msix_clean_rings(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t fm10k_msix_mbx_vf(int irq, void *data)
+static irqreturn_t fm10k_msix_mbx_vf(int __always_unused irq, void *data)
 {
 	struct fm10k_intfc *interface = data;
 	struct fm10k_hw *hw = &interface->hw;
@@ -840,6 +838,28 @@ static irqreturn_t fm10k_msix_mbx_vf(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+#ifdef CONFIG_NET_POLL_CONTROLLER
+/**
+ *  fm10k_netpoll - A Polling 'interrupt' handler
+ *  @netdev: network interface device structure
+ *
+ *  This is used by netconsole to send skbs without having to re-enable
+ *  interrupts. It's not called while the normal interrupt routine is executing.
+ **/
+void fm10k_netpoll(struct net_device *netdev)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	int i;
+
+	/* if interface is down do nothing */
+	if (test_bit(__FM10K_DOWN, &interface->state))
+		return;
+
+	for (i = 0; i < interface->num_q_vectors; i++)
+		fm10k_msix_clean_rings(0, interface->q_vector[i]);
+}
+
+#endif
 #define FM10K_ERR_MSG(type) case (type): error = #type; break
 static void fm10k_print_fault(struct fm10k_intfc *interface, int type,
 			      struct fm10k_fault *fault)
@@ -964,7 +984,7 @@ static void fm10k_reset_drop_on_empty(struct fm10k_intfc *interface, u32 eicr)
 	}
 }
 
-static irqreturn_t fm10k_msix_mbx_pf(int irq, void *data)
+static irqreturn_t fm10k_msix_mbx_pf(int __always_unused irq, void *data)
 {
 	struct fm10k_intfc *interface = data;
 	struct fm10k_hw *hw = &interface->hw;
@@ -986,6 +1006,7 @@ static irqreturn_t fm10k_msix_mbx_pf(int irq, void *data)
 	/* service mailboxes */
 	if (fm10k_mbx_trylock(interface)) {
 		mbx->ops.process(hw, mbx);
+		/* handle VFLRE events */
 		fm10k_iov_event(interface);
 		fm10k_mbx_unlock(interface);
 	}
@@ -1002,6 +1023,8 @@ static irqreturn_t fm10k_msix_mbx_pf(int irq, void *data)
 
 	/* we should validate host state after interrupt event */
 	hw->mac.get_host_state = 1;
+
+	/* validate host state, and handle VF mailboxes in the service task */
 	fm10k_service_event_schedule(interface);
 
 	/* re-enable mailbox interrupt and indicate 20us delay */
@@ -1069,7 +1092,7 @@ static s32 fm10k_mbx_mac_addr(struct fm10k_hw *hw, u32 **results,
 }
 
 static s32 fm10k_1588_msg_vf(struct fm10k_hw *hw, u32 **results,
-			     struct fm10k_mbx_info *mbx)
+			     struct fm10k_mbx_info __always_unused *mbx)
 {
 	struct fm10k_intfc *interface;
 	u64 timestamp;
@@ -1089,7 +1112,7 @@ static s32 fm10k_1588_msg_vf(struct fm10k_hw *hw, u32 **results,
 
 /* generic error handler for mailbox issues */
 static s32 fm10k_mbx_error(struct fm10k_hw *hw, u32 **results,
-			   struct fm10k_mbx_info *mbx)
+			   struct fm10k_mbx_info __always_unused *mbx)
 {
 	struct fm10k_intfc *interface;
 	struct pci_dev *pdev;
@@ -1165,7 +1188,7 @@ static s32 fm10k_lport_map(struct fm10k_hw *hw, u32 **results,
 }
 
 static s32 fm10k_update_pvid(struct fm10k_hw *hw, u32 **results,
-			     struct fm10k_mbx_info *mbx)
+			     struct fm10k_mbx_info __always_unused *mbx)
 {
 	struct fm10k_intfc *interface;
 	u16 glort, pvid;
@@ -1206,7 +1229,7 @@ static s32 fm10k_update_pvid(struct fm10k_hw *hw, u32 **results,
 }
 
 static s32 fm10k_1588_msg_pf(struct fm10k_hw *hw, u32 **results,
-			     struct fm10k_mbx_info *mbx)
+			     struct fm10k_mbx_info __always_unused *mbx)
 {
 	struct fm10k_swapi_1588_timestamp timestamp;
 	struct fm10k_iov_data *iov_data;
@@ -1488,7 +1511,7 @@ void fm10k_up(struct fm10k_intfc *interface)
 	/* enable transmits */
 	netif_tx_start_all_queues(interface->netdev);
 
-	/* kick off the service timer */
+	/* kick off the service timer now */
 	hw->mac.get_host_state = 1;
 	mod_timer(&interface->service_timer, jiffies);
 }
@@ -1528,8 +1551,6 @@ void fm10k_down(struct fm10k_intfc *interface)
 	/* disable polling routines */
 	fm10k_napi_disable_all(interface);
 
-	del_timer_sync(&interface->service_timer);
-
 	/* capture stats one last time before stopping interface */
 	fm10k_update_stats(interface);
 
@@ -1655,6 +1676,9 @@ static int fm10k_sw_init(struct fm10k_intfc *interface,
 		    (unsigned long)interface);
 	INIT_WORK(&interface->service_task, fm10k_service_task);
 
+	/* kick off service timer now, even when interface is down */
+	mod_timer(&interface->service_timer, (HZ * 2) + jiffies);
+
 	/* Intitialize timestamp data */
 	fm10k_ts_init(interface);
 
@@ -1871,6 +1895,8 @@ static void fm10k_remove(struct pci_dev *pdev)
 	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
 	struct net_device *netdev = interface->netdev;
 
+	del_timer_sync(&interface->service_timer);
+
 	set_bit(__FM10K_SERVICE_DISABLE, &interface->state);
 	cancel_work_sync(&interface->service_task);
 
@@ -1984,7 +2010,8 @@ static int fm10k_resume(struct pci_dev *pdev)
  * a sleep state.  The fm10k hardware does not support wake on lan so the
  * driver simply needs to shut down the device so it is in a low power state.
  **/
-static int fm10k_suspend(struct pci_dev *pdev, pm_message_t state)
+static int fm10k_suspend(struct pci_dev *pdev,
+			 pm_message_t __always_unused state)
 {
 	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
 	struct net_device *netdev = interface->netdev;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
index 7e4711958e46..891e21874b2a 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
@@ -234,8 +234,7 @@ static s32 fm10k_update_vlan_pf(struct fm10k_hw *hw, u32 vid, u8 vsi, bool set)
 	vid = (vid << 17) >> 17;
 
 	/* verify the reserved 0 fields are 0 */
-	if (len >= FM10K_VLAN_TABLE_VID_MAX ||
-	    vid >= FM10K_VLAN_TABLE_VID_MAX)
+	if (len >= FM10K_VLAN_TABLE_VID_MAX || vid >= FM10K_VLAN_TABLE_VID_MAX)
 		return FM10K_ERR_PARAM;
 
 	/* Loop through the table updating all required VLANs */
@@ -312,7 +311,7 @@ bool fm10k_glort_valid_pf(struct fm10k_hw *hw, u16 glort)
 }
 
 /**
- *  fm10k_update_uc_addr_pf - Update device unicast addresss
+ *  fm10k_update_xc_addr_pf - Update device addresses
  *  @hw: pointer to the HW structure
  *  @glort: base resource tag for this request
  *  @mac: MAC address to add/remove from table
@@ -330,6 +329,9 @@ static s32 fm10k_update_xc_addr_pf(struct fm10k_hw *hw, u16 glort,
 	struct fm10k_mac_update mac_update;
 	u32 msg[5];
 
+	/* clear set bit from VLAN ID */
+	vid &= ~FM10K_VLAN_CLEAR;
+
 	/* if glort or vlan are not valid return error */
 	if (!fm10k_glort_valid_pf(hw, glort) || vid >= FM10K_VLAN_TABLE_VID_MAX)
 		return FM10K_ERR_PARAM;
@@ -356,7 +358,7 @@ static s32 fm10k_update_xc_addr_pf(struct fm10k_hw *hw, u16 glort,
 }
 
 /**
- *  fm10k_update_uc_addr_pf - Update device unicast addresss
+ *  fm10k_update_uc_addr_pf - Update device unicast addresses
  *  @hw: pointer to the HW structure
  *  @glort: base resource tag for this request
  *  @mac: MAC address to add/remove from table
@@ -454,7 +456,7 @@ static void fm10k_update_int_moderator_pf(struct fm10k_hw *hw)
 			break;
 	}
 
-	/* always reset VFITR2[0] to point to last enabled PF vector*/
+	/* always reset VFITR2[0] to point to last enabled PF vector */
 	fm10k_write_reg(hw, FM10K_ITR2(FM10K_ITR_REG_COUNT_PF), i);
 
 	/* reset ITR2[0] to point to last enabled PF vector */
@@ -677,7 +679,8 @@ static s32 fm10k_iov_assign_resources_pf(struct fm10k_hw *hw, u16 num_vfs,
 	/* loop through unallocated rings assigning them back to PF */
 	for (i = FM10K_MAX_QUEUES_PF; i < vf_q_idx; i++) {
 		fm10k_write_reg(hw, FM10K_TXDCTL(i), 0);
-		fm10k_write_reg(hw, FM10K_TXQCTL(i), FM10K_TXQCTL_PF | vid);
+		fm10k_write_reg(hw, FM10K_TXQCTL(i), FM10K_TXQCTL_PF |
+				FM10K_TXQCTL_UNLIMITED_BW | vid);
 		fm10k_write_reg(hw, FM10K_RXQCTL(i), FM10K_RXQCTL_PF);
 	}
 
@@ -812,7 +815,7 @@ static s32 fm10k_iov_assign_int_moderator_pf(struct fm10k_hw *hw, u16 vf_idx)
 	if (vf_idx >= hw->iov.num_vfs)
 		return FM10K_ERR_PARAM;
 
-	/* determine vector offset and count*/
+	/* determine vector offset and count */
 	vf_v_idx = fm10k_vf_vector_index(hw, vf_idx);
 	vf_v_limit = vf_v_idx + fm10k_vectors_per_pool(hw);
 
@@ -951,7 +954,7 @@ static s32 fm10k_iov_reset_resources_pf(struct fm10k_hw *hw,
 	if (vf_info->mbx.ops.disconnect)
 		vf_info->mbx.ops.disconnect(hw, &vf_info->mbx);
 
-	/* determine vector offset and count*/
+	/* determine vector offset and count */
 	vf_v_idx = fm10k_vf_vector_index(hw, vf_idx);
 	vf_v_limit = vf_v_idx + fm10k_vectors_per_pool(hw);
 
@@ -1035,7 +1038,7 @@ static s32 fm10k_iov_reset_resources_pf(struct fm10k_hw *hw,
 			((u32)vf_info->mac[2]);
 	}
 
-	/* map queue pairs back to VF from last to first*/
+	/* map queue pairs back to VF from last to first */
 	for (i = queues_per_pool; i--;) {
 		fm10k_write_reg(hw, FM10K_TDBAL(vf_q_idx + i), tdbal);
 		fm10k_write_reg(hw, FM10K_TDBAH(vf_q_idx + i), tdbah);
@@ -1141,7 +1144,7 @@ static s32 fm10k_iov_report_timestamp_pf(struct fm10k_hw *hw,
  *
  *  This function is a default handler for MSI-X requests from the VF.  The
  *  assumption is that in this case it is acceptable to just directly
- *  hand off the message form the VF to the underlying shared code.
+ *  hand off the message from the VF to the underlying shared code.
  **/
 s32 fm10k_iov_msg_msix_pf(struct fm10k_hw *hw, u32 **results,
 			  struct fm10k_mbx_info *mbx)
@@ -1160,7 +1163,7 @@ s32 fm10k_iov_msg_msix_pf(struct fm10k_hw *hw, u32 **results,
  *
  *  This function is a default handler for MAC/VLAN requests from the VF.
  *  The assumption is that in this case it is acceptable to just directly
- *  hand off the message form the VF to the underlying shared code.
+ *  hand off the message from the VF to the underlying shared code.
  **/
 s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *hw, u32 **results,
 			      struct fm10k_mbx_info *mbx)
@@ -1250,8 +1253,8 @@ s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *hw, u32 **results,
 		}
 
 		/* notify switch of request for new multicast address */
-		err = hw->mac.ops.update_mc_addr(hw, vf_info->glort, mac,
-						 !(vlan & FM10K_VLAN_CLEAR), 0);
+		err = hw->mac.ops.update_mc_addr(hw, vf_info->glort, mac, vlan,
+						 !(vlan & FM10K_VLAN_CLEAR));
 	}
 
 	return err;
@@ -1404,7 +1407,7 @@ static void fm10k_update_hw_stats_pf(struct fm10k_hw *hw,
 						    &stats->vlan_drop);
 		loopback_drop = fm10k_read_hw_stats_32b(hw,
 							FM10K_STATS_LOOPBACK_DROP,
-						     &stats->loopback_drop);
+							&stats->loopback_drop);
 		nodesc_drop = fm10k_read_hw_stats_32b(hw,
 						      FM10K_STATS_NODESC_DROP,
 						      &stats->nodesc_drop);
@@ -1573,7 +1576,7 @@ static s32 fm10k_get_host_state_pf(struct fm10k_hw *hw, bool *switch_ready)
 	s32 ret_val = 0;
 	u32 dma_ctrl2;
 
-	/* verify the switch is ready for interraction */
+	/* verify the switch is ready for interaction */
 	dma_ctrl2 = fm10k_read_reg(hw, FM10K_DMA_CTRL2);
 	if (!(dma_ctrl2 & FM10K_DMA_CTRL2_SWITCH_READY))
 		goto out;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c b/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
index d966044e017a..9043633c3e50 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -285,7 +285,7 @@ static int fm10k_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int fm10k_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int fm10k_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct fm10k_intfc *interface;
 	unsigned long flags;
@@ -297,17 +297,17 @@ static int fm10k_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 	now = fm10k_systime_read(interface) + interface->ptp_adjust;
 	read_unlock_irqrestore(&interface->systime_lock, flags);
 
-	*ts = ns_to_timespec(now);
+	*ts = ns_to_timespec64(now);
 
 	return 0;
 }
 
 static int fm10k_ptp_settime(struct ptp_clock_info *ptp,
-			     const struct timespec *ts)
+			     const struct timespec64 *ts)
 {
 	struct fm10k_intfc *interface;
 	unsigned long flags;
-	u64 ns = timespec_to_ns(ts);
+	u64 ns = timespec64_to_ns(ts);
 
 	interface = container_of(ptp, struct fm10k_intfc, ptp_caps);
 
@@ -319,7 +319,8 @@ static int fm10k_ptp_settime(struct ptp_clock_info *ptp,
 }
 
 static int fm10k_ptp_enable(struct ptp_clock_info *ptp,
-			    struct ptp_clock_request *rq, int on)
+			    struct ptp_clock_request *rq,
+			    int __always_unused on)
 {
 	struct ptp_clock_time *t = &rq->perout.period;
 	struct fm10k_intfc *interface;
@@ -419,8 +420,8 @@ void fm10k_ptp_register(struct fm10k_intfc *interface)
 	ptp_caps->max_adj	= 976562;
 	ptp_caps->adjfreq	= fm10k_ptp_adjfreq;
 	ptp_caps->adjtime	= fm10k_ptp_adjtime;
-	ptp_caps->gettime	= fm10k_ptp_gettime;
-	ptp_caps->settime	= fm10k_ptp_settime;
+	ptp_caps->gettime64	= fm10k_ptp_gettime;
+	ptp_caps->settime64	= fm10k_ptp_settime;
 
 	/* provide pins if BAR4 is accessible */
 	if (interface->sw_addr) {
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
index fd0a05f011a8..9b29d7b0377a 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
@@ -710,7 +710,7 @@ void fm10k_tlv_msg_test_create(u32 *msg, u32 attr_flags)
 /**
  *  fm10k_tlv_msg_test - Validate all results on test message receive
  *  @hw: Pointer to hardware structure
- *  @results: Pointer array to attributes in the mesage
+ *  @results: Pointer array to attributes in the message
  *  @mbx: Pointer to mailbox information structure
  *
  *  This function does a check to verify all attributes match what the test
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_type.h b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
index 7c6d9d5a8ae5..4af96686c584 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_type.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
@@ -356,6 +356,9 @@ struct fm10k_hw;
 #define FM10K_QUEUE_DISABLE_TIMEOUT		100
 #define FM10K_RESET_TIMEOUT			150
 
+/* Maximum supported combined inner and outer header length for encapsulation */
+#define FM10K_TUNNEL_HEADER_LENGTH	184
+
 /* VF registers */
 #define FM10K_VFCTRL		0x00000
 #define FM10K_VFCTRL_RST			0x00000008
@@ -593,7 +596,7 @@ struct fm10k_vf_info {
 	u16			sw_vid;		/* Switch API assigned VLAN */
 	u16			pf_vid;		/* PF assigned Default VLAN */
 	u8			mac[ETH_ALEN];	/* PF Default MAC address */
-	u8			vsi;		/* VSI idenfifier */
+	u8			vsi;		/* VSI identifier */
 	u8			vf_idx;		/* which VF this is */
 	u8			vf_flags;	/* flags indicating what modes
 						 * are supported for the port
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_vf.c b/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
index f0aa0f97b4a9..94f0f6a146d9 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -37,7 +37,7 @@ static s32 fm10k_stop_hw_vf(struct fm10k_hw *hw)
 	if (err)
 		return err;
 
-	/* If permenant address is set then we need to restore it */
+	/* If permanent address is set then we need to restore it */
 	if (is_valid_ether_addr(perm_addr)) {
 		bal = (((u32)perm_addr[3]) << 24) |
 		      (((u32)perm_addr[4]) << 16) |
@@ -65,7 +65,7 @@ static s32 fm10k_stop_hw_vf(struct fm10k_hw *hw)
  *  fm10k_reset_hw_vf - VF hardware reset
  *  @hw: pointer to hardware structure
  *
- *  This function should return the hardare to a state similar to the
+ *  This function should return the hardware to a state similar to the
  *  one it is in after just being initialized.
  **/
 static s32 fm10k_reset_hw_vf(struct fm10k_hw *hw)
@@ -124,6 +124,10 @@ static s32 fm10k_init_hw_vf(struct fm10k_hw *hw)
 	/* record maximum queue count */
 	hw->mac.max_queues = i;
 
+	/* fetch default VLAN */
+	hw->mac.default_vid = (fm10k_read_reg(hw, FM10K_TXQCTL(0)) &
+			       FM10K_TXQCTL_VID_MASK) >> FM10K_TXQCTL_VID_SHIFT;
+
 	return 0;
 }
 
@@ -252,7 +256,7 @@ static s32 fm10k_read_mac_addr_vf(struct fm10k_hw *hw)
 }
 
 /**
- *  fm10k_update_uc_addr_vf - Update device unicast address
+ *  fm10k_update_uc_addr_vf - Update device unicast addresses
  *  @hw: pointer to the HW structure
  *  @glort: unused
  *  @mac: MAC address to add/remove from table
@@ -282,7 +286,7 @@ static s32 fm10k_update_uc_addr_vf(struct fm10k_hw *hw, u16 glort,
 	    memcmp(hw->mac.perm_addr, mac, ETH_ALEN))
 		return FM10K_ERR_PARAM;
 
-	/* add bit to notify us if this is a set of clear operation */
+	/* add bit to notify us if this is a set or clear operation */
 	if (!add)
 		vid |= FM10K_VLAN_CLEAR;
 
@@ -295,7 +299,7 @@ static s32 fm10k_update_uc_addr_vf(struct fm10k_hw *hw, u16 glort,
 }
 
 /**
- *  fm10k_update_mc_addr_vf - Update device multicast address
+ *  fm10k_update_mc_addr_vf - Update device multicast addresses
  *  @hw: pointer to the HW structure
  *  @glort: unused
  *  @mac: MAC address to add/remove from table
@@ -319,7 +323,7 @@ static s32 fm10k_update_mc_addr_vf(struct fm10k_hw *hw, u16 glort,
 	if (!is_multicast_ether_addr(mac))
 		return FM10K_ERR_PARAM;
 
-	/* add bit to notify us if this is a set of clear operation */
+	/* add bit to notify us if this is a set or clear operation */
 	if (!add)
 		vid |= FM10K_VLAN_CLEAR;
 
@@ -515,7 +519,7 @@ static s32 fm10k_adjust_systime_vf(struct fm10k_hw *hw, s32 ppb)
  *  @hw: pointer to the hardware structure
  *
  *  Function reads the content of 2 registers, combined to represent a 64 bit
- *  value measured in nanosecods.  In order to guarantee the value is accurate
+ *  value measured in nanoseconds.  In order to guarantee the value is accurate
  *  we check the 32 most significant bits both before and after reading the
  *  32 least significant bits to verify they didn't change as we were reading
  *  the registers.
diff --git a/drivers/net/ethernet/intel/i40e/Makefile b/drivers/net/ethernet/intel/i40e/Makefile
index c40581999121..b4729ba57c9c 100644
--- a/drivers/net/ethernet/intel/i40e/Makefile
+++ b/drivers/net/ethernet/intel/i40e/Makefile
@@ -1,7 +1,7 @@
 ################################################################################
 #
 # Intel Ethernet Controller XL710 Family Linux Driver
-# Copyright(c) 2013 - 2014 Intel Corporation.
+# Copyright(c) 2013 - 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,
diff --git a/drivers/net/ethernet/intel/i40e/i40e.h b/drivers/net/ethernet/intel/i40e/i40e.h
index 2b65cdcad6ba..33c35d3b7420 100644
--- a/drivers/net/ethernet/intel/i40e/i40e.h
+++ b/drivers/net/ethernet/intel/i40e/i40e.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -36,6 +36,7 @@
 #include <linux/aer.h>
 #include <linux/netdevice.h>
 #include <linux/ioport.h>
+#include <linux/iommu.h>
 #include <linux/slab.h>
 #include <linux/list.h>
 #include <linux/string.h>
@@ -49,6 +50,7 @@
 #include <net/ip6_checksum.h>
 #include <linux/ethtool.h>
 #include <linux/if_vlan.h>
+#include <linux/if_bridge.h>
 #include <linux/clocksource.h>
 #include <linux/net_tstamp.h>
 #include <linux/ptp_clock_kernel.h>
@@ -70,6 +72,7 @@
 
 #define I40E_MAX_NUM_DESCRIPTORS      4096
 #define I40E_MAX_REGISTER     0x800000
+#define I40E_MAX_CSR_SPACE (4 * 1024 * 1024 - 64 * 1024)
 #define I40E_DEFAULT_NUM_DESCRIPTORS  512
 #define I40E_REQ_DESCRIPTOR_MULTIPLE  32
 #define I40E_MIN_NUM_DESCRIPTORS      64
@@ -94,6 +97,9 @@
 #define I40E_QUEUE_WAIT_RETRY_LIMIT   10
 #define I40E_INT_NAME_STR_LEN        (IFNAMSIZ + 9)
 
+/* Ethtool Private Flags */
+#define I40E_PRIV_FLAGS_NPAR_FLAG	(1 << 0)
+
 #define I40E_NVM_VERSION_LO_SHIFT  0
 #define I40E_NVM_VERSION_LO_MASK   (0xff << I40E_NVM_VERSION_LO_SHIFT)
 #define I40E_NVM_VERSION_HI_SHIFT  12
@@ -140,6 +146,7 @@ enum i40e_state_t {
 	__I40E_CORE_RESET_REQUESTED,
 	__I40E_GLOBAL_RESET_REQUESTED,
 	__I40E_EMP_RESET_REQUESTED,
+	__I40E_EMP_RESET_INTR_RECEIVED,
 	__I40E_FILTER_OVERFLOW_PROMISC,
 	__I40E_SUSPENDED,
 	__I40E_PTP_TX_IN_PROGRESS,
@@ -168,6 +175,9 @@ struct i40e_lump_tracking {
 #define I40E_FDIR_MAX_RAW_PACKET_SIZE	512
 #define I40E_FDIR_BUFFER_FULL_MARGIN	10
 #define I40E_FDIR_BUFFER_HEAD_ROOM	32
+#define I40E_FDIR_BUFFER_HEAD_ROOM_FOR_ATR (I40E_FDIR_BUFFER_HEAD_ROOM * 4)
+
+#define I40E_HKEY_ARRAY_SIZE ((I40E_PFQF_HKEY_MAX_INDEX + 1) * 4)
 
 enum i40e_fd_stat_idx {
 	I40E_FD_STAT_ATR,
@@ -232,17 +242,17 @@ struct i40e_pf {
 	bool fc_autoneg_status;
 
 	u16 eeprom_version;
-	u16 num_vmdq_vsis;         /* num vmdq vsis this pf has set up */
+	u16 num_vmdq_vsis;         /* num vmdq vsis this PF has set up */
 	u16 num_vmdq_qps;          /* num queue pairs per vmdq pool */
 	u16 num_vmdq_msix;         /* num queue vectors per vmdq pool */
-	u16 num_req_vfs;           /* num vfs requested for this vf */
-	u16 num_vf_qps;            /* num queue pairs per vf */
+	u16 num_req_vfs;           /* num VFs requested for this VF */
+	u16 num_vf_qps;            /* num queue pairs per VF */
 #ifdef I40E_FCOE
-	u16 num_fcoe_qps;          /* num fcoe queues this pf has set up */
+	u16 num_fcoe_qps;          /* num fcoe queues this PF has set up */
 	u16 num_fcoe_msix;         /* num queue vectors per fcoe pool */
 #endif /* I40E_FCOE */
-	u16 num_lan_qps;           /* num lan queues this pf has set up */
-	u16 num_lan_msix;          /* num queue vectors for the base pf vsi */
+	u16 num_lan_qps;           /* num lan queues this PF has set up */
+	u16 num_lan_msix;          /* num queue vectors for the base PF vsi */
 	int queues_left;           /* queues left unclaimed */
 	u16 rss_size;              /* num queues in the RSS array */
 	u16 rss_size_max;          /* HW defined max RSS queues */
@@ -269,7 +279,7 @@ struct i40e_pf {
 	enum i40e_interrupt_policy int_policy;
 	u16 rx_itr_default;
 	u16 tx_itr_default;
-	u16 msg_enable;
+	u32 msg_enable;
 	char int_name[I40E_INT_NAME_STR_LEN];
 	u16 adminq_work_limit; /* num of admin receive queue desc to process */
 	unsigned long service_timer_period;
@@ -383,6 +393,9 @@ struct i40e_pf {
 	bool ptp_tx;
 	bool ptp_rx;
 	u16 rss_table_size;
+	/* These are only valid in NPAR modes */
+	u32 npar_max_bw;
+	u32 npar_min_bw;
 };
 
 struct i40e_mac_filter {
@@ -405,6 +418,7 @@ struct i40e_veb {
 	u16 uplink_seid;
 	u16 stats_idx;           /* index of VEB parent */
 	u8  enabled_tc;
+	u16 bridge_mode;	/* Bridge Mode (VEB/VEPA) */
 	u16 flags;
 	u16 bw_limit;
 	u8  bw_max_quanta;
@@ -461,6 +475,9 @@ struct i40e_vsi {
 	u16 rx_itr_setting;
 	u16 tx_itr_setting;
 
+	u16 rss_table_size;
+	u16 rss_size;
+
 	u16 max_frame;
 	u16 rx_hdr_len;
 	u16 rx_buf_len;
@@ -478,6 +495,7 @@ struct i40e_vsi {
 
 	u16 base_queue;      /* vsi's first queue in hw array */
 	u16 alloc_queue_pairs; /* Allocated Tx/Rx queues */
+	u16 req_queue_pairs; /* User requested queue pairs */
 	u16 num_queue_pairs; /* Used tx and rx pairs */
 	u16 num_desc;
 	enum i40e_vsi_type type;  /* VSI type, e.g., LAN, FCoE, etc */
@@ -504,6 +522,9 @@ struct i40e_vsi {
 
 	/* VSI specific handlers */
 	irqreturn_t (*irq_handler)(int irq, void *data);
+
+	/* current rxnfc data */
+	struct ethtool_rxnfc rxnfc; /* current rss hash opts */
 } ____cacheline_internodealigned_in_smp;
 
 struct i40e_netdev_priv {
@@ -544,14 +565,14 @@ static inline char *i40e_fw_version_str(struct i40e_hw *hw)
 	static char buf[32];
 
 	snprintf(buf, sizeof(buf),
-		 "f%d.%d a%d.%d n%02x.%02x e%08x",
-		 hw->aq.fw_maj_ver, hw->aq.fw_min_ver,
+		 "f%d.%d.%05d a%d.%d n%x.%02x e%x",
+		 hw->aq.fw_maj_ver, hw->aq.fw_min_ver, hw->aq.fw_build,
 		 hw->aq.api_maj_ver, hw->aq.api_min_ver,
 		 (hw->nvm.version & I40E_NVM_VERSION_HI_MASK) >>
 			I40E_NVM_VERSION_HI_SHIFT,
 		 (hw->nvm.version & I40E_NVM_VERSION_LO_MASK) >>
 			I40E_NVM_VERSION_LO_SHIFT,
-		 hw->nvm.eetrack);
+		 (hw->nvm.eetrack & 0xffffff));
 
 	return buf;
 }
@@ -593,7 +614,7 @@ static inline bool i40e_rx_is_programming_status(u64 qw)
 
 /**
  * i40e_get_fd_cnt_all - get the total FD filter space available
- * @pf: pointer to the pf struct
+ * @pf: pointer to the PF struct
  **/
 static inline int i40e_get_fd_cnt_all(struct i40e_pf *pf)
 {
@@ -607,6 +628,7 @@ extern const char i40e_driver_name[];
 extern const char i40e_driver_version_str[];
 void i40e_do_reset_safe(struct i40e_pf *pf, u32 reset_flags);
 void i40e_do_reset(struct i40e_pf *pf, u32 reset_flags);
+struct i40e_vsi *i40e_find_vsi_from_id(struct i40e_pf *pf, u16 id);
 void i40e_update_stats(struct i40e_vsi *vsi);
 void i40e_update_eth_stats(struct i40e_vsi *vsi);
 struct rtnl_link_stats64 *i40e_get_vsi_stats_struct(struct i40e_vsi *vsi);
@@ -618,9 +640,10 @@ int i40e_program_fdir_filter(struct i40e_fdir_filter *fdir_data, u8 *raw_packet,
 int i40e_add_del_fdir(struct i40e_vsi *vsi,
 		      struct i40e_fdir_filter *input, bool add);
 void i40e_fdir_check_and_reenable(struct i40e_pf *pf);
-int i40e_get_current_fd_count(struct i40e_pf *pf);
-int i40e_get_cur_guaranteed_fd_count(struct i40e_pf *pf);
-int i40e_get_current_atr_cnt(struct i40e_pf *pf);
+u32 i40e_get_current_fd_count(struct i40e_pf *pf);
+u32 i40e_get_cur_guaranteed_fd_count(struct i40e_pf *pf);
+u32 i40e_get_current_atr_cnt(struct i40e_pf *pf);
+u32 i40e_get_global_fd_count(struct i40e_pf *pf);
 bool i40e_set_ntuple(struct i40e_pf *pf, netdev_features_t features);
 void i40e_set_ethtool_ops(struct net_device *netdev);
 struct i40e_mac_filter *i40e_add_filter(struct i40e_vsi *vsi,
@@ -680,6 +703,7 @@ int i40e_vlan_rx_add_vid(struct net_device *netdev,
 int i40e_vlan_rx_kill_vid(struct net_device *netdev,
 			  __always_unused __be16 proto, u16 vid);
 #endif
+int i40e_open(struct net_device *netdev);
 int i40e_vsi_open(struct i40e_vsi *vsi);
 void i40e_vlan_stripping_disable(struct i40e_vsi *vsi);
 int i40e_vsi_add_vlan(struct i40e_vsi *vsi, s16 vid);
@@ -690,7 +714,6 @@ bool i40e_is_vsi_in_vlan(struct i40e_vsi *vsi);
 struct i40e_mac_filter *i40e_find_mac(struct i40e_vsi *vsi, u8 *macaddr,
 				      bool is_vf, bool is_netdev);
 #ifdef I40E_FCOE
-int i40e_open(struct net_device *netdev);
 int i40e_close(struct net_device *netdev);
 int i40e_setup_tc(struct net_device *netdev, u8 tc);
 void i40e_netpoll(struct net_device *netdev);
@@ -712,6 +735,7 @@ void i40e_fcoe_handle_status(struct i40e_ring *rx_ring,
 void i40e_vlan_stripping_enable(struct i40e_vsi *vsi);
 #ifdef CONFIG_I40E_DCB
 void i40e_dcbnl_flush_apps(struct i40e_pf *pf,
+			   struct i40e_dcbx_config *old_cfg,
 			   struct i40e_dcbx_config *new_cfg);
 void i40e_dcbnl_set_all(struct i40e_vsi *vsi);
 void i40e_dcbnl_setup(struct i40e_vsi *vsi);
@@ -727,4 +751,8 @@ int i40e_ptp_set_ts_config(struct i40e_pf *pf, struct ifreq *ifr);
 int i40e_ptp_get_ts_config(struct i40e_pf *pf, struct ifreq *ifr);
 void i40e_ptp_init(struct i40e_pf *pf);
 void i40e_ptp_stop(struct i40e_pf *pf);
+int i40e_is_vsi_uplink_mode_veb(struct i40e_vsi *vsi);
+i40e_status i40e_get_npar_bw_setting(struct i40e_pf *pf);
+i40e_status i40e_set_npar_bw_setting(struct i40e_pf *pf);
+i40e_status i40e_commit_npar_bw_setting(struct i40e_pf *pf);
 #endif /* _I40E_H_ */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_adminq.c b/drivers/net/ethernet/intel/i40e/i40e_adminq.c
index 77f6254a89ac..3e0d20037675 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_adminq.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_adminq.c
@@ -592,6 +592,7 @@ i40e_status i40e_init_adminq(struct i40e_hw *hw)
 		ret_code = i40e_aq_get_firmware_version(hw,
 							&hw->aq.fw_maj_ver,
 							&hw->aq.fw_min_ver,
+							&hw->aq.fw_build,
 							&hw->aq.api_maj_ver,
 							&hw->aq.api_min_ver,
 							NULL);
@@ -605,7 +606,8 @@ i40e_status i40e_init_adminq(struct i40e_hw *hw)
 		goto init_adminq_free_arq;
 
 	/* get the NVM version info */
-	i40e_read_nvm_word(hw, I40E_SR_NVM_IMAGE_VERSION, &hw->nvm.version);
+	i40e_read_nvm_word(hw, I40E_SR_NVM_DEV_STARTER_VERSION,
+			   &hw->nvm.version);
 	i40e_read_nvm_word(hw, I40E_SR_NVM_EETRACK_LO, &eetrack_lo);
 	i40e_read_nvm_word(hw, I40E_SR_NVM_EETRACK_HI, &eetrack_hi);
 	hw->nvm.eetrack = (eetrack_hi << 16) | eetrack_lo;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_adminq.h b/drivers/net/ethernet/intel/i40e/i40e_adminq.h
index de17b6fbcc4e..28e519a50de4 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_adminq.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_adminq.h
@@ -93,6 +93,7 @@ struct i40e_adminq_info {
 	u16 asq_buf_size;               /* send queue buffer size */
 	u16 fw_maj_ver;                 /* firmware major version */
 	u16 fw_min_ver;                 /* firmware minor version */
+	u32 fw_build;                   /* firmware build number */
 	u16 api_maj_ver;                /* api major version */
 	u16 api_min_ver;                /* api minor version */
 	bool nvm_release_on_done;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_common.c b/drivers/net/ethernet/intel/i40e/i40e_common.c
index 6aea65dae5ed..d596f6624025 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_common.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -51,6 +51,7 @@ static i40e_status i40e_set_mac_type(struct i40e_hw *hw)
 		case I40E_DEV_ID_QSFP_B:
 		case I40E_DEV_ID_QSFP_C:
 		case I40E_DEV_ID_10G_BASE_T:
+		case I40E_DEV_ID_20G_KR2:
 			hw->mac.type = I40E_MAC_XL710;
 			break;
 		case I40E_DEV_ID_VF:
@@ -85,46 +86,53 @@ void i40e_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
 {
 	struct i40e_aq_desc *aq_desc = (struct i40e_aq_desc *)desc;
 	u16 len = le16_to_cpu(aq_desc->datalen);
-	u8 *aq_buffer = (u8 *)buffer;
-	u32 data[4];
-	u32 i = 0;
+	u8 *buf = (u8 *)buffer;
+	u16 i = 0;
 
 	if ((!(mask & hw->debug_mask)) || (desc == NULL))
 		return;
 
 	i40e_debug(hw, mask,
 		   "AQ CMD: opcode 0x%04X, flags 0x%04X, datalen 0x%04X, retval 0x%04X\n",
-		   aq_desc->opcode, aq_desc->flags, aq_desc->datalen,
-		   aq_desc->retval);
+		   le16_to_cpu(aq_desc->opcode),
+		   le16_to_cpu(aq_desc->flags),
+		   le16_to_cpu(aq_desc->datalen),
+		   le16_to_cpu(aq_desc->retval));
 	i40e_debug(hw, mask, "\tcookie (h,l) 0x%08X 0x%08X\n",
-		   aq_desc->cookie_high, aq_desc->cookie_low);
+		   le32_to_cpu(aq_desc->cookie_high),
+		   le32_to_cpu(aq_desc->cookie_low));
 	i40e_debug(hw, mask, "\tparam (0,1)  0x%08X 0x%08X\n",
-		   aq_desc->params.internal.param0,
-		   aq_desc->params.internal.param1);
+		   le32_to_cpu(aq_desc->params.internal.param0),
+		   le32_to_cpu(aq_desc->params.internal.param1));
 	i40e_debug(hw, mask, "\taddr (h,l)   0x%08X 0x%08X\n",
-		   aq_desc->params.external.addr_high,
-		   aq_desc->params.external.addr_low);
+		   le32_to_cpu(aq_desc->params.external.addr_high),
+		   le32_to_cpu(aq_desc->params.external.addr_low));
 
 	if ((buffer != NULL) && (aq_desc->datalen != 0)) {
-		memset(data, 0, sizeof(data));
 		i40e_debug(hw, mask, "AQ CMD Buffer:\n");
 		if (buf_len < len)
 			len = buf_len;
-		for (i = 0; i < len; i++) {
-			data[((i % 16) / 4)] |=
-				((u32)aq_buffer[i]) << (8 * (i % 4));
-			if ((i % 16) == 15) {
-				i40e_debug(hw, mask,
-					   "\t0x%04X  %08X %08X %08X %08X\n",
-					   i - 15, data[0], data[1], data[2],
-					   data[3]);
-				memset(data, 0, sizeof(data));
-			}
+		/* write the full 16-byte chunks */
+		for (i = 0; i < (len - 16); i += 16)
+			i40e_debug(hw, mask,
+				   "\t0x%04X  %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X\n",
+				   i, buf[i], buf[i + 1], buf[i + 2],
+				   buf[i + 3], buf[i + 4], buf[i + 5],
+				   buf[i + 6], buf[i + 7], buf[i + 8],
+				   buf[i + 9], buf[i + 10], buf[i + 11],
+				   buf[i + 12], buf[i + 13], buf[i + 14],
+				   buf[i + 15]);
+		/* write whatever's left over without overrunning the buffer */
+		if (i < len) {
+			char d_buf[80];
+			int j = 0;
+
+			memset(d_buf, 0, sizeof(d_buf));
+			j += sprintf(d_buf, "\t0x%04X ", i);
+			while (i < len)
+				j += sprintf(&d_buf[j], " %02X", buf[i++]);
+			i40e_debug(hw, mask, "%s\n", d_buf);
 		}
-		if ((i % 16) != 0)
-			i40e_debug(hw, mask, "\t0x%04X  %08X %08X %08X %08X\n",
-				   i - (i % 16), data[0], data[1], data[2],
-				   data[3]);
 	}
 }
 
@@ -534,7 +542,6 @@ struct i40e_rx_ptype_decoded i40e_ptype_lookup[] = {
 	I40E_PTT_UNUSED_ENTRY(255)
 };
 
-
 /**
  * i40e_init_shared_code - Initialize the shared code
  * @hw: pointer to hardware structure
@@ -685,7 +692,7 @@ i40e_status i40e_get_port_mac_addr(struct i40e_hw *hw, u8 *mac_addr)
 /**
  * i40e_pre_tx_queue_cfg - pre tx queue configure
  * @hw: pointer to the HW structure
- * @queue: target pf queue index
+ * @queue: target PF queue index
  * @enable: state change request
  *
  * Handles hw requirement to indicate intention to enable
@@ -827,12 +834,15 @@ static enum i40e_media_type i40e_get_media_type(struct i40e_hw *hw)
 	case I40E_PHY_TYPE_10GBASE_CR1:
 	case I40E_PHY_TYPE_40GBASE_CR4:
 	case I40E_PHY_TYPE_10GBASE_SFPP_CU:
+	case I40E_PHY_TYPE_40GBASE_AOC:
+	case I40E_PHY_TYPE_10GBASE_AOC:
 		media = I40E_MEDIA_TYPE_DA;
 		break;
 	case I40E_PHY_TYPE_1000BASE_KX:
 	case I40E_PHY_TYPE_10GBASE_KX4:
 	case I40E_PHY_TYPE_10GBASE_KR:
 	case I40E_PHY_TYPE_40GBASE_KR4:
+	case I40E_PHY_TYPE_20GBASE_KR2:
 		media = I40E_MEDIA_TYPE_BACKPLANE;
 		break;
 	case I40E_PHY_TYPE_SGMII:
@@ -849,7 +859,7 @@ static enum i40e_media_type i40e_get_media_type(struct i40e_hw *hw)
 }
 
 #define I40E_PF_RESET_WAIT_COUNT_A0	200
-#define I40E_PF_RESET_WAIT_COUNT	110
+#define I40E_PF_RESET_WAIT_COUNT	200
 /**
  * i40e_pf_reset - Reset the PF
  * @hw: pointer to the hardware structure
@@ -947,7 +957,7 @@ void i40e_clear_hw(struct i40e_hw *hw)
 	u32 val;
 	u32 eol = 0x7ff;
 
-	/* get number of interrupts, queues, and vfs */
+	/* get number of interrupts, queues, and VFs */
 	val = rd32(hw, I40E_GLPCI_CNF2);
 	num_pf_int = (val & I40E_GLPCI_CNF2_MSI_X_PF_N_MASK) >>
 		     I40E_GLPCI_CNF2_MSI_X_PF_N_SHIFT;
@@ -1076,8 +1086,11 @@ static u32 i40e_led_is_mine(struct i40e_hw *hw, int idx)
 	return gpio_val;
 }
 
-#define I40E_LED0 22
+#define I40E_COMBINED_ACTIVITY 0xA
+#define I40E_FILTER_ACTIVITY 0xE
 #define I40E_LINK_ACTIVITY 0xC
+#define I40E_MAC_ACTIVITY 0xD
+#define I40E_LED0 22
 
 /**
  * i40e_led_get - return current on/off mode
@@ -1090,6 +1103,7 @@ static u32 i40e_led_is_mine(struct i40e_hw *hw, int idx)
  **/
 u32 i40e_led_get(struct i40e_hw *hw)
 {
+	u32 current_mode = 0;
 	u32 mode = 0;
 	int i;
 
@@ -1102,6 +1116,20 @@ u32 i40e_led_get(struct i40e_hw *hw)
 		if (!gpio_val)
 			continue;
 
+		/* ignore gpio LED src mode entries related to the activity
+		 * LEDs
+		 */
+		current_mode = ((gpio_val & I40E_GLGEN_GPIO_CTL_LED_MODE_MASK)
+				>> I40E_GLGEN_GPIO_CTL_LED_MODE_SHIFT);
+		switch (current_mode) {
+		case I40E_COMBINED_ACTIVITY:
+		case I40E_FILTER_ACTIVITY:
+		case I40E_MAC_ACTIVITY:
+			continue;
+		default:
+			break;
+		}
+
 		mode = (gpio_val & I40E_GLGEN_GPIO_CTL_LED_MODE_MASK) >>
 			I40E_GLGEN_GPIO_CTL_LED_MODE_SHIFT;
 		break;
@@ -1121,6 +1149,7 @@ u32 i40e_led_get(struct i40e_hw *hw)
  **/
 void i40e_led_set(struct i40e_hw *hw, u32 mode, bool blink)
 {
+	u32 current_mode = 0;
 	int i;
 
 	if (mode & 0xfffffff0)
@@ -1135,6 +1164,20 @@ void i40e_led_set(struct i40e_hw *hw, u32 mode, bool blink)
 		if (!gpio_val)
 			continue;
 
+		/* ignore gpio LED src mode entries related to the activity
+		 * LEDs
+		 */
+		current_mode = ((gpio_val & I40E_GLGEN_GPIO_CTL_LED_MODE_MASK)
+				>> I40E_GLGEN_GPIO_CTL_LED_MODE_SHIFT);
+		switch (current_mode) {
+		case I40E_COMBINED_ACTIVITY:
+		case I40E_FILTER_ACTIVITY:
+		case I40E_MAC_ACTIVITY:
+			continue;
+		default:
+			break;
+		}
+
 		gpio_val &= ~I40E_GLGEN_GPIO_CTL_LED_MODE_MASK;
 		/* this & is a bit of paranoia, but serves as a range check */
 		gpio_val |= ((mode << I40E_GLGEN_GPIO_CTL_LED_MODE_SHIFT) &
@@ -1298,14 +1341,14 @@ enum i40e_status_code i40e_set_fc(struct i40e_hw *hw, u8 *aq_failures,
 			*aq_failures |= I40E_SET_FC_AQ_FAIL_SET;
 	}
 	/* Update the link info */
-	status = i40e_update_link_info(hw, true);
+	status = i40e_aq_get_link_info(hw, true, NULL, NULL);
 	if (status) {
 		/* Wait a little bit (on 40G cards it sometimes takes a really
 		 * long time for link to come back from the atomic reset)
 		 * and try once more
 		 */
 		msleep(1000);
-		status = i40e_update_link_info(hw, true);
+		status = i40e_aq_get_link_info(hw, true, NULL, NULL);
 	}
 	if (status)
 		*aq_failures |= I40E_SET_FC_AQ_FAIL_UPDATE;
@@ -1441,6 +1484,10 @@ i40e_status i40e_aq_get_link_info(struct i40e_hw *hw,
 	else
 		hw_link_info->lse_enable = false;
 
+	if ((hw->aq.fw_maj_ver < 4 || (hw->aq.fw_maj_ver == 4 &&
+	     hw->aq.fw_min_ver < 40)) && hw_link_info->phy_type == 0xE)
+		hw_link_info->phy_type = I40E_PHY_TYPE_10GBASE_SFPP_CU;
+
 	/* save link status information */
 	if (link)
 		*link = *hw_link_info;
@@ -1453,35 +1500,6 @@ aq_get_link_info_exit:
 }
 
 /**
- * i40e_update_link_info
- * @hw: pointer to the hw struct
- * @enable_lse: enable/disable LinkStatusEvent reporting
- *
- * Returns the link status of the adapter
- **/
-i40e_status i40e_update_link_info(struct i40e_hw *hw, bool enable_lse)
-{
-	struct i40e_aq_get_phy_abilities_resp abilities;
-	i40e_status status;
-
-	status = i40e_aq_get_link_info(hw, enable_lse, NULL, NULL);
-	if (status)
-		return status;
-
-	status = i40e_aq_get_phy_capabilities(hw, false, false,
-					      &abilities, NULL);
-	if (status)
-		return status;
-
-	if (abilities.abilities & I40E_AQ_PHY_AN_ENABLED)
-		hw->phy.link_info.an_enabled = true;
-	else
-		hw->phy.link_info.an_enabled = false;
-
-	return status;
-}
-
-/**
  * i40e_aq_set_phy_int_mask
  * @hw: pointer to the hw struct
  * @mask: interrupt mask to be set
@@ -1760,6 +1778,7 @@ i40e_status i40e_aq_get_switch_config(struct i40e_hw *hw,
  * @hw: pointer to the hw struct
  * @fw_major_version: firmware major version
  * @fw_minor_version: firmware minor version
+ * @fw_build: firmware build number
  * @api_major_version: major queue version
  * @api_minor_version: minor queue version
  * @cmd_details: pointer to command details structure or NULL
@@ -1768,6 +1787,7 @@ i40e_status i40e_aq_get_switch_config(struct i40e_hw *hw,
  **/
 i40e_status i40e_aq_get_firmware_version(struct i40e_hw *hw,
 				u16 *fw_major_version, u16 *fw_minor_version,
+				u32 *fw_build,
 				u16 *api_major_version, u16 *api_minor_version,
 				struct i40e_asq_cmd_details *cmd_details)
 {
@@ -1781,13 +1801,15 @@ i40e_status i40e_aq_get_firmware_version(struct i40e_hw *hw,
 	status = i40e_asq_send_command(hw, &desc, NULL, 0, cmd_details);
 
 	if (!status) {
-		if (fw_major_version != NULL)
+		if (fw_major_version)
 			*fw_major_version = le16_to_cpu(resp->fw_major);
-		if (fw_minor_version != NULL)
+		if (fw_minor_version)
 			*fw_minor_version = le16_to_cpu(resp->fw_minor);
-		if (api_major_version != NULL)
+		if (fw_build)
+			*fw_build = le32_to_cpu(resp->fw_build);
+		if (api_major_version)
 			*api_major_version = le16_to_cpu(resp->api_major);
-		if (api_minor_version != NULL)
+		if (api_minor_version)
 			*api_minor_version = le16_to_cpu(resp->api_minor);
 	}
 
@@ -1817,7 +1839,7 @@ i40e_status i40e_aq_send_driver_version(struct i40e_hw *hw,
 
 	i40e_fill_default_direct_cmd_desc(&desc, i40e_aqc_opc_driver_version);
 
-	desc.flags |= cpu_to_le16(I40E_AQ_FLAG_SI);
+	desc.flags |= cpu_to_le16(I40E_AQ_FLAG_BUF | I40E_AQ_FLAG_RD);
 	cmd->driver_major_ver = dv->major_version;
 	cmd->driver_minor_ver = dv->minor_version;
 	cmd->driver_build_ver = dv->build_version;
@@ -1997,7 +2019,7 @@ i40e_status i40e_aq_add_macvlan(struct i40e_hw *hw, u16 seid,
 	if (count == 0 || !mv_list || !hw)
 		return I40E_ERR_PARAM;
 
-	buf_size = count * sizeof(struct i40e_aqc_add_macvlan_element_data);
+	buf_size = count * sizeof(*mv_list);
 
 	/* prep the rest of the request */
 	i40e_fill_default_direct_cmd_desc(&desc, i40e_aqc_opc_add_macvlan);
@@ -2039,7 +2061,7 @@ i40e_status i40e_aq_remove_macvlan(struct i40e_hw *hw, u16 seid,
 	if (count == 0 || !mv_list || !hw)
 		return I40E_ERR_PARAM;
 
-	buf_size = count * sizeof(struct i40e_aqc_remove_macvlan_element_data);
+	buf_size = count * sizeof(*mv_list);
 
 	/* prep the rest of the request */
 	i40e_fill_default_direct_cmd_desc(&desc, i40e_aqc_opc_remove_macvlan);
@@ -2061,7 +2083,7 @@ i40e_status i40e_aq_remove_macvlan(struct i40e_hw *hw, u16 seid,
 /**
  * i40e_aq_send_msg_to_vf
  * @hw: pointer to the hardware structure
- * @vfid: vf id to send msg
+ * @vfid: VF id to send msg
  * @v_opcode: opcodes for VF-PF communication
  * @v_retval: return error code
  * @msg: pointer to the msg buffer
@@ -2106,7 +2128,7 @@ i40e_status i40e_aq_send_msg_to_vf(struct i40e_hw *hw, u16 vfid,
  * Read the register using the admin queue commands
  **/
 i40e_status i40e_aq_debug_read_register(struct i40e_hw *hw,
-				u32  reg_addr, u64 *reg_val,
+				u32 reg_addr, u64 *reg_val,
 				struct i40e_asq_cmd_details *cmd_details)
 {
 	struct i40e_aq_desc desc;
@@ -2117,17 +2139,15 @@ i40e_status i40e_aq_debug_read_register(struct i40e_hw *hw,
 	if (reg_val == NULL)
 		return I40E_ERR_PARAM;
 
-	i40e_fill_default_direct_cmd_desc(&desc,
-					  i40e_aqc_opc_debug_read_reg);
+	i40e_fill_default_direct_cmd_desc(&desc, i40e_aqc_opc_debug_read_reg);
 
 	cmd_resp->address = cpu_to_le32(reg_addr);
 
 	status = i40e_asq_send_command(hw, &desc, NULL, 0, cmd_details);
 
 	if (!status) {
-		*reg_val = ((u64)cmd_resp->value_high << 32) |
-			    (u64)cmd_resp->value_low;
-		*reg_val = le64_to_cpu(*reg_val);
+		*reg_val = ((u64)le32_to_cpu(cmd_resp->value_high) << 32) |
+			   (u64)le32_to_cpu(cmd_resp->value_low);
 	}
 
 	return status;
@@ -3377,6 +3397,47 @@ i40e_status i40e_aq_add_rem_control_packet_filter(struct i40e_hw *hw,
 }
 
 /**
+ * i40e_aq_alternate_read
+ * @hw: pointer to the hardware structure
+ * @reg_addr0: address of first dword to be read
+ * @reg_val0: pointer for data read from 'reg_addr0'
+ * @reg_addr1: address of second dword to be read
+ * @reg_val1: pointer for data read from 'reg_addr1'
+ *
+ * Read one or two dwords from alternate structure. Fields are indicated
+ * by 'reg_addr0' and 'reg_addr1' register numbers. If 'reg_val1' pointer
+ * is not passed then only register at 'reg_addr0' is read.
+ *
+ **/
+static i40e_status i40e_aq_alternate_read(struct i40e_hw *hw,
+					  u32 reg_addr0, u32 *reg_val0,
+					  u32 reg_addr1, u32 *reg_val1)
+{
+	struct i40e_aq_desc desc;
+	struct i40e_aqc_alternate_write *cmd_resp =
+		(struct i40e_aqc_alternate_write *)&desc.params.raw;
+	i40e_status status;
+
+	if (!reg_val0)
+		return I40E_ERR_PARAM;
+
+	i40e_fill_default_direct_cmd_desc(&desc, i40e_aqc_opc_alternate_read);
+	cmd_resp->address0 = cpu_to_le32(reg_addr0);
+	cmd_resp->address1 = cpu_to_le32(reg_addr1);
+
+	status = i40e_asq_send_command(hw, &desc, NULL, 0, NULL);
+
+	if (!status) {
+		*reg_val0 = le32_to_cpu(cmd_resp->data0);
+
+		if (reg_val1)
+			*reg_val1 = le32_to_cpu(cmd_resp->data1);
+	}
+
+	return status;
+}
+
+/**
  * i40e_aq_resume_port_tx
  * @hw: pointer to the hardware structure
  * @cmd_details: pointer to command details structure or NULL
@@ -3440,3 +3501,79 @@ void i40e_set_pci_config_data(struct i40e_hw *hw, u16 link_status)
 		break;
 	}
 }
+
+/**
+ * i40e_read_bw_from_alt_ram
+ * @hw: pointer to the hardware structure
+ * @max_bw: pointer for max_bw read
+ * @min_bw: pointer for min_bw read
+ * @min_valid: pointer for bool that is true if min_bw is a valid value
+ * @max_valid: pointer for bool that is true if max_bw is a valid value
+ *
+ * Read bw from the alternate ram for the given pf
+ **/
+i40e_status i40e_read_bw_from_alt_ram(struct i40e_hw *hw,
+				      u32 *max_bw, u32 *min_bw,
+				      bool *min_valid, bool *max_valid)
+{
+	i40e_status status;
+	u32 max_bw_addr, min_bw_addr;
+
+	/* Calculate the address of the min/max bw registers */
+	max_bw_addr = I40E_ALT_STRUCT_FIRST_PF_OFFSET +
+		      I40E_ALT_STRUCT_MAX_BW_OFFSET +
+		      (I40E_ALT_STRUCT_DWORDS_PER_PF * hw->pf_id);
+	min_bw_addr = I40E_ALT_STRUCT_FIRST_PF_OFFSET +
+		      I40E_ALT_STRUCT_MIN_BW_OFFSET +
+		      (I40E_ALT_STRUCT_DWORDS_PER_PF * hw->pf_id);
+
+	/* Read the bandwidths from alt ram */
+	status = i40e_aq_alternate_read(hw, max_bw_addr, max_bw,
+					min_bw_addr, min_bw);
+
+	if (*min_bw & I40E_ALT_BW_VALID_MASK)
+		*min_valid = true;
+	else
+		*min_valid = false;
+
+	if (*max_bw & I40E_ALT_BW_VALID_MASK)
+		*max_valid = true;
+	else
+		*max_valid = false;
+
+	return status;
+}
+
+/**
+ * i40e_aq_configure_partition_bw
+ * @hw: pointer to the hardware structure
+ * @bw_data: Buffer holding valid pfs and bw limits
+ * @cmd_details: pointer to command details
+ *
+ * Configure partitions guaranteed/max bw
+ **/
+i40e_status i40e_aq_configure_partition_bw(struct i40e_hw *hw,
+			struct i40e_aqc_configure_partition_bw_data *bw_data,
+			struct i40e_asq_cmd_details *cmd_details)
+{
+	i40e_status status;
+	struct i40e_aq_desc desc;
+	u16 bwd_size = sizeof(*bw_data);
+
+	i40e_fill_default_direct_cmd_desc(&desc,
+					  i40e_aqc_opc_configure_partition_bw);
+
+	/* Indirect command */
+	desc.flags |= cpu_to_le16((u16)I40E_AQ_FLAG_BUF);
+	desc.flags |= cpu_to_le16((u16)I40E_AQ_FLAG_RD);
+
+	if (bwd_size > I40E_AQ_LARGE_BUF)
+		desc.flags |= cpu_to_le16((u16)I40E_AQ_FLAG_LB);
+
+	desc.datalen = cpu_to_le16(bwd_size);
+
+	status = i40e_asq_send_command(hw, &desc, bw_data, bwd_size,
+				       cmd_details);
+
+	return status;
+}
diff --git a/drivers/net/ethernet/intel/i40e/i40e_dcb.c b/drivers/net/ethernet/intel/i40e/i40e_dcb.c
index 3ce43588592d..6e1466756760 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_dcb.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_dcb.c
@@ -459,7 +459,7 @@ static void i40e_cee_to_dcb_v1_config(
 	sync = (status & I40E_TLV_STATUS_SYNC) ? 1 : 0;
 	oper = (status & I40E_TLV_STATUS_OPER) ? 1 : 0;
 	/* Add APPs if Error is False and Oper/Sync is True */
-	if (!err && sync && oper) {
+	if (!err) {
 		/* CEE operating configuration supports FCoE/iSCSI/FIP only */
 		dcbcfg->numapps = I40E_CEE_OPER_MAX_APPS;
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c b/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c
index a11c70ca5a28..bd5079d5c1b6 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_dcb_nl.c
@@ -178,6 +178,10 @@ void i40e_dcbnl_set_all(struct i40e_vsi *vsi)
 	if (!(pf->flags & I40E_FLAG_DCB_ENABLED))
 		return;
 
+	/* MFP mode but not an iSCSI PF so return */
+	if ((pf->flags & I40E_FLAG_MFP_ENABLED) && !(pf->hw.func_caps.iscsi))
+		return;
+
 	dcbxcfg = &hw->local_dcbx_config;
 
 	/* Set up all the App TLVs if DCBx is negotiated */
@@ -223,7 +227,7 @@ static int i40e_dcbnl_vsi_del_app(struct i40e_vsi *vsi,
 
 /**
  * i40e_dcbnl_del_app - Delete APP on all VSIs
- * @pf: the corresponding pf
+ * @pf: the corresponding PF
  * @app: APP to delete
  *
  * Delete given APP from all the VSIs for given PF
@@ -268,23 +272,26 @@ static bool i40e_dcbnl_find_app(struct i40e_dcbx_config *cfg,
 
 /**
  * i40e_dcbnl_flush_apps - Delete all removed APPs
- * @pf: the corresponding pf
+ * @pf: the corresponding PF
+ * @old_cfg: old DCBX configuration data
  * @new_cfg: new DCBX configuration data
  *
  * Find and delete all APPs that are not present in the passed
  * DCB configuration
  **/
 void i40e_dcbnl_flush_apps(struct i40e_pf *pf,
+			   struct i40e_dcbx_config *old_cfg,
 			   struct i40e_dcbx_config *new_cfg)
 {
 	struct i40e_dcb_app_priority_table app;
-	struct i40e_dcbx_config *dcbxcfg;
-	struct i40e_hw *hw = &pf->hw;
 	int i;
 
-	dcbxcfg = &hw->local_dcbx_config;
-	for (i = 0; i < dcbxcfg->numapps; i++) {
-		app = dcbxcfg->app[i];
+	/* MFP mode but not an iSCSI PF so return */
+	if ((pf->flags & I40E_FLAG_MFP_ENABLED) && !(pf->hw.func_caps.iscsi))
+		return;
+
+	for (i = 0; i < old_cfg->numapps; i++) {
+		app = old_cfg->app[i];
 		/* The APP is not available anymore delete it */
 		if (!i40e_dcbnl_find_app(new_cfg, &app))
 			i40e_dcbnl_del_app(pf, &app);
@@ -306,9 +313,7 @@ void i40e_dcbnl_setup(struct i40e_vsi *vsi)
 	if (!(pf->flags & I40E_FLAG_DCB_CAPABLE))
 		return;
 
-	/* Do not setup DCB NL ops for MFP mode */
-	if (!(pf->flags & I40E_FLAG_MFP_ENABLED))
-		dev->dcbnl_ops = &dcbnl_ops;
+	dev->dcbnl_ops = &dcbnl_ops;
 
 	/* Set initial IEEE DCB settings */
 	i40e_dcbnl_set_all(vsi);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
index c17ee77100d3..daa88263af66 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
@@ -35,7 +35,7 @@ static struct dentry *i40e_dbg_root;
 
 /**
  * i40e_dbg_find_vsi - searches for the vsi with the given seid
- * @pf - the pf structure to search for the vsi
+ * @pf - the PF structure to search for the vsi
  * @seid - seid of the vsi it is searching for
  **/
 static struct i40e_vsi *i40e_dbg_find_vsi(struct i40e_pf *pf, int seid)
@@ -54,7 +54,7 @@ static struct i40e_vsi *i40e_dbg_find_vsi(struct i40e_pf *pf, int seid)
 
 /**
  * i40e_dbg_find_veb - searches for the veb with the given seid
- * @pf - the pf structure to search for the veb
+ * @pf - the PF structure to search for the veb
  * @seid - seid of the veb it is searching for
  **/
 static struct i40e_veb *i40e_dbg_find_veb(struct i40e_pf *pf, int seid)
@@ -112,7 +112,7 @@ static ssize_t i40e_dbg_dump_read(struct file *filp, char __user *buffer,
 
 /**
  * i40e_dbg_prep_dump_buf
- * @pf: the pf we're working with
+ * @pf: the PF we're working with
  * @buflen: the desired buffer length
  *
  * Return positive if success, 0 if failed
@@ -318,7 +318,7 @@ static const struct file_operations i40e_dbg_dump_fops = {
  * setup, adding or removing filters, or other things.  Many of
  * these will be useful for some forms of unit testing.
  **************************************************************/
-static char i40e_dbg_command_buf[256] = "hello world";
+static char i40e_dbg_command_buf[256] = "";
 
 /**
  * i40e_dbg_command_read - read for command datum
@@ -390,6 +390,11 @@ static void i40e_dbg_dump_vsi_seid(struct i40e_pf *pf, int seid)
 		 "    netdev_registered = %i, current_netdev_flags = 0x%04x, state = %li flags = 0x%08lx\n",
 		 vsi->netdev_registered,
 		 vsi->current_netdev_flags, vsi->state, vsi->flags);
+	if (vsi == pf->vsi[pf->lan_vsi])
+		dev_info(&pf->pdev->dev, "MAC address: %pM SAN MAC: %pM Port MAC: %pM\n",
+			 pf->hw.mac.addr,
+			 pf->hw.mac.san_addr,
+			 pf->hw.mac.port_addr);
 	list_for_each_entry(f, &vsi->mac_filter_list, list) {
 		dev_info(&pf->pdev->dev,
 			 "    mac_filter_list: %pM vid=%d, is_netdev=%d is_vf=%d counter=%d\n",
@@ -675,7 +680,7 @@ static void i40e_dbg_dump_vsi_seid(struct i40e_pf *pf, int seid)
 		 vsi->info.resp_reserved[8], vsi->info.resp_reserved[9],
 		 vsi->info.resp_reserved[10], vsi->info.resp_reserved[11]);
 	if (vsi->back)
-		dev_info(&pf->pdev->dev, "    pf = %p\n", vsi->back);
+		dev_info(&pf->pdev->dev, "    PF = %p\n", vsi->back);
 	dev_info(&pf->pdev->dev, "    idx = %d\n", vsi->idx);
 	dev_info(&pf->pdev->dev,
 		 "    tc_config: numtc = %d, enabled_tc = 0x%x\n",
@@ -921,9 +926,10 @@ static void i40e_dbg_dump_veb_seid(struct i40e_pf *pf, int seid)
 		return;
 	}
 	dev_info(&pf->pdev->dev,
-		 "veb idx=%d,%d stats_ic=%d  seid=%d uplink=%d\n",
+		 "veb idx=%d,%d stats_ic=%d  seid=%d uplink=%d mode=%s\n",
 		 veb->idx, veb->veb_idx, veb->stats_idx, veb->seid,
-		 veb->uplink_seid);
+		 veb->uplink_seid,
+		 veb->bridge_mode == BRIDGE_MODE_VEPA ? "VEPA" : "VEB");
 	i40e_dbg_dump_eth_stats(pf, &veb->stats);
 }
 
@@ -945,7 +951,7 @@ static void i40e_dbg_dump_veb_all(struct i40e_pf *pf)
 
 /**
  * i40e_dbg_cmd_fd_ctrl - Enable/disable FD sideband/ATR
- * @pf: the pf that would be altered
+ * @pf: the PF that would be altered
  * @flag: flag that needs enabling or disabling
  * @enable: Enable/disable FD SD/ATR
  **/
@@ -957,7 +963,7 @@ static void i40e_dbg_cmd_fd_ctrl(struct i40e_pf *pf, u64 flag, bool enable)
 		pf->flags &= ~flag;
 		pf->auto_disable_flags |= flag;
 	}
-	dev_info(&pf->pdev->dev, "requesting a pf reset\n");
+	dev_info(&pf->pdev->dev, "requesting a PF reset\n");
 	i40e_do_reset_safe(pf, (1 << __I40E_PF_RESET_REQUESTED));
 }
 
@@ -1487,11 +1493,15 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 			} else {
 				dev_info(&pf->pdev->dev, "clear_stats vsi [seid]\n");
 			}
-		} else if (strncmp(&cmd_buf[12], "pf", 2) == 0) {
-			i40e_pf_reset_stats(pf);
-			dev_info(&pf->pdev->dev, "pf clear stats called\n");
+		} else if (strncmp(&cmd_buf[12], "port", 4) == 0) {
+			if (pf->hw.partition_id == 1) {
+				i40e_pf_reset_stats(pf);
+				dev_info(&pf->pdev->dev, "port stats cleared\n");
+			} else {
+				dev_info(&pf->pdev->dev, "clear port stats not allowed on this port partition\n");
+			}
 		} else {
-			dev_info(&pf->pdev->dev, "clear_stats vsi [seid] or clear_stats pf\n");
+			dev_info(&pf->pdev->dev, "clear_stats vsi [seid] or clear_stats port\n");
 		}
 	} else if (strncmp(cmd_buf, "send aq_cmd", 11) == 0) {
 		struct i40e_aq_desc *desc;
@@ -1897,7 +1907,7 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 		dev_info(&pf->pdev->dev, "  read <reg>\n");
 		dev_info(&pf->pdev->dev, "  write <reg> <value>\n");
 		dev_info(&pf->pdev->dev, "  clear_stats vsi [seid]\n");
-		dev_info(&pf->pdev->dev, "  clear_stats pf\n");
+		dev_info(&pf->pdev->dev, "  clear_stats port\n");
 		dev_info(&pf->pdev->dev, "  pfr\n");
 		dev_info(&pf->pdev->dev, "  corer\n");
 		dev_info(&pf->pdev->dev, "  globr\n");
@@ -1935,7 +1945,7 @@ static const struct file_operations i40e_dbg_command_fops = {
  * The netdev_ops entry in debugfs is for giving the driver commands
  * to be executed from the netdev operations.
  **************************************************************/
-static char i40e_dbg_netdev_ops_buf[256] = "hello world";
+static char i40e_dbg_netdev_ops_buf[256] = "";
 
 /**
  * i40e_dbg_netdev_ops - read for netdev_ops datum
@@ -2123,8 +2133,8 @@ static const struct file_operations i40e_dbg_netdev_ops_fops = {
 };
 
 /**
- * i40e_dbg_pf_init - setup the debugfs directory for the pf
- * @pf: the pf that is starting up
+ * i40e_dbg_pf_init - setup the debugfs directory for the PF
+ * @pf: the PF that is starting up
  **/
 void i40e_dbg_pf_init(struct i40e_pf *pf)
 {
@@ -2160,8 +2170,8 @@ create_failed:
 }
 
 /**
- * i40e_dbg_pf_exit - clear out the pf's debugfs entries
- * @pf: the pf that is stopping
+ * i40e_dbg_pf_exit - clear out the PF's debugfs entries
+ * @pf: the PF that is stopping
  **/
 void i40e_dbg_pf_exit(struct i40e_pf *pf)
 {
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
index b8230dc205ec..c848b1862512 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -113,7 +113,6 @@ static struct i40e_stats i40e_gstrings_stats[] = {
 	I40E_PF_STAT("tx_broadcast", stats.eth.tx_broadcast),
 	I40E_PF_STAT("tx_errors", stats.eth.tx_errors),
 	I40E_PF_STAT("rx_dropped", stats.eth.rx_discards),
-	I40E_PF_STAT("tx_dropped", stats.eth.tx_discards),
 	I40E_PF_STAT("tx_dropped_link_down", stats.tx_dropped_link_down),
 	I40E_PF_STAT("crc_errors", stats.crc_errors),
 	I40E_PF_STAT("illegal_bytes", stats.illegal_bytes),
@@ -218,6 +217,13 @@ static const char i40e_gstrings_test[][ETH_GSTRING_LEN] = {
 
 #define I40E_TEST_LEN (sizeof(i40e_gstrings_test) / ETH_GSTRING_LEN)
 
+static const char i40e_priv_flags_strings[][ETH_GSTRING_LEN] = {
+	"NPAR",
+};
+
+#define I40E_PRIV_FLAGS_STR_LEN \
+	(sizeof(i40e_priv_flags_strings) / ETH_GSTRING_LEN)
+
 /**
  * i40e_partition_setting_complaint - generic complaint for MFP restriction
  * @pf: the PF struct
@@ -229,73 +235,20 @@ static void i40e_partition_setting_complaint(struct i40e_pf *pf)
 }
 
 /**
- * i40e_get_settings - Get Link Speed and Duplex settings
+ * i40e_get_settings_link_up - Get the Link settings for when link is up
+ * @hw: hw structure
+ * @ecmd: ethtool command to fill in
  * @netdev: network interface device structure
- * @ecmd: ethtool command
  *
- * Reports speed/duplex settings based on media_type
  **/
-static int i40e_get_settings(struct net_device *netdev,
-			     struct ethtool_cmd *ecmd)
+static void i40e_get_settings_link_up(struct i40e_hw *hw,
+				      struct ethtool_cmd *ecmd,
+				      struct net_device *netdev)
 {
-	struct i40e_netdev_priv *np = netdev_priv(netdev);
-	struct i40e_pf *pf = np->vsi->back;
-	struct i40e_hw *hw = &pf->hw;
 	struct i40e_link_status *hw_link_info = &hw->phy.link_info;
-	bool link_up = hw_link_info->link_info & I40E_AQ_LINK_UP;
 	u32 link_speed = hw_link_info->link_speed;
 
-	/* hardware is either in 40G mode or 10G mode
-	 * NOTE: this section initializes supported and advertising
-	 */
-	if (!link_up) {
-		/* link is down and the driver needs to fall back on
-		 * device ID to determine what kinds of info to display,
-		 * it's mostly a guess that may change when link is up
-		 */
-		switch (hw->device_id) {
-		case I40E_DEV_ID_QSFP_A:
-		case I40E_DEV_ID_QSFP_B:
-		case I40E_DEV_ID_QSFP_C:
-			/* pluggable QSFP */
-			ecmd->supported = SUPPORTED_40000baseSR4_Full |
-					  SUPPORTED_40000baseCR4_Full |
-					  SUPPORTED_40000baseLR4_Full;
-			ecmd->advertising = ADVERTISED_40000baseSR4_Full |
-					    ADVERTISED_40000baseCR4_Full |
-					    ADVERTISED_40000baseLR4_Full;
-			break;
-		case I40E_DEV_ID_KX_B:
-			/* backplane 40G */
-			ecmd->supported = SUPPORTED_40000baseKR4_Full;
-			ecmd->advertising = ADVERTISED_40000baseKR4_Full;
-			break;
-		case I40E_DEV_ID_KX_C:
-			/* backplane 10G */
-			ecmd->supported = SUPPORTED_10000baseKR_Full;
-			ecmd->advertising = ADVERTISED_10000baseKR_Full;
-			break;
-		case I40E_DEV_ID_10G_BASE_T:
-			ecmd->supported = SUPPORTED_10000baseT_Full |
-					  SUPPORTED_1000baseT_Full |
-					  SUPPORTED_100baseT_Full;
-			ecmd->advertising = ADVERTISED_10000baseT_Full |
-					    ADVERTISED_1000baseT_Full |
-					    ADVERTISED_100baseT_Full;
-			break;
-		default:
-			/* all the rest are 10G/1G */
-			ecmd->supported = SUPPORTED_10000baseT_Full |
-					  SUPPORTED_1000baseT_Full;
-			ecmd->advertising = ADVERTISED_10000baseT_Full |
-					    ADVERTISED_1000baseT_Full;
-			break;
-		}
-
-		/* skip phy_type use as it is zero when link is down */
-		goto no_valid_phy_type;
-	}
-
+	/* Initialize supported and advertised settings based on phy settings */
 	switch (hw_link_info->phy_type) {
 	case I40E_PHY_TYPE_40GBASE_CR4:
 	case I40E_PHY_TYPE_40GBASE_CR4_CU:
@@ -304,6 +257,11 @@ static int i40e_get_settings(struct net_device *netdev,
 		ecmd->advertising = ADVERTISED_Autoneg |
 				    ADVERTISED_40000baseCR4_Full;
 		break;
+	case I40E_PHY_TYPE_XLAUI:
+	case I40E_PHY_TYPE_XLPPI:
+	case I40E_PHY_TYPE_40GBASE_AOC:
+		ecmd->supported = SUPPORTED_40000baseCR4_Full;
+		break;
 	case I40E_PHY_TYPE_40GBASE_KR4:
 		ecmd->supported = SUPPORTED_Autoneg |
 				  SUPPORTED_40000baseKR4_Full;
@@ -311,13 +269,17 @@ static int i40e_get_settings(struct net_device *netdev,
 				    ADVERTISED_40000baseKR4_Full;
 		break;
 	case I40E_PHY_TYPE_40GBASE_SR4:
-	case I40E_PHY_TYPE_XLPPI:
-	case I40E_PHY_TYPE_XLAUI:
 		ecmd->supported = SUPPORTED_40000baseSR4_Full;
 		break;
 	case I40E_PHY_TYPE_40GBASE_LR4:
 		ecmd->supported = SUPPORTED_40000baseLR4_Full;
 		break;
+	case I40E_PHY_TYPE_20GBASE_KR2:
+		ecmd->supported = SUPPORTED_Autoneg |
+				  SUPPORTED_20000baseKR2_Full;
+		ecmd->advertising = ADVERTISED_Autoneg |
+				    ADVERTISED_20000baseKR2_Full;
+		break;
 	case I40E_PHY_TYPE_10GBASE_KX4:
 		ecmd->supported = SUPPORTED_Autoneg |
 				  SUPPORTED_10000baseKX4_Full;
@@ -334,55 +296,56 @@ static int i40e_get_settings(struct net_device *netdev,
 	case I40E_PHY_TYPE_10GBASE_LR:
 	case I40E_PHY_TYPE_1000BASE_SX:
 	case I40E_PHY_TYPE_1000BASE_LX:
-		ecmd->supported = SUPPORTED_10000baseT_Full;
-		ecmd->supported |= SUPPORTED_1000baseT_Full;
+		ecmd->supported = SUPPORTED_10000baseT_Full |
+				  SUPPORTED_1000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_10GB)
+			ecmd->advertising |= ADVERTISED_10000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_1GB)
+			ecmd->advertising |= ADVERTISED_1000baseT_Full;
+		break;
+	case I40E_PHY_TYPE_1000BASE_KX:
+		ecmd->supported = SUPPORTED_Autoneg |
+				  SUPPORTED_1000baseKX_Full;
+		ecmd->advertising = ADVERTISED_Autoneg |
+				    ADVERTISED_1000baseKX_Full;
 		break;
-	case I40E_PHY_TYPE_10GBASE_CR1_CU:
-	case I40E_PHY_TYPE_10GBASE_CR1:
 	case I40E_PHY_TYPE_10GBASE_T:
+	case I40E_PHY_TYPE_1000BASE_T:
+	case I40E_PHY_TYPE_100BASE_TX:
 		ecmd->supported = SUPPORTED_Autoneg |
 				  SUPPORTED_10000baseT_Full |
 				  SUPPORTED_1000baseT_Full |
 				  SUPPORTED_100baseT_Full;
+		ecmd->advertising = ADVERTISED_Autoneg;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_10GB)
+			ecmd->advertising |= ADVERTISED_10000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_1GB)
+			ecmd->advertising |= ADVERTISED_1000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_100MB)
+			ecmd->advertising |= ADVERTISED_100baseT_Full;
+		break;
+	case I40E_PHY_TYPE_10GBASE_CR1_CU:
+	case I40E_PHY_TYPE_10GBASE_CR1:
+		ecmd->supported = SUPPORTED_Autoneg |
+				  SUPPORTED_10000baseT_Full;
 		ecmd->advertising = ADVERTISED_Autoneg |
-				    ADVERTISED_10000baseT_Full |
-				    ADVERTISED_1000baseT_Full |
-				    ADVERTISED_100baseT_Full;
+				    ADVERTISED_10000baseT_Full;
 		break;
 	case I40E_PHY_TYPE_XAUI:
 	case I40E_PHY_TYPE_XFI:
 	case I40E_PHY_TYPE_SFI:
 	case I40E_PHY_TYPE_10GBASE_SFPP_CU:
+	case I40E_PHY_TYPE_10GBASE_AOC:
 		ecmd->supported = SUPPORTED_10000baseT_Full;
 		break;
-	case I40E_PHY_TYPE_1000BASE_KX:
-	case I40E_PHY_TYPE_1000BASE_T:
-		ecmd->supported = SUPPORTED_Autoneg |
-				  SUPPORTED_10000baseT_Full |
-				  SUPPORTED_1000baseT_Full |
-				  SUPPORTED_100baseT_Full;
-		ecmd->advertising = ADVERTISED_Autoneg |
-				    ADVERTISED_10000baseT_Full |
-				    ADVERTISED_1000baseT_Full |
-				    ADVERTISED_100baseT_Full;
-		break;
-	case I40E_PHY_TYPE_100BASE_TX:
-		ecmd->supported = SUPPORTED_Autoneg |
-				  SUPPORTED_10000baseT_Full |
-				  SUPPORTED_1000baseT_Full |
-				  SUPPORTED_100baseT_Full;
-		ecmd->advertising = ADVERTISED_Autoneg |
-				    ADVERTISED_10000baseT_Full |
-				    ADVERTISED_1000baseT_Full |
-				    ADVERTISED_100baseT_Full;
-		break;
 	case I40E_PHY_TYPE_SGMII:
 		ecmd->supported = SUPPORTED_Autoneg |
 				  SUPPORTED_1000baseT_Full |
 				  SUPPORTED_100baseT_Full;
-		ecmd->advertising = ADVERTISED_Autoneg |
-				    ADVERTISED_1000baseT_Full |
-				    ADVERTISED_100baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_1GB)
+			ecmd->advertising |= ADVERTISED_1000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_100MB)
+			ecmd->advertising |= ADVERTISED_100baseT_Full;
 		break;
 	default:
 		/* if we got here and link is up something bad is afoot */
@@ -390,8 +353,126 @@ static int i40e_get_settings(struct net_device *netdev,
 			    hw_link_info->phy_type);
 	}
 
-no_valid_phy_type:
-	/* this is if autoneg is enabled or disabled */
+	/* Set speed and duplex */
+	switch (link_speed) {
+	case I40E_LINK_SPEED_40GB:
+		/* need a SPEED_40000 in ethtool.h */
+		ethtool_cmd_speed_set(ecmd, 40000);
+		break;
+	case I40E_LINK_SPEED_20GB:
+		ethtool_cmd_speed_set(ecmd, SPEED_20000);
+		break;
+	case I40E_LINK_SPEED_10GB:
+		ethtool_cmd_speed_set(ecmd, SPEED_10000);
+		break;
+	case I40E_LINK_SPEED_1GB:
+		ethtool_cmd_speed_set(ecmd, SPEED_1000);
+		break;
+	case I40E_LINK_SPEED_100MB:
+		ethtool_cmd_speed_set(ecmd, SPEED_100);
+		break;
+	default:
+		break;
+	}
+	ecmd->duplex = DUPLEX_FULL;
+}
+
+/**
+ * i40e_get_settings_link_down - Get the Link settings for when link is down
+ * @hw: hw structure
+ * @ecmd: ethtool command to fill in
+ *
+ * Reports link settings that can be determined when link is down
+ **/
+static void i40e_get_settings_link_down(struct i40e_hw *hw,
+					struct ethtool_cmd *ecmd)
+{
+	struct i40e_link_status *hw_link_info = &hw->phy.link_info;
+
+	/* link is down and the driver needs to fall back on
+	 * device ID to determine what kinds of info to display,
+	 * it's mostly a guess that may change when link is up
+	 */
+	switch (hw->device_id) {
+	case I40E_DEV_ID_QSFP_A:
+	case I40E_DEV_ID_QSFP_B:
+	case I40E_DEV_ID_QSFP_C:
+		/* pluggable QSFP */
+		ecmd->supported = SUPPORTED_40000baseSR4_Full |
+				  SUPPORTED_40000baseCR4_Full |
+				  SUPPORTED_40000baseLR4_Full;
+		ecmd->advertising = ADVERTISED_40000baseSR4_Full |
+				    ADVERTISED_40000baseCR4_Full |
+				    ADVERTISED_40000baseLR4_Full;
+		break;
+	case I40E_DEV_ID_KX_B:
+		/* backplane 40G */
+		ecmd->supported = SUPPORTED_40000baseKR4_Full;
+		ecmd->advertising = ADVERTISED_40000baseKR4_Full;
+		break;
+	case I40E_DEV_ID_KX_C:
+		/* backplane 10G */
+		ecmd->supported = SUPPORTED_10000baseKR_Full;
+		ecmd->advertising = ADVERTISED_10000baseKR_Full;
+		break;
+	case I40E_DEV_ID_10G_BASE_T:
+		ecmd->supported = SUPPORTED_10000baseT_Full |
+				  SUPPORTED_1000baseT_Full |
+				  SUPPORTED_100baseT_Full;
+		/* Figure out what has been requested */
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_10GB)
+			ecmd->advertising |= ADVERTISED_10000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_1GB)
+			ecmd->advertising |= ADVERTISED_1000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_100MB)
+			ecmd->advertising |= ADVERTISED_100baseT_Full;
+		break;
+	case I40E_DEV_ID_20G_KR2:
+		/* backplane 20G */
+		ecmd->supported = SUPPORTED_20000baseKR2_Full;
+		ecmd->advertising = ADVERTISED_20000baseKR2_Full;
+		break;
+	default:
+		/* all the rest are 10G/1G */
+		ecmd->supported = SUPPORTED_10000baseT_Full |
+				  SUPPORTED_1000baseT_Full;
+		/* Figure out what has been requested */
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_10GB)
+			ecmd->advertising |= ADVERTISED_10000baseT_Full;
+		if (hw_link_info->requested_speeds & I40E_LINK_SPEED_1GB)
+			ecmd->advertising |= ADVERTISED_1000baseT_Full;
+		break;
+	}
+
+	/* With no link speed and duplex are unknown */
+	ethtool_cmd_speed_set(ecmd, SPEED_UNKNOWN);
+	ecmd->duplex = DUPLEX_UNKNOWN;
+}
+
+/**
+ * i40e_get_settings - Get Link Speed and Duplex settings
+ * @netdev: network interface device structure
+ * @ecmd: ethtool command
+ *
+ * Reports speed/duplex settings based on media_type
+ **/
+static int i40e_get_settings(struct net_device *netdev,
+			     struct ethtool_cmd *ecmd)
+{
+	struct i40e_netdev_priv *np = netdev_priv(netdev);
+	struct i40e_pf *pf = np->vsi->back;
+	struct i40e_hw *hw = &pf->hw;
+	struct i40e_link_status *hw_link_info = &hw->phy.link_info;
+	bool link_up = hw_link_info->link_info & I40E_AQ_LINK_UP;
+
+	if (link_up)
+		i40e_get_settings_link_up(hw, ecmd, netdev);
+	else
+		i40e_get_settings_link_down(hw, ecmd);
+
+	/* Now set the settings that don't rely on link being up/down */
+
+	/* Set autoneg settings */
 	ecmd->autoneg = ((hw_link_info->an_info & I40E_AQ_AN_COMPLETED) ?
 			  AUTONEG_ENABLE : AUTONEG_DISABLE);
 
@@ -424,11 +505,13 @@ no_valid_phy_type:
 		break;
 	}
 
+	/* Set transceiver */
 	ecmd->transceiver = XCVR_EXTERNAL;
 
+	/* Set flow control settings */
 	ecmd->supported |= SUPPORTED_Pause;
 
-	switch (hw->fc.current_mode) {
+	switch (hw->fc.requested_mode) {
 	case I40E_FC_FULL:
 		ecmd->advertising |= ADVERTISED_Pause;
 		break;
@@ -445,30 +528,6 @@ no_valid_phy_type:
 		break;
 	}
 
-	if (link_up) {
-		switch (link_speed) {
-		case I40E_LINK_SPEED_40GB:
-			/* need a SPEED_40000 in ethtool.h */
-			ethtool_cmd_speed_set(ecmd, 40000);
-			break;
-		case I40E_LINK_SPEED_10GB:
-			ethtool_cmd_speed_set(ecmd, SPEED_10000);
-			break;
-		case I40E_LINK_SPEED_1GB:
-			ethtool_cmd_speed_set(ecmd, SPEED_1000);
-			break;
-		case I40E_LINK_SPEED_100MB:
-			ethtool_cmd_speed_set(ecmd, SPEED_100);
-			break;
-		default:
-			break;
-		}
-		ecmd->duplex = DUPLEX_FULL;
-	} else {
-		ethtool_cmd_speed_set(ecmd, SPEED_UNKNOWN);
-		ecmd->duplex = DUPLEX_UNKNOWN;
-	}
-
 	return 0;
 }
 
@@ -588,6 +647,8 @@ static int i40e_set_settings(struct net_device *netdev,
 	    advertise & ADVERTISED_10000baseKX4_Full ||
 	    advertise & ADVERTISED_10000baseKR_Full)
 		config.link_speed |= I40E_LINK_SPEED_10GB;
+	if (advertise & ADVERTISED_20000baseKR2_Full)
+		config.link_speed |= I40E_LINK_SPEED_20GB;
 	if (advertise & ADVERTISED_40000baseKR4_Full ||
 	    advertise & ADVERTISED_40000baseCR4_Full ||
 	    advertise & ADVERTISED_40000baseSR4_Full ||
@@ -601,6 +662,8 @@ static int i40e_set_settings(struct net_device *netdev,
 		config.eeer = abilities.eeer_val;
 		config.low_power_ctrl = abilities.d3_lpan;
 
+		/* save the requested speeds */
+		hw->phy.link_info.requested_speeds = config.link_speed;
 		/* set link and auto negotiation so changes take effect */
 		config.abilities |= I40E_AQ_PHY_ENABLE_ATOMIC_LINK;
 		/* If link is up put link down */
@@ -621,7 +684,7 @@ static int i40e_set_settings(struct net_device *netdev,
 			return -EAGAIN;
 		}
 
-		status = i40e_update_link_info(hw, true);
+		status = i40e_aq_get_link_info(hw, true, NULL, NULL);
 		if (status)
 			netdev_info(netdev, "Updating link info failed with error %d\n",
 				    status);
@@ -767,7 +830,7 @@ static int i40e_set_pauseparam(struct net_device *netdev,
 		err = -EAGAIN;
 	}
 	if (aq_failures & I40E_SET_FC_AQ_FAIL_UPDATE) {
-		netdev_info(netdev, "Set fc failed on the update_link_info call with error %d and status %d\n",
+		netdev_info(netdev, "Set fc failed on the get_link_info call with error %d and status %d\n",
 			    status, hw->aq.asq_last_status);
 		err = -EAGAIN;
 	}
@@ -870,7 +933,9 @@ static int i40e_get_eeprom(struct net_device *netdev,
 
 		cmd = (struct i40e_nvm_access *)eeprom;
 		ret_val = i40e_nvmupd_command(hw, cmd, bytes, &errno);
-		if (ret_val)
+		if (ret_val &&
+		    ((hw->aq.asq_last_status != I40E_AQ_RC_EACCES) ||
+		     (hw->debug_mask & I40E_DEBUG_NVM)))
 			dev_info(&pf->pdev->dev,
 				 "NVMUpdate read failed err=%d status=0x%x errno=%d module=%d offset=0x%x size=%d\n",
 				 ret_val, hw->aq.asq_last_status, errno,
@@ -974,7 +1039,10 @@ static int i40e_set_eeprom(struct net_device *netdev,
 
 	cmd = (struct i40e_nvm_access *)eeprom;
 	ret_val = i40e_nvmupd_command(hw, cmd, bytes, &errno);
-	if (ret_val && hw->aq.asq_last_status != I40E_AQ_RC_EBUSY)
+	if (ret_val &&
+	    ((hw->aq.asq_last_status != I40E_AQ_RC_EPERM &&
+	      hw->aq.asq_last_status != I40E_AQ_RC_EBUSY) ||
+	     (hw->debug_mask & I40E_DEBUG_NVM)))
 		dev_info(&pf->pdev->dev,
 			 "NVMUpdate write failed err=%d status=0x%x errno=%d module=%d offset=0x%x size=%d\n",
 			 ret_val, hw->aq.asq_last_status, errno,
@@ -998,6 +1066,7 @@ static void i40e_get_drvinfo(struct net_device *netdev,
 		sizeof(drvinfo->fw_version));
 	strlcpy(drvinfo->bus_info, pci_name(pf->pdev),
 		sizeof(drvinfo->bus_info));
+	drvinfo->n_priv_flags = I40E_PRIV_FLAGS_STR_LEN;
 }
 
 static void i40e_get_ringparam(struct net_device *netdev,
@@ -1176,7 +1245,7 @@ static int i40e_get_sset_count(struct net_device *netdev, int sset)
 	case ETH_SS_TEST:
 		return I40E_TEST_LEN;
 	case ETH_SS_STATS:
-		if (vsi == pf->vsi[pf->lan_vsi]) {
+		if (vsi == pf->vsi[pf->lan_vsi] && pf->hw.partition_id == 1) {
 			int len = I40E_PF_STATS_LEN(netdev);
 
 			if (pf->lan_veb != I40E_NO_VEB)
@@ -1185,6 +1254,8 @@ static int i40e_get_sset_count(struct net_device *netdev, int sset)
 		} else {
 			return I40E_VSI_STATS_LEN(netdev);
 		}
+	case ETH_SS_PRIV_FLAGS:
+		return I40E_PRIV_FLAGS_STR_LEN;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -1247,7 +1318,7 @@ static void i40e_get_ethtool_stats(struct net_device *netdev,
 		i += 2;
 	}
 	rcu_read_unlock();
-	if (vsi != pf->vsi[pf->lan_vsi])
+	if (vsi != pf->vsi[pf->lan_vsi] || pf->hw.partition_id != 1)
 		return;
 
 	if (pf->lan_veb != I40E_NO_VEB) {
@@ -1320,7 +1391,7 @@ static void i40e_get_strings(struct net_device *netdev, u32 stringset,
 			snprintf(p, ETH_GSTRING_LEN, "rx-%u.rx_bytes", i);
 			p += ETH_GSTRING_LEN;
 		}
-		if (vsi != pf->vsi[pf->lan_vsi])
+		if (vsi != pf->vsi[pf->lan_vsi] || pf->hw.partition_id != 1)
 			return;
 
 		if (pf->lan_veb != I40E_NO_VEB) {
@@ -1358,6 +1429,15 @@ static void i40e_get_strings(struct net_device *netdev, u32 stringset,
 		}
 		/* BUG_ON(p - data != I40E_STATS_LEN * ETH_GSTRING_LEN); */
 		break;
+	case ETH_SS_PRIV_FLAGS:
+		for (i = 0; i < I40E_PRIV_FLAGS_STR_LEN; i++) {
+			memcpy(data, i40e_priv_flags_strings[i],
+			       ETH_GSTRING_LEN);
+			data += ETH_GSTRING_LEN;
+		}
+		break;
+	default:
+		break;
 	}
 }
 
@@ -1473,6 +1553,7 @@ static void i40e_diag_test(struct net_device *netdev,
 			   struct ethtool_test *eth_test, u64 *data)
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
+	bool if_running = netif_running(netdev);
 	struct i40e_pf *pf = np->vsi->back;
 
 	if (eth_test->flags == ETH_TEST_FL_OFFLINE) {
@@ -1480,6 +1561,12 @@ static void i40e_diag_test(struct net_device *netdev,
 		netif_info(pf, drv, netdev, "offline testing starting\n");
 
 		set_bit(__I40E_TESTING, &pf->state);
+		/* If the device is online then take it offline */
+		if (if_running)
+			/* indicate we're in test mode */
+			dev_close(netdev);
+		else
+			i40e_do_reset(pf, (1 << __I40E_PF_RESET_REQUESTED));
 
 		/* Link test performed before hardware reset
 		 * so autoneg doesn't interfere with test result
@@ -1502,6 +1589,9 @@ static void i40e_diag_test(struct net_device *netdev,
 
 		clear_bit(__I40E_TESTING, &pf->state);
 		i40e_do_reset(pf, (1 << __I40E_PF_RESET_REQUESTED));
+
+		if (if_running)
+			dev_open(netdev);
 	} else {
 		/* Online tests */
 		netif_info(pf, drv, netdev, "online testing starting\n");
@@ -1599,6 +1689,8 @@ static int i40e_set_phys_id(struct net_device *netdev,
 	case ETHTOOL_ID_INACTIVE:
 		i40e_led_set(hw, pf->led_status, false);
 		break;
+	default:
+		break;
 	}
 
 	return 0;
@@ -1703,6 +1795,11 @@ static int i40e_get_rss_hash_opts(struct i40e_pf *pf, struct ethtool_rxnfc *cmd)
 {
 	cmd->data = 0;
 
+	if (pf->vsi[pf->lan_vsi]->rxnfc.data != 0) {
+		cmd->data = pf->vsi[pf->lan_vsi]->rxnfc.data;
+		cmd->flow_type = pf->vsi[pf->lan_vsi]->rxnfc.flow_type;
+		return 0;
+	}
 	/* Report default options for RSS on i40e */
 	switch (cmd->flow_type) {
 	case TCP_V4_FLOW:
@@ -1974,6 +2071,9 @@ static int i40e_set_rss_hash_opt(struct i40e_pf *pf, struct ethtool_rxnfc *nfc)
 	wr32(hw, I40E_PFQF_HENA(1), (u32)(hena >> 32));
 	i40e_flush(hw);
 
+	/* Save setting for future output/update */
+	pf->vsi[pf->lan_vsi]->rxnfc = *nfc;
+
 	return 0;
 }
 
@@ -2281,10 +2381,6 @@ static int i40e_set_channels(struct net_device *dev,
 	/* update feature limits from largest to smallest supported values */
 	/* TODO: Flow director limit, DCB etc */
 
-	/* cap RSS limit */
-	if (count > pf->rss_size_max)
-		count = pf->rss_size_max;
-
 	/* use rss_reconfig to rebuild with new queue count and update traffic
 	 * class queue mapping
 	 */
@@ -2295,6 +2391,133 @@ static int i40e_set_channels(struct net_device *dev,
 		return -EINVAL;
 }
 
+#define I40E_HLUT_ARRAY_SIZE ((I40E_PFQF_HLUT_MAX_INDEX + 1) * 4)
+/**
+ * i40e_get_rxfh_key_size - get the RSS hash key size
+ * @netdev: network interface device structure
+ *
+ * Returns the table size.
+ **/
+static u32 i40e_get_rxfh_key_size(struct net_device *netdev)
+{
+	return I40E_HKEY_ARRAY_SIZE;
+}
+
+/**
+ * i40e_get_rxfh_indir_size - get the rx flow hash indirection table size
+ * @netdev: network interface device structure
+ *
+ * Returns the table size.
+ **/
+static u32 i40e_get_rxfh_indir_size(struct net_device *netdev)
+{
+	return I40E_HLUT_ARRAY_SIZE;
+}
+
+static int i40e_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
+			 u8 *hfunc)
+{
+	struct i40e_netdev_priv *np = netdev_priv(netdev);
+	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	struct i40e_hw *hw = &pf->hw;
+	u32 reg_val;
+	int i, j;
+
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+
+	if (!indir)
+		return 0;
+
+	for (i = 0, j = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++) {
+		reg_val = rd32(hw, I40E_PFQF_HLUT(i));
+		indir[j++] = reg_val & 0xff;
+		indir[j++] = (reg_val >> 8) & 0xff;
+		indir[j++] = (reg_val >> 16) & 0xff;
+		indir[j++] = (reg_val >> 24) & 0xff;
+	}
+
+	if (key) {
+		for (i = 0, j = 0; i <= I40E_PFQF_HKEY_MAX_INDEX; i++) {
+			reg_val = rd32(hw, I40E_PFQF_HKEY(i));
+			key[j++] = (u8)(reg_val & 0xff);
+			key[j++] = (u8)((reg_val >> 8) & 0xff);
+			key[j++] = (u8)((reg_val >> 16) & 0xff);
+			key[j++] = (u8)((reg_val >> 24) & 0xff);
+		}
+	}
+	return 0;
+}
+
+/**
+ * i40e_set_rxfh - set the rx flow hash indirection table
+ * @netdev: network interface device structure
+ * @indir: indirection table
+ * @key: hash key
+ *
+ * Returns -EINVAL if the table specifies an inavlid queue id, otherwise
+ * returns 0 after programming the table.
+ **/
+static int i40e_set_rxfh(struct net_device *netdev, const u32 *indir,
+			 const u8 *key, const u8 hfunc)
+{
+	struct i40e_netdev_priv *np = netdev_priv(netdev);
+	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	struct i40e_hw *hw = &pf->hw;
+	u32 reg_val;
+	int i, j;
+
+	if (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP)
+		return -EOPNOTSUPP;
+
+	if (!indir)
+		return 0;
+
+	for (i = 0, j = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++) {
+		reg_val = indir[j++];
+		reg_val |= indir[j++] << 8;
+		reg_val |= indir[j++] << 16;
+		reg_val |= indir[j++] << 24;
+		wr32(hw, I40E_PFQF_HLUT(i), reg_val);
+	}
+
+	if (key) {
+		for (i = 0, j = 0; i <= I40E_PFQF_HKEY_MAX_INDEX; i++) {
+			reg_val = key[j++];
+			reg_val |= key[j++] << 8;
+			reg_val |= key[j++] << 16;
+			reg_val |= key[j++] << 24;
+			wr32(hw, I40E_PFQF_HKEY(i), reg_val);
+		}
+	}
+	return 0;
+}
+
+/**
+ * i40e_get_priv_flags - report device private flags
+ * @dev: network interface device structure
+ *
+ * The get string set count and the string set should be matched for each
+ * flag returned.  Add new strings for each flag to the i40e_priv_flags_strings
+ * array.
+ *
+ * Returns a u32 bitmap of flags.
+ **/
+static u32 i40e_get_priv_flags(struct net_device *dev)
+{
+	struct i40e_netdev_priv *np = netdev_priv(dev);
+	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	u32 ret_flags = 0;
+
+	ret_flags |= pf->hw.func_caps.npar_enable ?
+		I40E_PRIV_FLAGS_NPAR_FLAG : 0;
+
+	return ret_flags;
+}
+
 static const struct ethtool_ops i40e_ethtool_ops = {
 	.get_settings		= i40e_get_settings,
 	.set_settings		= i40e_set_settings,
@@ -2323,9 +2546,14 @@ static const struct ethtool_ops i40e_ethtool_ops = {
 	.get_ethtool_stats	= i40e_get_ethtool_stats,
 	.get_coalesce		= i40e_get_coalesce,
 	.set_coalesce		= i40e_set_coalesce,
+	.get_rxfh_key_size	= i40e_get_rxfh_key_size,
+	.get_rxfh_indir_size	= i40e_get_rxfh_indir_size,
+	.get_rxfh		= i40e_get_rxfh,
+	.set_rxfh		= i40e_set_rxfh,
 	.get_channels		= i40e_get_channels,
 	.set_channels		= i40e_set_channels,
 	.get_ts_info		= i40e_get_ts_info,
+	.get_priv_flags		= i40e_get_priv_flags,
 };
 
 void i40e_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
index 27c206e62da7..1803afeef23e 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -24,7 +24,6 @@
  *
  ******************************************************************************/
 
-
 #include <linux/if_ether.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_device.h>
@@ -150,7 +149,7 @@ static inline bool i40e_fcoe_xid_is_valid(u16 xid)
 
 /**
  * i40e_fcoe_ddp_unmap - unmap the mapped sglist associated
- * @pf: pointer to pf
+ * @pf: pointer to PF
  * @ddp: sw DDP context
  *
  * Unmap the scatter-gather list associated with the given SW DDP context
@@ -269,7 +268,7 @@ out:
 
 /**
  * i40e_fcoe_sw_init - sets up the HW for FCoE
- * @pf: pointer to pf
+ * @pf: pointer to PF
  *
  * Returns 0 if FCoE is supported otherwise the error code
  **/
@@ -329,7 +328,7 @@ int i40e_init_pf_fcoe(struct i40e_pf *pf)
 
 /**
  * i40e_get_fcoe_tc_map - Return TC map for FCoE APP
- * @pf: pointer to pf
+ * @pf: pointer to PF
  *
  **/
 u8 i40e_get_fcoe_tc_map(struct i40e_pf *pf)
@@ -381,12 +380,11 @@ int i40e_fcoe_vsi_init(struct i40e_vsi *vsi, struct i40e_vsi_context *ctxt)
 	ctxt->pf_num = hw->pf_id;
 	ctxt->vf_num = 0;
 	ctxt->uplink_seid = vsi->uplink_seid;
-	ctxt->connection_type = 0x1;
+	ctxt->connection_type = I40E_AQ_VSI_CONN_TYPE_NORMAL;
 	ctxt->flags = I40E_AQ_VSI_TYPE_PF;
 
 	/* FCoE VSI would need the following sections */
-	info->valid_sections |= cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID |
-					    I40E_AQ_VSI_PROP_QUEUE_OPT_VALID);
+	info->valid_sections |= cpu_to_le16(I40E_AQ_VSI_PROP_QUEUE_OPT_VALID);
 
 	/* FCoE VSI does not need these sections */
 	info->valid_sections &= cpu_to_le16(~(I40E_AQ_VSI_PROP_SECURITY_VALID |
@@ -395,7 +393,12 @@ int i40e_fcoe_vsi_init(struct i40e_vsi *vsi, struct i40e_vsi_context *ctxt)
 					    I40E_AQ_VSI_PROP_INGRESS_UP_VALID |
 					    I40E_AQ_VSI_PROP_EGRESS_UP_VALID));
 
-	info->switch_id = cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+	if (i40e_is_vsi_uplink_mode_veb(vsi)) {
+		info->valid_sections |=
+				cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
+		info->switch_id =
+				cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+	}
 	enabled_tc = i40e_get_fcoe_tc_map(pf);
 	i40e_vsi_setup_queue_map(vsi, ctxt, enabled_tc, true);
 
@@ -1303,8 +1306,7 @@ static void i40e_fcoe_tx_map(struct i40e_ring *tx_ring,
 	/* MACLEN is ether header length in words not bytes */
 	td_offset |= (maclen >> 1) << I40E_TX_DESC_LENGTH_MACLEN_SHIFT;
 
-	return i40e_tx_map(tx_ring, skb, first, tx_flags, hdr_len,
-			   td_cmd, td_offset);
+	i40e_tx_map(tx_ring, skb, first, tx_flags, hdr_len, td_cmd, td_offset);
 }
 
 /**
@@ -1443,7 +1445,6 @@ static int i40e_fcoe_set_features(struct net_device *netdev,
 	return 0;
 }
 
-
 static const struct net_device_ops i40e_fcoe_netdev_ops = {
 	.ndo_open		= i40e_open,
 	.ndo_stop		= i40e_close,
@@ -1470,6 +1471,11 @@ static const struct net_device_ops i40e_fcoe_netdev_ops = {
 	.ndo_set_features	= i40e_fcoe_set_features,
 };
 
+/* fcoe network device type */
+static struct device_type fcoe_netdev_type = {
+	.name = "fcoe",
+};
+
 /**
  * i40e_fcoe_config_netdev - prepares the VSI context for creating a FCoE VSI
  * @vsi: pointer to the associated VSI struct
@@ -1503,6 +1509,7 @@ void i40e_fcoe_config_netdev(struct net_device *netdev, struct i40e_vsi *vsi)
 	strlcpy(netdev->name, "fcoe%d", IFNAMSIZ-1);
 	netdev->mtu = FCOE_MTU;
 	SET_NETDEV_DEV(netdev, &pf->pdev->dev);
+	SET_NETDEV_DEVTYPE(netdev, &fcoe_netdev_type);
 	/* set different dev_port value 1 for FCoE netdev than the default
 	 * zero dev_port value for PF netdev, this helps biosdevname user
 	 * tool to differentiate them correctly while both attached to the
@@ -1523,7 +1530,7 @@ void i40e_fcoe_config_netdev(struct net_device *netdev, struct i40e_vsi *vsi)
 
 /**
  * i40e_fcoe_vsi_setup - allocate and set up FCoE VSI
- * @pf: the pf that VSI is associated with
+ * @pf: the PF that VSI is associated with
  *
  **/
 void i40e_fcoe_vsi_setup(struct i40e_pf *pf)
@@ -1550,7 +1557,7 @@ void i40e_fcoe_vsi_setup(struct i40e_pf *pf)
 	vsi = i40e_vsi_setup(pf, I40E_VSI_FCOE, seid, 0);
 	if (vsi) {
 		dev_dbg(&pf->pdev->dev,
-			"Successfully created FCoE VSI seid %d id %d uplink_seid %d pf seid %d\n",
+			"Successfully created FCoE VSI seid %d id %d uplink_seid %d PF seid %d\n",
 			vsi->seid, vsi->id, vsi->uplink_seid, seid);
 	} else {
 		dev_info(&pf->pdev->dev, "Failed to create FCoE VSI\n");
diff --git a/drivers/net/ethernet/intel/i40e/i40e_fcoe.h b/drivers/net/ethernet/intel/i40e/i40e_fcoe.h
index 21e0f582031c..0d49e2d15d40 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_fcoe.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_fcoe.h
@@ -37,7 +37,6 @@
 #define I40E_FILTER_CONTEXT_DESC(R, i)  \
 	(&(((struct i40e_fcoe_filter_context_desc *)((R)->desc))[i]))
 
-
 /* receive queue descriptor filter status for FCoE */
 #define I40E_RX_DESC_FLTSTAT_FCMASK	0x3
 #define I40E_RX_DESC_FLTSTAT_NOMTCH	0x0	/* no ddp context match */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c b/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c
index 4627588f4613..0079ad7bcd0e 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c
@@ -856,7 +856,7 @@ static void i40e_write_dword(u8 *hmc_bits,
 	if (ce_info->width < 32)
 		mask = ((u32)1 << ce_info->width) - 1;
 	else
-		mask = 0xFFFFFFFF;
+		mask = ~(u32)0;
 
 	/* don't swizzle the bits until after the mask because the mask bits
 	 * will be in a different bit position on big endian machines
@@ -908,7 +908,7 @@ static void i40e_write_qword(u8 *hmc_bits,
 	if (ce_info->width < 64)
 		mask = ((u64)1 << ce_info->width) - 1;
 	else
-		mask = 0xFFFFFFFFFFFFFFFF;
+		mask = ~(u64)0;
 
 	/* don't swizzle the bits until after the mask because the mask bits
 	 * will be in a different bit position on big endian machines
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index dadda3c5d658..63de3f4b7a94 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -38,8 +38,8 @@ static const char i40e_driver_string[] =
 #define DRV_KERN "-k"
 
 #define DRV_VERSION_MAJOR 1
-#define DRV_VERSION_MINOR 2
-#define DRV_VERSION_BUILD 6
+#define DRV_VERSION_MINOR 3
+#define DRV_VERSION_BUILD 1
 #define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
 	     __stringify(DRV_VERSION_MINOR) "." \
 	     __stringify(DRV_VERSION_BUILD)    DRV_KERN
@@ -75,6 +75,7 @@ static const struct pci_device_id i40e_pci_tbl[] = {
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_QSFP_B), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_QSFP_C), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_10G_BASE_T), 0},
+	{PCI_VDEVICE(INTEL, I40E_DEV_ID_20G_KR2), 0},
 	/* required last entry */
 	{0, }
 };
@@ -249,6 +250,22 @@ static int i40e_put_lump(struct i40e_lump_tracking *pile, u16 index, u16 id)
 }
 
 /**
+ * i40e_find_vsi_from_id - searches for the vsi with the given id
+ * @pf - the pf structure to search for the vsi
+ * @id - id of the vsi it is searching for
+ **/
+struct i40e_vsi *i40e_find_vsi_from_id(struct i40e_pf *pf, u16 id)
+{
+	int i;
+
+	for (i = 0; i < pf->num_alloc_vsi; i++)
+		if (pf->vsi[i] && (pf->vsi[i]->id == id))
+			return pf->vsi[i];
+
+	return NULL;
+}
+
+/**
  * i40e_service_event_schedule - Schedule the service task to wake up
  * @pf: board private structure
  *
@@ -450,7 +467,7 @@ void i40e_vsi_reset_stats(struct i40e_vsi *vsi)
 }
 
 /**
- * i40e_pf_reset_stats - Reset all of the stats for the given pf
+ * i40e_pf_reset_stats - Reset all of the stats for the given PF
  * @pf: the PF to be reset
  **/
 void i40e_pf_reset_stats(struct i40e_pf *pf)
@@ -896,7 +913,7 @@ static void i40e_update_vsi_stats(struct i40e_vsi *vsi)
 }
 
 /**
- * i40e_update_pf_stats - Update the pf statistics counters.
+ * i40e_update_pf_stats - Update the PF statistics counters.
  * @pf: the PF to be updated
  **/
 static void i40e_update_pf_stats(struct i40e_pf *pf)
@@ -919,11 +936,6 @@ static void i40e_update_pf_stats(struct i40e_pf *pf)
 			   pf->stat_offsets_loaded,
 			   &osd->eth.rx_discards,
 			   &nsd->eth.rx_discards);
-	i40e_stat_update32(hw, I40E_GLPRT_TDPC(hw->port),
-			   pf->stat_offsets_loaded,
-			   &osd->eth.tx_discards,
-			   &nsd->eth.tx_discards);
-
 	i40e_stat_update48(hw, I40E_GLPRT_UPRCH(hw->port),
 			   I40E_GLPRT_UPRCL(hw->port),
 			   pf->stat_offsets_loaded,
@@ -1133,7 +1145,7 @@ void i40e_update_stats(struct i40e_vsi *vsi)
  * @vsi: the VSI to be searched
  * @macaddr: the MAC address
  * @vlan: the vlan
- * @is_vf: make sure its a vf filter, else doesn't matter
+ * @is_vf: make sure its a VF filter, else doesn't matter
  * @is_netdev: make sure its a netdev filter, else doesn't matter
  *
  * Returns ptr to the filter object or NULL
@@ -1161,7 +1173,7 @@ static struct i40e_mac_filter *i40e_find_filter(struct i40e_vsi *vsi,
  * i40e_find_mac - Find a mac addr in the macvlan filters list
  * @vsi: the VSI to be searched
  * @macaddr: the MAC address we are searching for
- * @is_vf: make sure its a vf filter, else doesn't matter
+ * @is_vf: make sure its a VF filter, else doesn't matter
  * @is_netdev: make sure its a netdev filter, else doesn't matter
  *
  * Returns the first filter with the provided MAC address or NULL if
@@ -1209,7 +1221,7 @@ bool i40e_is_vsi_in_vlan(struct i40e_vsi *vsi)
  * i40e_put_mac_in_vlan - Make macvlan filters from macaddrs and vlans
  * @vsi: the VSI to be searched
  * @macaddr: the mac address to be filtered
- * @is_vf: true if it is a vf
+ * @is_vf: true if it is a VF
  * @is_netdev: true if it is a netdev
  *
  * Goes through all the macvlan filters and adds a
@@ -1270,7 +1282,7 @@ static int i40e_rm_default_mac_filter(struct i40e_vsi *vsi, u8 *macaddr)
  * @vsi: the VSI to be searched
  * @macaddr: the MAC address
  * @vlan: the vlan
- * @is_vf: make sure its a vf filter, else doesn't matter
+ * @is_vf: make sure its a VF filter, else doesn't matter
  * @is_netdev: make sure its a netdev filter, else doesn't matter
  *
  * Returns ptr to the filter object or NULL when no memory available.
@@ -1330,7 +1342,7 @@ add_filter_out:
  * @vsi: the VSI to be searched
  * @macaddr: the MAC address
  * @vlan: the vlan
- * @is_vf: make sure it's a vf filter, else doesn't matter
+ * @is_vf: make sure it's a VF filter, else doesn't matter
  * @is_netdev: make sure it's a netdev filter, else doesn't matter
  **/
 void i40e_del_filter(struct i40e_vsi *vsi,
@@ -1357,7 +1369,7 @@ void i40e_del_filter(struct i40e_vsi *vsi,
 			f->counter--;
 		}
 	} else {
-		/* make sure we don't remove a filter in use by vf or netdev */
+		/* make sure we don't remove a filter in use by VF or netdev */
 		int min_f = 0;
 		min_f += (f->is_vf ? 1 : 0);
 		min_f += (f->is_netdev ? 1 : 0);
@@ -1546,7 +1558,7 @@ static void i40e_vsi_setup_queue_map(struct i40e_vsi *vsi,
 			vsi->tc_config.tc_info[i].qoffset = offset;
 			vsi->tc_config.tc_info[i].qcount = qcount;
 
-			/* find the power-of-2 of the number of queue pairs */
+			/* find the next higher power-of-2 of num queue pairs */
 			num_qps = qcount;
 			pow = 0;
 			while (num_qps && ((1 << pow) < qcount)) {
@@ -1576,6 +1588,12 @@ static void i40e_vsi_setup_queue_map(struct i40e_vsi *vsi,
 
 	/* Set actual Tx/Rx queue pairs */
 	vsi->num_queue_pairs = offset;
+	if ((vsi->type == I40E_VSI_MAIN) && (numtc == 1)) {
+		if (vsi->req_queue_pairs > 0)
+			vsi->num_queue_pairs = vsi->req_queue_pairs;
+		else
+			vsi->num_queue_pairs = pf->num_lan_msix;
+	}
 
 	/* Scheduler section valid can only be set for ADD VSI */
 	if (is_add) {
@@ -1967,7 +1985,7 @@ void i40e_vlan_stripping_enable(struct i40e_vsi *vsi)
 				    I40E_AQ_VSI_PVLAN_EMOD_STR_BOTH;
 
 	ctxt.seid = vsi->seid;
-	memcpy(&ctxt.info, &vsi->info, sizeof(vsi->info));
+	ctxt.info = vsi->info;
 	ret = i40e_aq_update_vsi_params(&vsi->back->hw, &ctxt, NULL);
 	if (ret) {
 		dev_info(&vsi->back->pdev->dev,
@@ -1996,7 +2014,7 @@ void i40e_vlan_stripping_disable(struct i40e_vsi *vsi)
 				    I40E_AQ_VSI_PVLAN_EMOD_NOTHING;
 
 	ctxt.seid = vsi->seid;
-	memcpy(&ctxt.info, &vsi->info, sizeof(vsi->info));
+	ctxt.info = vsi->info;
 	ret = i40e_aq_update_vsi_params(&vsi->back->hw, &ctxt, NULL);
 	if (ret) {
 		dev_info(&vsi->back->pdev->dev,
@@ -2280,7 +2298,7 @@ int i40e_vsi_add_pvid(struct i40e_vsi *vsi, u16 vid)
 				    I40E_AQ_VSI_PVLAN_EMOD_STR;
 
 	ctxt.seid = vsi->seid;
-	memcpy(&ctxt.info, &vsi->info, sizeof(vsi->info));
+	ctxt.info = vsi->info;
 	aq_ret = i40e_aq_update_vsi_params(&vsi->back->hw, &ctxt, NULL);
 	if (aq_ret) {
 		dev_info(&vsi->back->pdev->dev,
@@ -2398,20 +2416,20 @@ static void i40e_config_xps_tx_ring(struct i40e_ring *ring)
 	struct i40e_vsi *vsi = ring->vsi;
 	cpumask_var_t mask;
 
-	if (ring->q_vector && ring->netdev) {
-		/* Single TC mode enable XPS */
-		if (vsi->tc_config.numtc <= 1 &&
-		    !test_and_set_bit(__I40E_TX_XPS_INIT_DONE, &ring->state)) {
+	if (!ring->q_vector || !ring->netdev)
+		return;
+
+	/* Single TC mode enable XPS */
+	if (vsi->tc_config.numtc <= 1) {
+		if (!test_and_set_bit(__I40E_TX_XPS_INIT_DONE, &ring->state))
 			netif_set_xps_queue(ring->netdev,
 					    &ring->q_vector->affinity_mask,
 					    ring->queue_index);
-		} else if (alloc_cpumask_var(&mask, GFP_KERNEL)) {
-			/* Disable XPS to allow selection based on TC */
-			bitmap_zero(cpumask_bits(mask), nr_cpumask_bits);
-			netif_set_xps_queue(ring->netdev, mask,
-					    ring->queue_index);
-			free_cpumask_var(mask);
-		}
+	} else if (alloc_cpumask_var(&mask, GFP_KERNEL)) {
+		/* Disable XPS to allow selection based on TC */
+		bitmap_zero(cpumask_bits(mask), nr_cpumask_bits);
+		netif_set_xps_queue(ring->netdev, mask, ring->queue_index);
+		free_cpumask_var(mask);
 	}
 }
 
@@ -2596,7 +2614,12 @@ static int i40e_configure_rx_ring(struct i40e_ring *ring)
 	ring->tail = hw->hw_addr + I40E_QRX_TAIL(pf_q);
 	writel(0, ring->tail);
 
-	i40e_alloc_rx_buffers(ring, I40E_DESC_UNUSED(ring));
+	if (ring_is_ps_enabled(ring)) {
+		i40e_alloc_rx_headers(ring);
+		i40e_alloc_rx_buffers_ps(ring, I40E_DESC_UNUSED(ring));
+	} else {
+		i40e_alloc_rx_buffers_1buf(ring, I40E_DESC_UNUSED(ring));
+	}
 
 	return 0;
 }
@@ -3183,13 +3206,16 @@ static irqreturn_t i40e_intr(int irq, void *data)
 			pf->globr_count++;
 		} else if (val == I40E_RESET_EMPR) {
 			pf->empr_count++;
-			set_bit(__I40E_EMP_RESET_REQUESTED, &pf->state);
+			set_bit(__I40E_EMP_RESET_INTR_RECEIVED, &pf->state);
 		}
 	}
 
 	if (icr0 & I40E_PFINT_ICR0_HMC_ERR_MASK) {
 		icr0 &= ~I40E_PFINT_ICR0_HMC_ERR_MASK;
 		dev_info(&pf->pdev->dev, "HMC error interrupt\n");
+		dev_info(&pf->pdev->dev, "HMC error info 0x%x, HMC error data 0x%x\n",
+			 rd32(hw, I40E_PFHMC_ERRORINFO),
+			 rd32(hw, I40E_PFHMC_ERRORDATA));
 	}
 
 	if (icr0 & I40E_PFINT_ICR0_TIMESYNC_MASK) {
@@ -3825,6 +3851,8 @@ static void i40e_reset_interrupt_capability(struct i40e_pf *pf)
 		pci_disable_msix(pf->pdev);
 		kfree(pf->msix_entries);
 		pf->msix_entries = NULL;
+		kfree(pf->irq_pile);
+		pf->irq_pile = NULL;
 	} else if (pf->flags & I40E_FLAG_MSI_ENABLED) {
 		pci_disable_msi(pf->pdev);
 	}
@@ -4021,7 +4049,7 @@ static int i40e_pf_wait_txq_disabled(struct i40e_pf *pf)
 #endif
 /**
  * i40e_get_iscsi_tc_map - Return TC map for iSCSI APP
- * @pf: pointer to pf
+ * @pf: pointer to PF
  *
  * Get TC map for ISCSI PF type that will include iSCSI TC
  * and LAN TC.
@@ -4119,7 +4147,7 @@ static u8 i40e_pf_get_num_tc(struct i40e_pf *pf)
 	if (pf->hw.func_caps.iscsi)
 		enabled_tc =  i40e_get_iscsi_tc_map(pf);
 	else
-		enabled_tc = pf->hw.func_caps.enabled_tcmap;
+		return 1; /* Only TC0 */
 
 	/* At least have TC0 */
 	enabled_tc = (enabled_tc ? enabled_tc : 0x1);
@@ -4169,11 +4197,11 @@ static u8 i40e_pf_get_tc_map(struct i40e_pf *pf)
 	if (!(pf->flags & I40E_FLAG_MFP_ENABLED))
 		return i40e_dcb_get_enabled_tc(&pf->hw.local_dcbx_config);
 
-	/* MPF enabled and iSCSI PF type */
+	/* MFP enabled and iSCSI PF type */
 	if (pf->hw.func_caps.iscsi)
 		return i40e_get_iscsi_tc_map(pf);
 	else
-		return pf->hw.func_caps.enabled_tcmap;
+		return i40e_pf_get_default_tc(pf);
 }
 
 /**
@@ -4196,7 +4224,7 @@ static int i40e_vsi_get_bw_info(struct i40e_vsi *vsi)
 	aq_ret = i40e_aq_query_vsi_bw_config(hw, vsi->seid, &bw_config, NULL);
 	if (aq_ret) {
 		dev_info(&pf->pdev->dev,
-			 "couldn't get pf vsi bw config, err %d, aq_err %d\n",
+			 "couldn't get PF vsi bw config, err %d, aq_err %d\n",
 			 aq_ret, pf->hw.aq.asq_last_status);
 		return -EINVAL;
 	}
@@ -4206,7 +4234,7 @@ static int i40e_vsi_get_bw_info(struct i40e_vsi *vsi)
 						  NULL);
 	if (aq_ret) {
 		dev_info(&pf->pdev->dev,
-			 "couldn't get pf vsi ets bw config, err %d, aq_err %d\n",
+			 "couldn't get PF vsi ets bw config, err %d, aq_err %d\n",
 			 aq_ret, pf->hw.aq.asq_last_status);
 		return -EINVAL;
 	}
@@ -4383,7 +4411,7 @@ static int i40e_vsi_config_tc(struct i40e_vsi *vsi, u8 enabled_tc)
 	ctxt.pf_num = vsi->back->hw.pf_id;
 	ctxt.vf_num = 0;
 	ctxt.uplink_seid = vsi->uplink_seid;
-	memcpy(&ctxt.info, &vsi->info, sizeof(vsi->info));
+	ctxt.info = vsi->info;
 	i40e_vsi_setup_queue_map(vsi, &ctxt, enabled_tc, false);
 
 	/* Update the VSI after updating the VSI queue-mapping information */
@@ -4563,6 +4591,11 @@ static int i40e_init_pf_dcb(struct i40e_pf *pf)
 	struct i40e_hw *hw = &pf->hw;
 	int err = 0;
 
+	/* Do not enable DCB for SW1 and SW2 images even if the FW is capable */
+	if (((pf->hw.aq.fw_maj_ver == 4) && (pf->hw.aq.fw_min_ver < 33)) ||
+	    (pf->hw.aq.fw_maj_ver < 4))
+		goto out;
+
 	/* Get the initial DCB configuration */
 	err = i40e_init_dcb(hw);
 	if (!err) {
@@ -4626,6 +4659,9 @@ static void i40e_print_link_message(struct i40e_vsi *vsi, bool isup)
 	case I40E_LINK_SPEED_40GB:
 		strlcpy(speed, "40 Gbps", SPEED_SIZE);
 		break;
+	case I40E_LINK_SPEED_20GB:
+		strncpy(speed, "20 Gbps", SPEED_SIZE);
+		break;
 	case I40E_LINK_SPEED_10GB:
 		strlcpy(speed, "10 Gbps", SPEED_SIZE);
 		break;
@@ -4853,11 +4889,7 @@ exit:
  *
  * Returns 0 on success, negative value on failure
  **/
-#ifdef I40E_FCOE
 int i40e_open(struct net_device *netdev)
-#else
-static int i40e_open(struct net_device *netdev)
-#endif
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
 	struct i40e_vsi *vsi = np->vsi;
@@ -4967,7 +4999,7 @@ err_setup_tx:
 
 /**
  * i40e_fdir_filter_exit - Cleans up the Flow Director accounting
- * @pf: Pointer to pf
+ * @pf: Pointer to PF
  *
  * This function destroys the hlist where all the Flow Director
  * filters were saved.
@@ -5055,24 +5087,6 @@ void i40e_do_reset(struct i40e_pf *pf, u32 reset_flags)
 		wr32(&pf->hw, I40E_GLGEN_RTRIG, val);
 		i40e_flush(&pf->hw);
 
-	} else if (reset_flags & (1 << __I40E_EMP_RESET_REQUESTED)) {
-
-		/* Request a Firmware Reset
-		 *
-		 * Same as Global reset, plus restarting the
-		 * embedded firmware engine.
-		 */
-		/* enable EMP Reset */
-		val = rd32(&pf->hw, I40E_GLGEN_RSTENA_EMP);
-		val |= I40E_GLGEN_RSTENA_EMP_EMP_RST_ENA_MASK;
-		wr32(&pf->hw, I40E_GLGEN_RSTENA_EMP, val);
-
-		/* force the reset */
-		val = rd32(&pf->hw, I40E_GLGEN_RTRIG);
-		val |= I40E_GLGEN_RTRIG_EMPFWR_MASK;
-		wr32(&pf->hw, I40E_GLGEN_RTRIG, val);
-		i40e_flush(&pf->hw);
-
 	} else if (reset_flags & (1 << __I40E_PF_RESET_REQUESTED)) {
 
 		/* Request a PF Reset
@@ -5195,7 +5209,6 @@ static int i40e_handle_lldp_event(struct i40e_pf *pf,
 	struct i40e_aqc_lldp_get_mib *mib =
 		(struct i40e_aqc_lldp_get_mib *)&e->desc.params.raw;
 	struct i40e_hw *hw = &pf->hw;
-	struct i40e_dcbx_config *dcbx_cfg = &hw->local_dcbx_config;
 	struct i40e_dcbx_config tmp_dcbx_cfg;
 	bool need_reconfig = false;
 	int ret = 0;
@@ -5226,10 +5239,11 @@ static int i40e_handle_lldp_event(struct i40e_pf *pf,
 		goto exit;
 	}
 
-	memset(&tmp_dcbx_cfg, 0, sizeof(tmp_dcbx_cfg));
 	/* Store the old configuration */
-	tmp_dcbx_cfg = *dcbx_cfg;
+	tmp_dcbx_cfg = hw->local_dcbx_config;
 
+	/* Reset the old DCBx configuration data */
+	memset(&hw->local_dcbx_config, 0, sizeof(hw->local_dcbx_config));
 	/* Get updated DCBX data from firmware */
 	ret = i40e_get_dcb_config(&pf->hw);
 	if (ret) {
@@ -5238,20 +5252,22 @@ static int i40e_handle_lldp_event(struct i40e_pf *pf,
 	}
 
 	/* No change detected in DCBX configs */
-	if (!memcmp(&tmp_dcbx_cfg, dcbx_cfg, sizeof(tmp_dcbx_cfg))) {
+	if (!memcmp(&tmp_dcbx_cfg, &hw->local_dcbx_config,
+		    sizeof(tmp_dcbx_cfg))) {
 		dev_dbg(&pf->pdev->dev, "No change detected in DCBX configuration.\n");
 		goto exit;
 	}
 
-	need_reconfig = i40e_dcb_need_reconfig(pf, &tmp_dcbx_cfg, dcbx_cfg);
+	need_reconfig = i40e_dcb_need_reconfig(pf, &tmp_dcbx_cfg,
+					       &hw->local_dcbx_config);
 
-	i40e_dcbnl_flush_apps(pf, dcbx_cfg);
+	i40e_dcbnl_flush_apps(pf, &tmp_dcbx_cfg, &hw->local_dcbx_config);
 
 	if (!need_reconfig)
 		goto exit;
 
 	/* Enable DCB tagging only when more than one TC */
-	if (i40e_dcb_get_num_tc(dcbx_cfg) > 1)
+	if (i40e_dcb_get_num_tc(&hw->local_dcbx_config) > 1)
 		pf->flags |= I40E_FLAG_DCB_ENABLED;
 	else
 		pf->flags &= ~I40E_FLAG_DCB_ENABLED;
@@ -5351,9 +5367,9 @@ static void i40e_service_event_complete(struct i40e_pf *pf)
  * i40e_get_cur_guaranteed_fd_count - Get the consumed guaranteed FD filters
  * @pf: board private structure
  **/
-int i40e_get_cur_guaranteed_fd_count(struct i40e_pf *pf)
+u32 i40e_get_cur_guaranteed_fd_count(struct i40e_pf *pf)
 {
-	int val, fcnt_prog;
+	u32 val, fcnt_prog;
 
 	val = rd32(&pf->hw, I40E_PFQF_FDSTAT);
 	fcnt_prog = (val & I40E_PFQF_FDSTAT_GUARANT_CNT_MASK);
@@ -5361,12 +5377,13 @@ int i40e_get_cur_guaranteed_fd_count(struct i40e_pf *pf)
 }
 
 /**
- * i40e_get_current_fd_count - Get the count of total FD filters programmed
+ * i40e_get_current_fd_count - Get total FD filters programmed for this PF
  * @pf: board private structure
  **/
-int i40e_get_current_fd_count(struct i40e_pf *pf)
+u32 i40e_get_current_fd_count(struct i40e_pf *pf)
 {
-	int val, fcnt_prog;
+	u32 val, fcnt_prog;
+
 	val = rd32(&pf->hw, I40E_PFQF_FDSTAT);
 	fcnt_prog = (val & I40E_PFQF_FDSTAT_GUARANT_CNT_MASK) +
 		    ((val & I40E_PFQF_FDSTAT_BEST_CNT_MASK) >>
@@ -5375,6 +5392,21 @@ int i40e_get_current_fd_count(struct i40e_pf *pf)
 }
 
 /**
+ * i40e_get_global_fd_count - Get total FD filters programmed on device
+ * @pf: board private structure
+ **/
+u32 i40e_get_global_fd_count(struct i40e_pf *pf)
+{
+	u32 val, fcnt_prog;
+
+	val = rd32(&pf->hw, I40E_GLQF_FDCNT_0);
+	fcnt_prog = (val & I40E_GLQF_FDCNT_0_GUARANT_CNT_MASK) +
+		    ((val & I40E_GLQF_FDCNT_0_BESTCNT_MASK) >>
+		     I40E_GLQF_FDCNT_0_BESTCNT_SHIFT);
+	return fcnt_prog;
+}
+
+/**
  * i40e_fdir_check_and_reenable - Function to reenabe FD ATR or SB if disabled
  * @pf: board private structure
  **/
@@ -5388,7 +5420,7 @@ void i40e_fdir_check_and_reenable(struct i40e_pf *pf)
 	/* Check if, FD SB or ATR was auto disabled and if there is enough room
 	 * to re-enable
 	 */
-	fcnt_prog = i40e_get_cur_guaranteed_fd_count(pf);
+	fcnt_prog = i40e_get_global_fd_count(pf);
 	fcnt_avail = pf->fdir_pf_filter_count;
 	if ((fcnt_prog < (fcnt_avail - I40E_FDIR_BUFFER_HEAD_ROOM)) ||
 	    (pf->fd_add_err == 0) ||
@@ -5410,13 +5442,17 @@ void i40e_fdir_check_and_reenable(struct i40e_pf *pf)
 }
 
 #define I40E_MIN_FD_FLUSH_INTERVAL 10
+#define I40E_MIN_FD_FLUSH_SB_ATR_UNSTABLE 30
 /**
  * i40e_fdir_flush_and_replay - Function to flush all FD filters and replay SB
  * @pf: board private structure
  **/
 static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 {
+	unsigned long min_flush_time;
 	int flush_wait_retry = 50;
+	bool disable_atr = false;
+	int fd_room;
 	int reg;
 
 	if (!(pf->flags & (I40E_FLAG_FD_SB_ENABLED | I40E_FLAG_FD_ATR_ENABLED)))
@@ -5424,9 +5460,20 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 
 	if (time_after(jiffies, pf->fd_flush_timestamp +
 				(I40E_MIN_FD_FLUSH_INTERVAL * HZ))) {
-		set_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
+		/* If the flush is happening too quick and we have mostly
+		 * SB rules we should not re-enable ATR for some time.
+		 */
+		min_flush_time = pf->fd_flush_timestamp
+				+ (I40E_MIN_FD_FLUSH_SB_ATR_UNSTABLE * HZ);
+		fd_room = pf->fdir_pf_filter_count - pf->fdir_pf_active_filters;
+
+		if (!(time_after(jiffies, min_flush_time)) &&
+		    (fd_room < I40E_FDIR_BUFFER_HEAD_ROOM_FOR_ATR)) {
+			dev_info(&pf->pdev->dev, "ATR disabled, not enough FD filter space.\n");
+			disable_atr = true;
+		}
+
 		pf->fd_flush_timestamp = jiffies;
-		pf->auto_disable_flags |= I40E_FLAG_FD_SB_ENABLED;
 		pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
 		/* flush all filters */
 		wr32(&pf->hw, I40E_PFQF_CTL_1,
@@ -5446,10 +5493,8 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 		} else {
 			/* replay sideband filters */
 			i40e_fdir_filter_restore(pf->vsi[pf->lan_vsi]);
-
-			pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
-			pf->auto_disable_flags &= ~I40E_FLAG_FD_ATR_ENABLED;
-			pf->auto_disable_flags &= ~I40E_FLAG_FD_SB_ENABLED;
+			if (!disable_atr)
+				pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
 			clear_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
 			dev_info(&pf->pdev->dev, "FD Filter table flushed and FD-SB replayed.\n");
 		}
@@ -5460,7 +5505,7 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
  * i40e_get_current_atr_count - Get the count of total FD ATR filters programmed
  * @pf: board private structure
  **/
-int i40e_get_current_atr_cnt(struct i40e_pf *pf)
+u32 i40e_get_current_atr_cnt(struct i40e_pf *pf)
 {
 	return i40e_get_current_fd_count(pf) - pf->fdir_pf_active_filters;
 }
@@ -5486,9 +5531,7 @@ static void i40e_fdir_reinit_subtask(struct i40e_pf *pf)
 	if (!(pf->flags & (I40E_FLAG_FD_SB_ENABLED | I40E_FLAG_FD_ATR_ENABLED)))
 		return;
 
-	if ((pf->fd_add_err >= I40E_MAX_FD_PROGRAM_ERROR) &&
-	    (i40e_get_current_atr_cnt(pf) >= pf->fd_atr_cnt) &&
-	    (i40e_get_current_atr_cnt(pf) > pf->fdir_pf_filter_count))
+	if (test_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state))
 		i40e_fdir_flush_and_replay(pf);
 
 	i40e_fdir_check_and_reenable(pf);
@@ -5757,11 +5800,9 @@ static void i40e_handle_link_event(struct i40e_pf *pf,
 	struct i40e_hw *hw = &pf->hw;
 	struct i40e_aqc_get_link_status *status =
 		(struct i40e_aqc_get_link_status *)&e->desc.params.raw;
-	struct i40e_link_status *hw_link_info = &hw->phy.link_info;
 
 	/* save off old link status information */
-	memcpy(&pf->hw.phy.link_info_old, hw_link_info,
-	       sizeof(pf->hw.phy.link_info_old));
+	hw->phy.link_info_old = hw->phy.link_info;
 
 	/* Do a new status request to re-enable LSE reporting
 	 * and load new status information into the hw struct
@@ -5875,6 +5916,10 @@ static void i40e_clean_adminq_subtask(struct i40e_pf *pf)
 		case i40e_aqc_opc_send_msg_to_peer:
 			dev_info(&pf->pdev->dev, "ARQ: Msg from other pf\n");
 			break;
+		case i40e_aqc_opc_nvm_erase:
+		case i40e_aqc_opc_nvm_update:
+			i40e_debug(&pf->hw, I40E_DEBUG_NVM, "ARQ NVM operation completed\n");
+			break;
 		default:
 			dev_info(&pf->pdev->dev,
 				 "ARQ Error: Unknown event 0x%04x received\n",
@@ -5919,6 +5964,94 @@ static void i40e_verify_eeprom(struct i40e_pf *pf)
 }
 
 /**
+ * i40e_enable_pf_switch_lb
+ * @pf: pointer to the PF structure
+ *
+ * enable switch loop back or die - no point in a return value
+ **/
+static void i40e_enable_pf_switch_lb(struct i40e_pf *pf)
+{
+	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
+	struct i40e_vsi_context ctxt;
+	int aq_ret;
+
+	ctxt.seid = pf->main_vsi_seid;
+	ctxt.pf_num = pf->hw.pf_id;
+	ctxt.vf_num = 0;
+	aq_ret = i40e_aq_get_vsi_params(&pf->hw, &ctxt, NULL);
+	if (aq_ret) {
+		dev_info(&pf->pdev->dev,
+			 "%s couldn't get PF vsi config, err %d, aq_err %d\n",
+			 __func__, aq_ret, pf->hw.aq.asq_last_status);
+		return;
+	}
+	ctxt.flags = I40E_AQ_VSI_TYPE_PF;
+	ctxt.info.valid_sections = cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
+	ctxt.info.switch_id |= cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+
+	aq_ret = i40e_aq_update_vsi_params(&vsi->back->hw, &ctxt, NULL);
+	if (aq_ret) {
+		dev_info(&pf->pdev->dev,
+			 "%s: update vsi switch failed, aq_err=%d\n",
+			 __func__, vsi->back->hw.aq.asq_last_status);
+	}
+}
+
+/**
+ * i40e_disable_pf_switch_lb
+ * @pf: pointer to the PF structure
+ *
+ * disable switch loop back or die - no point in a return value
+ **/
+static void i40e_disable_pf_switch_lb(struct i40e_pf *pf)
+{
+	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
+	struct i40e_vsi_context ctxt;
+	int aq_ret;
+
+	ctxt.seid = pf->main_vsi_seid;
+	ctxt.pf_num = pf->hw.pf_id;
+	ctxt.vf_num = 0;
+	aq_ret = i40e_aq_get_vsi_params(&pf->hw, &ctxt, NULL);
+	if (aq_ret) {
+		dev_info(&pf->pdev->dev,
+			 "%s couldn't get PF vsi config, err %d, aq_err %d\n",
+			 __func__, aq_ret, pf->hw.aq.asq_last_status);
+		return;
+	}
+	ctxt.flags = I40E_AQ_VSI_TYPE_PF;
+	ctxt.info.valid_sections = cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
+	ctxt.info.switch_id &= ~cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+
+	aq_ret = i40e_aq_update_vsi_params(&vsi->back->hw, &ctxt, NULL);
+	if (aq_ret) {
+		dev_info(&pf->pdev->dev,
+			 "%s: update vsi switch failed, aq_err=%d\n",
+			 __func__, vsi->back->hw.aq.asq_last_status);
+	}
+}
+
+/**
+ * i40e_config_bridge_mode - Configure the HW bridge mode
+ * @veb: pointer to the bridge instance
+ *
+ * Configure the loop back mode for the LAN VSI that is downlink to the
+ * specified HW bridge instance. It is expected this function is called
+ * when a new HW bridge is instantiated.
+ **/
+static void i40e_config_bridge_mode(struct i40e_veb *veb)
+{
+	struct i40e_pf *pf = veb->pf;
+
+	dev_info(&pf->pdev->dev, "enabling bridge mode: %s\n",
+		 veb->bridge_mode == BRIDGE_MODE_VEPA ? "VEPA" : "VEB");
+	if (veb->bridge_mode & BRIDGE_MODE_VEPA)
+		i40e_disable_pf_switch_lb(pf);
+	else
+		i40e_enable_pf_switch_lb(pf);
+}
+
+/**
  * i40e_reconstitute_veb - rebuild the VEB and anything connected to it
  * @veb: pointer to the VEB instance
  *
@@ -5964,8 +6097,7 @@ static int i40e_reconstitute_veb(struct i40e_veb *veb)
 	if (ret)
 		goto end_reconstitute;
 
-	/* Enable LB mode for the main VSI now that it is on a VEB */
-	i40e_enable_pf_switch_lb(pf);
+	i40e_config_bridge_mode(veb);
 
 	/* create the remaining VSIs attached to this VEB */
 	for (v = 0; v < pf->num_alloc_vsi; v++) {
@@ -6137,7 +6269,7 @@ static void i40e_fdir_teardown(struct i40e_pf *pf)
  * i40e_prep_for_reset - prep for the core to reset
  * @pf: board private structure
  *
- * Close up the VFs and other things in prep for pf Reset.
+ * Close up the VFs and other things in prep for PF Reset.
   **/
 static void i40e_prep_for_reset(struct i40e_pf *pf)
 {
@@ -6222,10 +6354,8 @@ static void i40e_reset_and_rebuild(struct i40e_pf *pf, bool reinit)
 	}
 
 	/* re-verify the eeprom if we just had an EMP reset */
-	if (test_bit(__I40E_EMP_RESET_REQUESTED, &pf->state)) {
-		clear_bit(__I40E_EMP_RESET_REQUESTED, &pf->state);
+	if (test_and_clear_bit(__I40E_EMP_RESET_INTR_RECEIVED, &pf->state))
 		i40e_verify_eeprom(pf);
-	}
 
 	i40e_clear_pxe_mode(hw);
 	ret = i40e_get_capabilities(pf);
@@ -6335,13 +6465,14 @@ static void i40e_reset_and_rebuild(struct i40e_pf *pf, bool reinit)
 		}
 	}
 
-	msleep(75);
-	ret = i40e_aq_set_link_restart_an(&pf->hw, true, NULL);
-	if (ret) {
-		dev_info(&pf->pdev->dev, "link restart failed, aq_err=%d\n",
-			 pf->hw.aq.asq_last_status);
+	if (((pf->hw.aq.fw_maj_ver == 4) && (pf->hw.aq.fw_min_ver < 33)) ||
+	    (pf->hw.aq.fw_maj_ver < 4)) {
+		msleep(75);
+		ret = i40e_aq_set_link_restart_an(&pf->hw, true, NULL);
+		if (ret)
+			dev_info(&pf->pdev->dev, "link restart failed, aq_err=%d\n",
+				 pf->hw.aq.asq_last_status);
 	}
-
 	/* reinit the misc interrupt */
 	if (pf->flags & I40E_FLAG_MSIX_ENABLED)
 		ret = i40e_setup_misc_vector(pf);
@@ -6364,7 +6495,7 @@ clear_recovery:
 }
 
 /**
- * i40e_handle_reset_warning - prep for the pf to reset, reset and rebuild
+ * i40e_handle_reset_warning - prep for the PF to reset, reset and rebuild
  * @pf: board private structure
  *
  * Close up the VFs and other things in prep for a Core Reset,
@@ -6378,7 +6509,7 @@ static void i40e_handle_reset_warning(struct i40e_pf *pf)
 
 /**
  * i40e_handle_mdd_event
- * @pf: pointer to the pf structure
+ * @pf: pointer to the PF structure
  *
  * Called from the MDD irq handler to identify possibly malicious vfs
  **/
@@ -6407,7 +6538,7 @@ static void i40e_handle_mdd_event(struct i40e_pf *pf)
 				I40E_GL_MDET_TX_QUEUE_SHIFT) -
 				pf->hw.func_caps.base_queue;
 		if (netif_msg_tx_err(pf))
-			dev_info(&pf->pdev->dev, "Malicious Driver Detection event 0x%02x on TX queue %d pf number 0x%02x vf number 0x%02x\n",
+			dev_info(&pf->pdev->dev, "Malicious Driver Detection event 0x%02x on TX queue %d PF number 0x%02x VF number 0x%02x\n",
 				 event, queue, pf_num, vf_num);
 		wr32(hw, I40E_GL_MDET_TX, 0xffffffff);
 		mdd_detected = true;
@@ -6493,7 +6624,6 @@ static void i40e_sync_vxlan_filters_subtask(struct i40e_pf *pf)
 {
 	struct i40e_hw *hw = &pf->hw;
 	i40e_status ret;
-	u8 filter_index;
 	__be16 port;
 	int i;
 
@@ -6506,22 +6636,20 @@ static void i40e_sync_vxlan_filters_subtask(struct i40e_pf *pf)
 		if (pf->pending_vxlan_bitmap & (1 << i)) {
 			pf->pending_vxlan_bitmap &= ~(1 << i);
 			port = pf->vxlan_ports[i];
-			ret = port ?
-			      i40e_aq_add_udp_tunnel(hw, ntohs(port),
+			if (port)
+				ret = i40e_aq_add_udp_tunnel(hw, ntohs(port),
 						     I40E_AQC_TUNNEL_TYPE_VXLAN,
-						     &filter_index, NULL)
-			      : i40e_aq_del_udp_tunnel(hw, i, NULL);
+						     NULL, NULL);
+			else
+				ret = i40e_aq_del_udp_tunnel(hw, i, NULL);
 
 			if (ret) {
-				dev_info(&pf->pdev->dev, "Failed to execute AQ command for %s port %d with index %d\n",
-					 port ? "adding" : "deleting",
-					 ntohs(port), port ? i : i);
-
+				dev_info(&pf->pdev->dev,
+					 "%s vxlan port %d, index %d failed, err %d, aq_err %d\n",
+					 port ? "add" : "delete",
+					 ntohs(port), i, ret,
+					 pf->hw.aq.asq_last_status);
 				pf->vxlan_ports[i] = 0;
-			} else {
-				dev_info(&pf->pdev->dev, "%s port %d with AQ command with index %d\n",
-					 port ? "Added" : "Deleted",
-					 ntohs(port), port ? i : filter_index);
 			}
 		}
 	}
@@ -6728,6 +6856,8 @@ static int i40e_vsi_mem_alloc(struct i40e_pf *pf, enum i40e_vsi_type type)
 	vsi->idx = vsi_idx;
 	vsi->rx_itr_setting = pf->rx_itr_default;
 	vsi->tx_itr_setting = pf->tx_itr_default;
+	vsi->rss_table_size = (vsi->type == I40E_VSI_MAIN) ?
+				pf->rss_table_size : 64;
 	vsi->netdev_registered = false;
 	vsi->work_limit = I40E_DEFAULT_IRQ_WORK;
 	INIT_LIST_HEAD(&vsi->mac_filter_list);
@@ -6808,7 +6938,7 @@ static int i40e_vsi_clear(struct i40e_vsi *vsi)
 		goto unlock_vsi;
 	}
 
-	/* updates the pf for this cleared vsi */
+	/* updates the PF for this cleared vsi */
 	i40e_put_lump(pf->qp_pile, vsi->base_queue, vsi->idx);
 	i40e_put_lump(pf->irq_pile, vsi->base_vector, vsi->idx);
 
@@ -6921,15 +7051,14 @@ static int i40e_reserve_msix_vectors(struct i40e_pf *pf, int vectors)
  *
  * Work with the OS to set up the MSIX vectors needed.
  *
- * Returns 0 on success, negative on failure
+ * Returns the number of vectors reserved or negative on failure
  **/
 static int i40e_init_msix(struct i40e_pf *pf)
 {
-	i40e_status err = 0;
 	struct i40e_hw *hw = &pf->hw;
-	int other_vecs = 0;
+	int vectors_left;
 	int v_budget, i;
-	int vec;
+	int v_actual;
 
 	if (!(pf->flags & I40E_FLAG_MSIX_ENABLED))
 		return -ENODEV;
@@ -6951,24 +7080,62 @@ static int i40e_init_msix(struct i40e_pf *pf)
 	 * If we can't get what we want, we'll simplify to nearly nothing
 	 * and try again.  If that still fails, we punt.
 	 */
-	pf->num_lan_msix = pf->num_lan_qps - (pf->rss_size_max - pf->rss_size);
-	pf->num_vmdq_msix = pf->num_vmdq_qps;
-	other_vecs = 1;
-	other_vecs += (pf->num_vmdq_vsis * pf->num_vmdq_msix);
-	if (pf->flags & I40E_FLAG_FD_SB_ENABLED)
-		other_vecs++;
-
-	/* Scale down if necessary, and the rings will share vectors */
-	pf->num_lan_msix = min_t(int, pf->num_lan_msix,
-			(hw->func_caps.num_msix_vectors - other_vecs));
-	v_budget = pf->num_lan_msix + other_vecs;
+	vectors_left = hw->func_caps.num_msix_vectors;
+	v_budget = 0;
+
+	/* reserve one vector for miscellaneous handler */
+	if (vectors_left) {
+		v_budget++;
+		vectors_left--;
+	}
+
+	/* reserve vectors for the main PF traffic queues */
+	pf->num_lan_msix = min_t(int, num_online_cpus(), vectors_left);
+	vectors_left -= pf->num_lan_msix;
+	v_budget += pf->num_lan_msix;
+
+	/* reserve one vector for sideband flow director */
+	if (pf->flags & I40E_FLAG_FD_SB_ENABLED) {
+		if (vectors_left) {
+			v_budget++;
+			vectors_left--;
+		} else {
+			pf->flags &= ~I40E_FLAG_FD_SB_ENABLED;
+		}
+	}
 
 #ifdef I40E_FCOE
+	/* can we reserve enough for FCoE? */
 	if (pf->flags & I40E_FLAG_FCOE_ENABLED) {
-		pf->num_fcoe_msix = pf->num_fcoe_qps;
+		if (!vectors_left)
+			pf->num_fcoe_msix = 0;
+		else if (vectors_left >= pf->num_fcoe_qps)
+			pf->num_fcoe_msix = pf->num_fcoe_qps;
+		else
+			pf->num_fcoe_msix = 1;
 		v_budget += pf->num_fcoe_msix;
+		vectors_left -= pf->num_fcoe_msix;
 	}
+
 #endif
+	/* any vectors left over go for VMDq support */
+	if (pf->flags & I40E_FLAG_VMDQ_ENABLED) {
+		int vmdq_vecs_wanted = pf->num_vmdq_vsis * pf->num_vmdq_qps;
+		int vmdq_vecs = min_t(int, vectors_left, vmdq_vecs_wanted);
+
+		/* if we're short on vectors for what's desired, we limit
+		 * the queues per vmdq.  If this is still more than are
+		 * available, the user will need to change the number of
+		 * queues/vectors used by the PF later with the ethtool
+		 * channels command
+		 */
+		if (vmdq_vecs < vmdq_vecs_wanted)
+			pf->num_vmdq_qps = 1;
+		pf->num_vmdq_msix = pf->num_vmdq_qps;
+
+		v_budget += vmdq_vecs;
+		vectors_left -= vmdq_vecs;
+	}
 
 	pf->msix_entries = kcalloc(v_budget, sizeof(struct msix_entry),
 				   GFP_KERNEL);
@@ -6977,9 +7144,9 @@ static int i40e_init_msix(struct i40e_pf *pf)
 
 	for (i = 0; i < v_budget; i++)
 		pf->msix_entries[i].entry = i;
-	vec = i40e_reserve_msix_vectors(pf, v_budget);
+	v_actual = i40e_reserve_msix_vectors(pf, v_budget);
 
-	if (vec != v_budget) {
+	if (v_actual != v_budget) {
 		/* If we have limited resources, we will start with no vectors
 		 * for the special features and then allocate vectors to some
 		 * of these features based on the policy and at the end disable
@@ -6992,26 +7159,30 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		pf->num_vmdq_msix = 0;
 	}
 
-	if (vec < I40E_MIN_MSIX) {
+	if (v_actual < I40E_MIN_MSIX) {
 		pf->flags &= ~I40E_FLAG_MSIX_ENABLED;
 		kfree(pf->msix_entries);
 		pf->msix_entries = NULL;
 		return -ENODEV;
 
-	} else if (vec == I40E_MIN_MSIX) {
+	} else if (v_actual == I40E_MIN_MSIX) {
 		/* Adjust for minimal MSIX use */
 		pf->num_vmdq_vsis = 0;
 		pf->num_vmdq_qps = 0;
 		pf->num_lan_qps = 1;
 		pf->num_lan_msix = 1;
 
-	} else if (vec != v_budget) {
+	} else if (v_actual != v_budget) {
+		int vec;
+
 		/* reserve the misc vector */
-		vec--;
+		vec = v_actual - 1;
 
 		/* Scale vector usage down */
 		pf->num_vmdq_msix = 1;    /* force VMDqs to only one vector */
 		pf->num_vmdq_vsis = 1;
+		pf->num_vmdq_qps = 1;
+		pf->flags &= ~I40E_FLAG_FD_SB_ENABLED;
 
 		/* partition out the remaining vectors */
 		switch (vec) {
@@ -7037,10 +7208,8 @@ static int i40e_init_msix(struct i40e_pf *pf)
 				vec--;
 			}
 #endif
-			pf->num_lan_msix = min_t(int, (vec / 2),
-						 pf->num_lan_qps);
-			pf->num_vmdq_vsis = min_t(int, (vec - pf->num_lan_msix),
-						  I40E_DEFAULT_NUM_VMDQ_VSI);
+			/* give the rest to the PF */
+			pf->num_lan_msix = min_t(int, vec, pf->num_lan_qps);
 			break;
 		}
 	}
@@ -7057,7 +7226,7 @@ static int i40e_init_msix(struct i40e_pf *pf)
 		pf->flags &= ~I40E_FLAG_FCOE_ENABLED;
 	}
 #endif
-	return err;
+	return v_actual;
 }
 
 /**
@@ -7134,11 +7303,12 @@ err_out:
  **/
 static void i40e_init_interrupt_scheme(struct i40e_pf *pf)
 {
-	int err = 0;
+	int vectors = 0;
+	ssize_t size;
 
 	if (pf->flags & I40E_FLAG_MSIX_ENABLED) {
-		err = i40e_init_msix(pf);
-		if (err) {
+		vectors = i40e_init_msix(pf);
+		if (vectors < 0) {
 			pf->flags &= ~(I40E_FLAG_MSIX_ENABLED	|
 #ifdef I40E_FCOE
 				       I40E_FLAG_FCOE_ENABLED	|
@@ -7158,18 +7328,26 @@ static void i40e_init_interrupt_scheme(struct i40e_pf *pf)
 	if (!(pf->flags & I40E_FLAG_MSIX_ENABLED) &&
 	    (pf->flags & I40E_FLAG_MSI_ENABLED)) {
 		dev_info(&pf->pdev->dev, "MSI-X not available, trying MSI\n");
-		err = pci_enable_msi(pf->pdev);
-		if (err) {
-			dev_info(&pf->pdev->dev, "MSI init failed - %d\n", err);
+		vectors = pci_enable_msi(pf->pdev);
+		if (vectors < 0) {
+			dev_info(&pf->pdev->dev, "MSI init failed - %d\n",
+				 vectors);
 			pf->flags &= ~I40E_FLAG_MSI_ENABLED;
 		}
+		vectors = 1;  /* one MSI or Legacy vector */
 	}
 
 	if (!(pf->flags & (I40E_FLAG_MSIX_ENABLED | I40E_FLAG_MSI_ENABLED)))
 		dev_info(&pf->pdev->dev, "MSI-X and MSI not available, falling back to Legacy IRQ\n");
 
+	/* set up vector assignment tracking */
+	size = sizeof(struct i40e_lump_tracking) + (sizeof(u16) * vectors);
+	pf->irq_pile = kzalloc(size, GFP_KERNEL);
+	pf->irq_pile->num_entries = vectors;
+	pf->irq_pile->search_hint = 0;
+
 	/* track first vector for misc interrupts */
-	err = i40e_get_lump(pf, pf->irq_pile, 1, I40E_PILE_VALID_BIT-1);
+	(void)i40e_get_lump(pf, pf->irq_pile, 1, I40E_PILE_VALID_BIT - 1);
 }
 
 /**
@@ -7219,6 +7397,7 @@ static int i40e_setup_misc_vector(struct i40e_pf *pf)
 static int i40e_config_rss(struct i40e_pf *pf)
 {
 	u32 rss_key[I40E_PFQF_HKEY_MAX_INDEX + 1];
+	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
 	struct i40e_hw *hw = &pf->hw;
 	u32 lut = 0;
 	int i, j;
@@ -7236,15 +7415,14 @@ static int i40e_config_rss(struct i40e_pf *pf)
 	wr32(hw, I40E_PFQF_HENA(0), (u32)hena);
 	wr32(hw, I40E_PFQF_HENA(1), (u32)(hena >> 32));
 
+	vsi->rss_size = min_t(int, pf->rss_size, vsi->num_queue_pairs);
+
 	/* Check capability and Set table size and register per hw expectation*/
 	reg_val = rd32(hw, I40E_PFQF_CTL_0);
-	if (hw->func_caps.rss_table_size == 512) {
+	if (pf->rss_table_size == 512)
 		reg_val |= I40E_PFQF_CTL_0_HASHLUTSIZE_512;
-		pf->rss_table_size = 512;
-	} else {
-		pf->rss_table_size = 128;
+	else
 		reg_val &= ~I40E_PFQF_CTL_0_HASHLUTSIZE_512;
-	}
 	wr32(hw, I40E_PFQF_CTL_0, reg_val);
 
 	/* Populate the LUT with max no. of queues in round robin fashion */
@@ -7257,7 +7435,7 @@ static int i40e_config_rss(struct i40e_pf *pf)
 		 * If LAN VSI is the only consumer for RSS then this requirement
 		 * is not necessary.
 		 */
-		if (j == pf->rss_size)
+		if (j == vsi->rss_size)
 			j = 0;
 		/* lut = 4-byte sliding window of 4 lut entries */
 		lut = (lut << 8) | (j &
@@ -7281,15 +7459,19 @@ static int i40e_config_rss(struct i40e_pf *pf)
  **/
 int i40e_reconfig_rss_queues(struct i40e_pf *pf, int queue_count)
 {
+	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
+	int new_rss_size;
+
 	if (!(pf->flags & I40E_FLAG_RSS_ENABLED))
 		return 0;
 
-	queue_count = min_t(int, queue_count, pf->rss_size_max);
+	new_rss_size = min_t(int, queue_count, pf->rss_size_max);
 
-	if (queue_count != pf->rss_size) {
+	if (queue_count != vsi->num_queue_pairs) {
+		vsi->req_queue_pairs = queue_count;
 		i40e_prep_for_reset(pf);
 
-		pf->rss_size = queue_count;
+		pf->rss_size = new_rss_size;
 
 		i40e_reset_and_rebuild(pf, true);
 		i40e_config_rss(pf);
@@ -7299,6 +7481,128 @@ int i40e_reconfig_rss_queues(struct i40e_pf *pf, int queue_count)
 }
 
 /**
+ * i40e_get_npar_bw_setting - Retrieve BW settings for this PF partition
+ * @pf: board private structure
+ **/
+i40e_status i40e_get_npar_bw_setting(struct i40e_pf *pf)
+{
+	i40e_status status;
+	bool min_valid, max_valid;
+	u32 max_bw, min_bw;
+
+	status = i40e_read_bw_from_alt_ram(&pf->hw, &max_bw, &min_bw,
+					   &min_valid, &max_valid);
+
+	if (!status) {
+		if (min_valid)
+			pf->npar_min_bw = min_bw;
+		if (max_valid)
+			pf->npar_max_bw = max_bw;
+	}
+
+	return status;
+}
+
+/**
+ * i40e_set_npar_bw_setting - Set BW settings for this PF partition
+ * @pf: board private structure
+ **/
+i40e_status i40e_set_npar_bw_setting(struct i40e_pf *pf)
+{
+	struct i40e_aqc_configure_partition_bw_data bw_data;
+	i40e_status status;
+
+	/* Set the valid bit for this PF */
+	bw_data.pf_valid_bits = cpu_to_le16(1 << pf->hw.pf_id);
+	bw_data.max_bw[pf->hw.pf_id] = pf->npar_max_bw & I40E_ALT_BW_VALUE_MASK;
+	bw_data.min_bw[pf->hw.pf_id] = pf->npar_min_bw & I40E_ALT_BW_VALUE_MASK;
+
+	/* Set the new bandwidths */
+	status = i40e_aq_configure_partition_bw(&pf->hw, &bw_data, NULL);
+
+	return status;
+}
+
+/**
+ * i40e_commit_npar_bw_setting - Commit BW settings for this PF partition
+ * @pf: board private structure
+ **/
+i40e_status i40e_commit_npar_bw_setting(struct i40e_pf *pf)
+{
+	/* Commit temporary BW setting to permanent NVM image */
+	enum i40e_admin_queue_err last_aq_status;
+	i40e_status ret;
+	u16 nvm_word;
+
+	if (pf->hw.partition_id != 1) {
+		dev_info(&pf->pdev->dev,
+			 "Commit BW only works on partition 1! This is partition %d",
+			 pf->hw.partition_id);
+		ret = I40E_NOT_SUPPORTED;
+		goto bw_commit_out;
+	}
+
+	/* Acquire NVM for read access */
+	ret = i40e_acquire_nvm(&pf->hw, I40E_RESOURCE_READ);
+	last_aq_status = pf->hw.aq.asq_last_status;
+	if (ret) {
+		dev_info(&pf->pdev->dev,
+			 "Cannot acquire NVM for read access, err %d: aq_err %d\n",
+			 ret, last_aq_status);
+		goto bw_commit_out;
+	}
+
+	/* Read word 0x10 of NVM - SW compatibility word 1 */
+	ret = i40e_aq_read_nvm(&pf->hw,
+			       I40E_SR_NVM_CONTROL_WORD,
+			       0x10, sizeof(nvm_word), &nvm_word,
+			       false, NULL);
+	/* Save off last admin queue command status before releasing
+	 * the NVM
+	 */
+	last_aq_status = pf->hw.aq.asq_last_status;
+	i40e_release_nvm(&pf->hw);
+	if (ret) {
+		dev_info(&pf->pdev->dev, "NVM read error, err %d aq_err %d\n",
+			 ret, last_aq_status);
+		goto bw_commit_out;
+	}
+
+	/* Wait a bit for NVM release to complete */
+	msleep(50);
+
+	/* Acquire NVM for write access */
+	ret = i40e_acquire_nvm(&pf->hw, I40E_RESOURCE_WRITE);
+	last_aq_status = pf->hw.aq.asq_last_status;
+	if (ret) {
+		dev_info(&pf->pdev->dev,
+			 "Cannot acquire NVM for write access, err %d: aq_err %d\n",
+			 ret, last_aq_status);
+		goto bw_commit_out;
+	}
+	/* Write it back out unchanged to initiate update NVM,
+	 * which will force a write of the shadow (alt) RAM to
+	 * the NVM - thus storing the bandwidth values permanently.
+	 */
+	ret = i40e_aq_update_nvm(&pf->hw,
+				 I40E_SR_NVM_CONTROL_WORD,
+				 0x10, sizeof(nvm_word),
+				 &nvm_word, true, NULL);
+	/* Save off last admin queue command status before releasing
+	 * the NVM
+	 */
+	last_aq_status = pf->hw.aq.asq_last_status;
+	i40e_release_nvm(&pf->hw);
+	if (ret)
+		dev_info(&pf->pdev->dev,
+			 "BW settings NOT SAVED, err %d aq_err %d\n",
+			 ret, last_aq_status);
+bw_commit_out:
+
+	return ret;
+}
+
+/**
  * i40e_sw_init - Initialize general software structures (struct i40e_pf)
  * @pf: board private structure to initialize
  *
@@ -7324,8 +7628,12 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	/* Set default capability flags */
 	pf->flags = I40E_FLAG_RX_CSUM_ENABLED |
 		    I40E_FLAG_MSI_ENABLED     |
-		    I40E_FLAG_MSIX_ENABLED    |
-		    I40E_FLAG_RX_1BUF_ENABLED;
+		    I40E_FLAG_MSIX_ENABLED;
+
+	if (iommu_present(&pci_bus_type))
+		pf->flags |= I40E_FLAG_RX_PS_ENABLED;
+	else
+		pf->flags |= I40E_FLAG_RX_1BUF_ENABLED;
 
 	/* Set default ITR */
 	pf->rx_itr_default = I40E_ITR_DYNAMIC | I40E_ITR_RX_DEF;
@@ -7336,6 +7644,7 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	 */
 	pf->rss_size_max = 0x1 << pf->hw.func_caps.rss_table_entry_width;
 	pf->rss_size = 1;
+	pf->rss_table_size = pf->hw.func_caps.rss_table_size;
 	pf->rss_size_max = min_t(int, pf->rss_size_max,
 				 pf->hw.func_caps.num_tx_qp);
 	if (pf->hw.func_caps.rss) {
@@ -7347,6 +7656,13 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	if (pf->hw.func_caps.npar_enable || pf->hw.func_caps.mfp_mode_1) {
 		pf->flags |= I40E_FLAG_MFP_ENABLED;
 		dev_info(&pf->pdev->dev, "MFP mode Enabled\n");
+		if (i40e_get_npar_bw_setting(pf))
+			dev_warn(&pf->pdev->dev,
+				 "Could not get NPAR bw settings\n");
+		else
+			dev_info(&pf->pdev->dev,
+				 "Min BW = %8.8x, Max BW = %8.8x\n",
+				 pf->npar_min_bw, pf->npar_max_bw);
 	}
 
 	/* FW/NVM is not yet fixed in this regard */
@@ -7354,11 +7670,11 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	    (pf->hw.func_caps.fd_filters_best_effort > 0)) {
 		pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
 		pf->atr_sample_rate = I40E_DEFAULT_ATR_SAMPLE_RATE;
-		/* Setup a counter for fd_atr per pf */
+		/* Setup a counter for fd_atr per PF */
 		pf->fd_atr_cnt_idx = I40E_FD_ATR_STAT_IDX(pf->hw.pf_id);
 		if (!(pf->flags & I40E_FLAG_MFP_ENABLED)) {
 			pf->flags |= I40E_FLAG_FD_SB_ENABLED;
-			/* Setup a counter for fd_sb per pf */
+			/* Setup a counter for fd_sb per PF */
 			pf->fd_sb_cnt_idx = I40E_FD_SB_STAT_IDX(pf->hw.pf_id);
 		} else {
 			dev_info(&pf->pdev->dev,
@@ -7406,22 +7722,14 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	pf->qp_pile->num_entries = pf->hw.func_caps.num_tx_qp;
 	pf->qp_pile->search_hint = 0;
 
-	/* set up vector assignment tracking */
-	size = sizeof(struct i40e_lump_tracking)
-		+ (sizeof(u16) * pf->hw.func_caps.num_msix_vectors);
-	pf->irq_pile = kzalloc(size, GFP_KERNEL);
-	if (!pf->irq_pile) {
-		kfree(pf->qp_pile);
-		err = -ENOMEM;
-		goto sw_init_done;
-	}
-	pf->irq_pile->num_entries = pf->hw.func_caps.num_msix_vectors;
-	pf->irq_pile->search_hint = 0;
-
 	pf->tx_timeout_recovery_level = 1;
 
 	mutex_init(&pf->switch_mutex);
 
+	/* If NPAR is enabled nudge the Tx scheduler */
+	if (pf->hw.func_caps.npar_enable && (!i40e_get_npar_bw_setting(pf)))
+		i40e_set_npar_bw_setting(pf);
+
 sw_init_done:
 	return err;
 }
@@ -7534,7 +7842,8 @@ static void i40e_add_vxlan_port(struct net_device *netdev,
 
 	/* Check if port already exists */
 	if (idx < I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
-		netdev_info(netdev, "Port %d already offloaded\n", ntohs(port));
+		netdev_info(netdev, "vxlan port %d already offloaded\n",
+			    ntohs(port));
 		return;
 	}
 
@@ -7542,7 +7851,7 @@ static void i40e_add_vxlan_port(struct net_device *netdev,
 	next_idx = i40e_get_vxlan_port_idx(pf, 0);
 
 	if (next_idx == I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
-		netdev_info(netdev, "Maximum number of UDP ports reached, not adding port %d\n",
+		netdev_info(netdev, "maximum number of vxlan UDP ports reached, not adding port %d\n",
 			    ntohs(port));
 		return;
 	}
@@ -7550,8 +7859,9 @@ static void i40e_add_vxlan_port(struct net_device *netdev,
 	/* New port: add it and mark its index in the bitmap */
 	pf->vxlan_ports[next_idx] = port;
 	pf->pending_vxlan_bitmap |= (1 << next_idx);
-
 	pf->flags |= I40E_FLAG_VXLAN_FILTER_SYNC;
+
+	dev_info(&pf->pdev->dev, "adding vxlan port %d\n", ntohs(port));
 }
 
 /**
@@ -7579,12 +7889,13 @@ static void i40e_del_vxlan_port(struct net_device *netdev,
 		 * and make it pending
 		 */
 		pf->vxlan_ports[idx] = 0;
-
 		pf->pending_vxlan_bitmap |= (1 << idx);
-
 		pf->flags |= I40E_FLAG_VXLAN_FILTER_SYNC;
+
+		dev_info(&pf->pdev->dev, "deleting vxlan port %d\n",
+			 ntohs(port));
 	} else {
-		netdev_warn(netdev, "Port %d was not found, not deleting\n",
+		netdev_warn(netdev, "vxlan port %d was not found, not deleting\n",
 			    ntohs(port));
 	}
 }
@@ -7653,6 +7964,118 @@ static int i40e_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	return err;
 }
 
+#ifdef HAVE_BRIDGE_ATTRIBS
+/**
+ * i40e_ndo_bridge_setlink - Set the hardware bridge mode
+ * @dev: the netdev being configured
+ * @nlh: RTNL message
+ *
+ * Inserts a new hardware bridge if not already created and
+ * enables the bridging mode requested (VEB or VEPA). If the
+ * hardware bridge has already been inserted and the request
+ * is to change the mode then that requires a PF reset to
+ * allow rebuild of the components with required hardware
+ * bridge mode enabled.
+ **/
+static int i40e_ndo_bridge_setlink(struct net_device *dev,
+				   struct nlmsghdr *nlh)
+{
+	struct i40e_netdev_priv *np = netdev_priv(dev);
+	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	struct i40e_veb *veb = NULL;
+	struct nlattr *attr, *br_spec;
+	int i, rem;
+
+	/* Only for PF VSI for now */
+	if (vsi->seid != pf->vsi[pf->lan_vsi]->seid)
+		return -EOPNOTSUPP;
+
+	/* Find the HW bridge for PF VSI */
+	for (i = 0; i < I40E_MAX_VEB && !veb; i++) {
+		if (pf->veb[i] && pf->veb[i]->seid == vsi->uplink_seid)
+			veb = pf->veb[i];
+	}
+
+	br_spec = nlmsg_find_attr(nlh, sizeof(struct ifinfomsg), IFLA_AF_SPEC);
+
+	nla_for_each_nested(attr, br_spec, rem) {
+		__u16 mode;
+
+		if (nla_type(attr) != IFLA_BRIDGE_MODE)
+			continue;
+
+		mode = nla_get_u16(attr);
+		if ((mode != BRIDGE_MODE_VEPA) &&
+		    (mode != BRIDGE_MODE_VEB))
+			return -EINVAL;
+
+		/* Insert a new HW bridge */
+		if (!veb) {
+			veb = i40e_veb_setup(pf, 0, vsi->uplink_seid, vsi->seid,
+					     vsi->tc_config.enabled_tc);
+			if (veb) {
+				veb->bridge_mode = mode;
+				i40e_config_bridge_mode(veb);
+			} else {
+				/* No Bridge HW offload available */
+				return -ENOENT;
+			}
+			break;
+		} else if (mode != veb->bridge_mode) {
+			/* Existing HW bridge but different mode needs reset */
+			veb->bridge_mode = mode;
+			i40e_do_reset(pf, (1 << __I40E_PF_RESET_REQUESTED));
+			break;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * i40e_ndo_bridge_getlink - Get the hardware bridge mode
+ * @skb: skb buff
+ * @pid: process id
+ * @seq: RTNL message seq #
+ * @dev: the netdev being configured
+ * @filter_mask: unused
+ *
+ * Return the mode in which the hardware bridge is operating in
+ * i.e VEB or VEPA.
+ **/
+#ifdef HAVE_BRIDGE_FILTER
+static int i40e_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
+				   struct net_device *dev,
+				   u32 __always_unused filter_mask)
+#else
+static int i40e_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
+				   struct net_device *dev)
+#endif /* HAVE_BRIDGE_FILTER */
+{
+	struct i40e_netdev_priv *np = netdev_priv(dev);
+	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	struct i40e_veb *veb = NULL;
+	int i;
+
+	/* Only for PF VSI for now */
+	if (vsi->seid != pf->vsi[pf->lan_vsi]->seid)
+		return -EOPNOTSUPP;
+
+	/* Find the HW bridge for the PF VSI */
+	for (i = 0; i < I40E_MAX_VEB && !veb; i++) {
+		if (pf->veb[i] && pf->veb[i]->seid == vsi->uplink_seid)
+			veb = pf->veb[i];
+	}
+
+	if (!veb)
+		return 0;
+
+	return ndo_dflt_bridge_getlink(skb, pid, seq, dev, veb->bridge_mode);
+}
+#endif /* HAVE_BRIDGE_ATTRIBS */
+
 static const struct net_device_ops i40e_netdev_ops = {
 	.ndo_open		= i40e_open,
 	.ndo_stop		= i40e_close,
@@ -7687,6 +8110,10 @@ static const struct net_device_ops i40e_netdev_ops = {
 #endif
 	.ndo_get_phys_port_id	= i40e_get_phys_port_id,
 	.ndo_fdb_add		= i40e_ndo_fdb_add,
+#ifdef HAVE_BRIDGE_ATTRIBS
+	.ndo_bridge_getlink	= i40e_ndo_bridge_getlink,
+	.ndo_bridge_setlink	= i40e_ndo_bridge_setlink,
+#endif /* HAVE_BRIDGE_ATTRIBS */
 };
 
 /**
@@ -7799,6 +8226,30 @@ static void i40e_vsi_delete(struct i40e_vsi *vsi)
 }
 
 /**
+ * i40e_is_vsi_uplink_mode_veb - Check if the VSI's uplink bridge mode is VEB
+ * @vsi: the VSI being queried
+ *
+ * Returns 1 if HW bridge mode is VEB and return 0 in case of VEPA mode
+ **/
+int i40e_is_vsi_uplink_mode_veb(struct i40e_vsi *vsi)
+{
+	struct i40e_veb *veb;
+	struct i40e_pf *pf = vsi->back;
+
+	/* Uplink is not a bridge so default to VEB */
+	if (vsi->veb_idx == I40E_NO_VEB)
+		return 1;
+
+	veb = pf->veb[vsi->veb_idx];
+	/* Uplink is a bridge in VEPA mode */
+	if (veb && (veb->bridge_mode & BRIDGE_MODE_VEPA))
+		return 0;
+
+	/* Uplink is a bridge in VEB mode */
+	return 1;
+}
+
+/**
  * i40e_add_vsi - Add a VSI to the switch
  * @vsi: the VSI being configured
  *
@@ -7830,11 +8281,11 @@ static int i40e_add_vsi(struct i40e_vsi *vsi)
 		ctxt.flags = I40E_AQ_VSI_TYPE_PF;
 		if (ret) {
 			dev_info(&pf->pdev->dev,
-				 "couldn't get pf vsi config, err %d, aq_err %d\n",
+				 "couldn't get PF vsi config, err %d, aq_err %d\n",
 				 ret, pf->hw.aq.asq_last_status);
 			return -ENOENT;
 		}
-		memcpy(&vsi->info, &ctxt.info, sizeof(ctxt.info));
+		vsi->info = ctxt.info;
 		vsi->info.valid_sections = 0;
 
 		vsi->seid = ctxt.seid;
@@ -7883,12 +8334,14 @@ static int i40e_add_vsi(struct i40e_vsi *vsi)
 		ctxt.pf_num = hw->pf_id;
 		ctxt.vf_num = 0;
 		ctxt.uplink_seid = vsi->uplink_seid;
-		ctxt.connection_type = 0x1;     /* regular data port */
+		ctxt.connection_type = I40E_AQ_VSI_CONN_TYPE_NORMAL;
 		ctxt.flags = I40E_AQ_VSI_TYPE_PF;
-		ctxt.info.valid_sections |=
+		if (i40e_is_vsi_uplink_mode_veb(vsi)) {
+			ctxt.info.valid_sections |=
 				cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
-		ctxt.info.switch_id =
+			ctxt.info.switch_id =
 				cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+		}
 		i40e_vsi_setup_queue_map(vsi, &ctxt, enabled_tc, true);
 		break;
 
@@ -7896,16 +8349,18 @@ static int i40e_add_vsi(struct i40e_vsi *vsi)
 		ctxt.pf_num = hw->pf_id;
 		ctxt.vf_num = 0;
 		ctxt.uplink_seid = vsi->uplink_seid;
-		ctxt.connection_type = 0x1;     /* regular data port */
+		ctxt.connection_type = I40E_AQ_VSI_CONN_TYPE_NORMAL;
 		ctxt.flags = I40E_AQ_VSI_TYPE_VMDQ2;
 
-		ctxt.info.valid_sections |= cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
-
 		/* This VSI is connected to VEB so the switch_id
 		 * should be set to zero by default.
 		 */
-		ctxt.info.switch_id = 0;
-		ctxt.info.switch_id |= cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+		if (i40e_is_vsi_uplink_mode_veb(vsi)) {
+			ctxt.info.valid_sections |=
+				cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
+			ctxt.info.switch_id =
+				cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+		}
 
 		/* Setup the VSI tx/rx queue map for TC0 only for now */
 		i40e_vsi_setup_queue_map(vsi, &ctxt, enabled_tc, true);
@@ -7915,15 +8370,18 @@ static int i40e_add_vsi(struct i40e_vsi *vsi)
 		ctxt.pf_num = hw->pf_id;
 		ctxt.vf_num = vsi->vf_id + hw->func_caps.vf_base_id;
 		ctxt.uplink_seid = vsi->uplink_seid;
-		ctxt.connection_type = 0x1;     /* regular data port */
+		ctxt.connection_type = I40E_AQ_VSI_CONN_TYPE_NORMAL;
 		ctxt.flags = I40E_AQ_VSI_TYPE_VF;
 
-		ctxt.info.valid_sections |= cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
-
 		/* This VSI is connected to VEB so the switch_id
 		 * should be set to zero by default.
 		 */
-		ctxt.info.switch_id = cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+		if (i40e_is_vsi_uplink_mode_veb(vsi)) {
+			ctxt.info.valid_sections |=
+				cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
+			ctxt.info.switch_id =
+				cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+		}
 
 		ctxt.info.valid_sections |= cpu_to_le16(I40E_AQ_VSI_PROP_VLAN_VALID);
 		ctxt.info.port_vlan_flags |= I40E_AQ_VSI_PVLAN_MODE_ALL;
@@ -7961,7 +8419,7 @@ static int i40e_add_vsi(struct i40e_vsi *vsi)
 			ret = -ENOENT;
 			goto err;
 		}
-		memcpy(&vsi->info, &ctxt.info, sizeof(ctxt.info));
+		vsi->info = ctxt.info;
 		vsi->info.valid_sections = 0;
 		vsi->seid = ctxt.seid;
 		vsi->id = ctxt.vsi_number;
@@ -8281,7 +8739,7 @@ struct i40e_vsi *i40e_vsi_setup(struct i40e_pf *pf, u8 type,
 					 __func__);
 				return NULL;
 			}
-			i40e_enable_pf_switch_lb(pf);
+			i40e_config_bridge_mode(veb);
 		}
 		for (i = 0; i < I40E_MAX_VEB && !veb; i++) {
 			if (pf->veb[i] && pf->veb[i]->seid == vsi->uplink_seid)
@@ -8724,7 +9182,7 @@ err_alloc:
 }
 
 /**
- * i40e_setup_pf_switch_element - set pf vars based on switch type
+ * i40e_setup_pf_switch_element - set PF vars based on switch type
  * @pf: board private structure
  * @ele: element we are building info from
  * @num_reported: total number of elements
@@ -8930,15 +9388,7 @@ static int i40e_setup_pf_switch(struct i40e_pf *pf, bool reinit)
 		i40e_config_rss(pf);
 
 	/* fill in link information and enable LSE reporting */
-	i40e_update_link_info(&pf->hw, true);
-	i40e_link_event(pf);
-
-	/* Initialize user-specific link properties */
-	pf->fc_autoneg_status = ((pf->hw.phy.link_info.an_info &
-				  I40E_AQ_AN_COMPLETED) ? true : false);
-
-	/* fill in link information and enable LSE reporting */
-	i40e_update_link_info(&pf->hw, true);
+	i40e_aq_get_link_info(&pf->hw, true, NULL, NULL);
 	i40e_link_event(pf);
 
 	/* Initialize user-specific link properties */
@@ -9008,7 +9458,11 @@ static void i40e_determine_queue_usage(struct i40e_pf *pf)
 			pf->flags &= ~I40E_FLAG_DCB_CAPABLE;
 			dev_info(&pf->pdev->dev, "not enough queues for DCB. DCB is disabled.\n");
 		}
-		pf->num_lan_qps = pf->rss_size_max;
+		pf->num_lan_qps = max_t(int, pf->rss_size_max,
+					num_online_cpus());
+		pf->num_lan_qps = min_t(int, pf->num_lan_qps,
+					pf->hw.func_caps.num_tx_qp);
+
 		queues_left -= pf->num_lan_qps;
 	}
 
@@ -9061,7 +9515,7 @@ static void i40e_determine_queue_usage(struct i40e_pf *pf)
  * i40e_setup_pf_filter_control - Setup PF static filter control
  * @pf: PF to be setup
  *
- * i40e_setup_pf_filter_control sets up a pf's initial filter control
+ * i40e_setup_pf_filter_control sets up a PF's initial filter control
  * settings. If PE/FCoE are enabled then it will also set the per PF
  * based filter sizes required for them. It also enables Flow director,
  * ethertype and macvlan type filter settings for the pf.
@@ -9106,8 +9560,10 @@ static void i40e_print_features(struct i40e_pf *pf)
 #ifdef CONFIG_PCI_IOV
 	buf += sprintf(buf, "VFs: %d ", pf->num_req_vfs);
 #endif
-	buf += sprintf(buf, "VSIs: %d QP: %d ", pf->hw.func_caps.num_vsis,
-		       pf->vsi[pf->lan_vsi]->num_queue_pairs);
+	buf += sprintf(buf, "VSIs: %d QP: %d RX: %s ",
+		       pf->hw.func_caps.num_vsis,
+		       pf->vsi[pf->lan_vsi]->num_queue_pairs,
+		       pf->flags & I40E_FLAG_RX_PS_ENABLED ? "PS" : "1BUF");
 
 	if (pf->flags & I40E_FLAG_RSS_ENABLED)
 		buf += sprintf(buf, "RSS ");
@@ -9136,14 +9592,16 @@ static void i40e_print_features(struct i40e_pf *pf)
  * @pdev: PCI device information struct
  * @ent: entry in i40e_pci_tbl
  *
- * i40e_probe initializes a pf identified by a pci_dev structure.
- * The OS initialization, configuring of the pf private structure,
+ * i40e_probe initializes a PF identified by a pci_dev structure.
+ * The OS initialization, configuring of the PF private structure,
  * and a hardware reset occur.
  *
  * Returns 0 on success, negative on failure
  **/
 static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
+	struct i40e_aq_get_phy_abilities_resp abilities;
+	unsigned long ioremap_len;
 	struct i40e_pf *pf;
 	struct i40e_hw *hw;
 	static u16 pfs_found;
@@ -9195,8 +9653,11 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	hw = &pf->hw;
 	hw->back = pf;
-	hw->hw_addr = ioremap(pci_resource_start(pdev, 0),
-			      pci_resource_len(pdev, 0));
+
+	ioremap_len = min_t(unsigned long, pci_resource_len(pdev, 0),
+			    I40E_MAX_CSR_SPACE);
+
+	hw->hw_addr = ioremap(pci_resource_start(pdev, 0), ioremap_len);
 	if (!hw->hw_addr) {
 		err = -EIO;
 		dev_info(&pdev->dev, "ioremap(0x%04x, 0x%04x) failed: 0x%x\n",
@@ -9274,7 +9735,6 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev_info(&pdev->dev,
 			 "The driver for the device detected an older version of the NVM image than expected. Please update the NVM image.\n");
 
-
 	i40e_verify_eeprom(pf);
 
 	/* Rev 0 hardware was never productized */
@@ -9409,13 +9869,14 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (err)
 		dev_info(&pf->pdev->dev, "set phy mask fail, aq_err %d\n", err);
 
-	msleep(75);
-	err = i40e_aq_set_link_restart_an(&pf->hw, true, NULL);
-	if (err) {
-		dev_info(&pf->pdev->dev, "link restart failed, aq_err=%d\n",
-			 pf->hw.aq.asq_last_status);
+	if (((pf->hw.aq.fw_maj_ver == 4) && (pf->hw.aq.fw_min_ver < 33)) ||
+	    (pf->hw.aq.fw_maj_ver < 4)) {
+		msleep(75);
+		err = i40e_aq_set_link_restart_an(&pf->hw, true, NULL);
+		if (err)
+			dev_info(&pf->pdev->dev, "link restart failed, aq_err=%d\n",
+				 pf->hw.aq.asq_last_status);
 	}
-
 	/* The main driver is (mostly) up and happy. We need to set this state
 	 * before setting up the misc vector or we get a race and the vector
 	 * ends up disabled forever.
@@ -9499,6 +9960,13 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev_warn(&pdev->dev, "Please move the device to a different PCI-e link with more lanes and/or higher transfer rate.\n");
 	}
 
+	/* get the requested speeds from the fw */
+	err = i40e_aq_get_phy_capabilities(hw, false, false, &abilities, NULL);
+	if (err)
+		dev_info(&pf->pdev->dev, "get phy abilities failed, aq_err %d, advertised speed settings may not be correct\n",
+			 err);
+	pf->hw.phy.link_info.requested_speeds = abilities.link_speed;
+
 	/* print a string summarizing features */
 	i40e_print_features(pf);
 
@@ -9517,7 +9985,6 @@ err_configure_lan_hmc:
 	(void)i40e_shutdown_lan_hmc(hw);
 err_init_lan_hmc:
 	kfree(pf->qp_pile);
-	kfree(pf->irq_pile);
 err_sw_init:
 err_adminq_setup:
 	(void)i40e_shutdown_adminq(hw);
@@ -9617,7 +10084,6 @@ static void i40e_remove(struct pci_dev *pdev)
 	}
 
 	kfree(pf->qp_pile);
-	kfree(pf->irq_pile);
 	kfree(pf->vsi);
 
 	iounmap(pf->hw.hw_addr);
@@ -9760,6 +10226,8 @@ static int i40e_suspend(struct pci_dev *pdev, pm_message_t state)
 	set_bit(__I40E_DOWN, &pf->state);
 	del_timer_sync(&pf->service_timer);
 	cancel_work_sync(&pf->service_task);
+	i40e_fdir_teardown(pf);
+
 	rtnl_lock();
 	i40e_prep_for_reset(pf);
 	rtnl_unlock();
@@ -9844,6 +10312,7 @@ static int __init i40e_init_module(void)
 	pr_info("%s: %s - version %s\n", i40e_driver_name,
 		i40e_driver_string, i40e_driver_version_str);
 	pr_info("%s: %s\n", i40e_driver_name, i40e_copyright);
+
 	i40e_dbg_init();
 	return pci_register_driver(&i40e_driver);
 }
diff --git a/drivers/net/ethernet/intel/i40e/i40e_nvm.c b/drivers/net/ethernet/intel/i40e/i40e_nvm.c
index 5defe0d63514..e49acd2accd3 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_nvm.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_nvm.c
@@ -164,15 +164,15 @@ static i40e_status i40e_poll_sr_srctl_done_bit(struct i40e_hw *hw)
 }
 
 /**
- * i40e_read_nvm_word - Reads Shadow RAM
+ * i40e_read_nvm_word_srctl - Reads Shadow RAM via SRCTL register
  * @hw: pointer to the HW structure
  * @offset: offset of the Shadow RAM word to read (0x000000 - 0x001FFF)
  * @data: word read from the Shadow RAM
  *
  * Reads one 16 bit word from the Shadow RAM using the GLNVM_SRCTL register.
  **/
-i40e_status i40e_read_nvm_word(struct i40e_hw *hw, u16 offset,
-					 u16 *data)
+static i40e_status i40e_read_nvm_word_srctl(struct i40e_hw *hw, u16 offset,
+					    u16 *data)
 {
 	i40e_status ret_code = I40E_ERR_TIMEOUT;
 	u32 sr_reg;
@@ -212,7 +212,21 @@ read_nvm_exit:
 }
 
 /**
- * i40e_read_nvm_buffer - Reads Shadow RAM buffer
+ * i40e_read_nvm_word - Reads Shadow RAM
+ * @hw: pointer to the HW structure
+ * @offset: offset of the Shadow RAM word to read (0x000000 - 0x001FFF)
+ * @data: word read from the Shadow RAM
+ *
+ * Reads one 16 bit word from the Shadow RAM using the GLNVM_SRCTL register.
+ **/
+i40e_status i40e_read_nvm_word(struct i40e_hw *hw, u16 offset,
+			       u16 *data)
+{
+	return i40e_read_nvm_word_srctl(hw, offset, data);
+}
+
+/**
+ * i40e_read_nvm_buffer_srctl - Reads Shadow RAM buffer via SRCTL register
  * @hw: pointer to the HW structure
  * @offset: offset of the Shadow RAM word to read (0x000000 - 0x001FFF).
  * @words: (in) number of words to read; (out) number of words actually read
@@ -222,8 +236,8 @@ read_nvm_exit:
  * method. The buffer read is preceded by the NVM ownership take
  * and followed by the release.
  **/
-i40e_status i40e_read_nvm_buffer(struct i40e_hw *hw, u16 offset,
-					   u16 *words, u16 *data)
+static i40e_status i40e_read_nvm_buffer_srctl(struct i40e_hw *hw, u16 offset,
+					      u16 *words, u16 *data)
 {
 	i40e_status ret_code = 0;
 	u16 index, word;
@@ -231,7 +245,7 @@ i40e_status i40e_read_nvm_buffer(struct i40e_hw *hw, u16 offset,
 	/* Loop thru the selected region */
 	for (word = 0; word < *words; word++) {
 		index = offset + word;
-		ret_code = i40e_read_nvm_word(hw, index, &data[word]);
+		ret_code = i40e_read_nvm_word_srctl(hw, index, &data[word]);
 		if (ret_code)
 			break;
 	}
@@ -243,6 +257,23 @@ i40e_status i40e_read_nvm_buffer(struct i40e_hw *hw, u16 offset,
 }
 
 /**
+ * i40e_read_nvm_buffer - Reads Shadow RAM buffer
+ * @hw: pointer to the HW structure
+ * @offset: offset of the Shadow RAM word to read (0x000000 - 0x001FFF).
+ * @words: (in) number of words to read; (out) number of words actually read
+ * @data: words read from the Shadow RAM
+ *
+ * Reads 16 bit words (data buffer) from the SR using the i40e_read_nvm_srrd()
+ * method. The buffer read is preceded by the NVM ownership take
+ * and followed by the release.
+ **/
+i40e_status i40e_read_nvm_buffer(struct i40e_hw *hw, u16 offset,
+				 u16 *words, u16 *data)
+{
+	return i40e_read_nvm_buffer_srctl(hw, offset, words, data);
+}
+
+/**
  * i40e_write_nvm_aq - Writes Shadow RAM.
  * @hw: pointer to the HW structure.
  * @module_pointer: module pointer location in words from the NVM beginning
@@ -302,11 +333,18 @@ static i40e_status i40e_calc_nvm_checksum(struct i40e_hw *hw,
 						    u16 *checksum)
 {
 	i40e_status ret_code = 0;
+	struct i40e_virt_mem vmem;
 	u16 pcie_alt_module = 0;
 	u16 checksum_local = 0;
 	u16 vpd_module = 0;
-	u16 word = 0;
-	u32 i = 0;
+	u16 *data;
+	u16 i = 0;
+
+	ret_code = i40e_allocate_virt_mem(hw, &vmem,
+				    I40E_SR_SECTOR_SIZE_IN_WORDS * sizeof(u16));
+	if (ret_code)
+		goto i40e_calc_nvm_checksum_exit;
+	data = (u16 *)vmem.va;
 
 	/* read pointer to VPD area */
 	ret_code = i40e_read_nvm_word(hw, I40E_SR_VPD_PTR, &vpd_module);
@@ -317,7 +355,7 @@ static i40e_status i40e_calc_nvm_checksum(struct i40e_hw *hw,
 
 	/* read pointer to PCIe Alt Auto-load module */
 	ret_code = i40e_read_nvm_word(hw, I40E_SR_PCIE_ALT_AUTO_LOAD_PTR,
-				       &pcie_alt_module);
+				      &pcie_alt_module);
 	if (ret_code) {
 		ret_code = I40E_ERR_NVM_CHECKSUM;
 		goto i40e_calc_nvm_checksum_exit;
@@ -327,33 +365,40 @@ static i40e_status i40e_calc_nvm_checksum(struct i40e_hw *hw,
 	 * except the VPD and PCIe ALT Auto-load modules
 	 */
 	for (i = 0; i < hw->nvm.sr_size; i++) {
+		/* Read SR page */
+		if ((i % I40E_SR_SECTOR_SIZE_IN_WORDS) == 0) {
+			u16 words = I40E_SR_SECTOR_SIZE_IN_WORDS;
+
+			ret_code = i40e_read_nvm_buffer(hw, i, &words, data);
+			if (ret_code) {
+				ret_code = I40E_ERR_NVM_CHECKSUM;
+				goto i40e_calc_nvm_checksum_exit;
+			}
+		}
+
 		/* Skip Checksum word */
 		if (i == I40E_SR_SW_CHECKSUM_WORD)
-			i++;
+			continue;
 		/* Skip VPD module (convert byte size to word count) */
-		if (i == (u32)vpd_module) {
-			i += (I40E_SR_VPD_MODULE_MAX_SIZE / 2);
-			if (i >= hw->nvm.sr_size)
-				break;
+		if ((i >= (u32)vpd_module) &&
+		    (i < ((u32)vpd_module +
+		     (I40E_SR_VPD_MODULE_MAX_SIZE / 2)))) {
+			continue;
 		}
 		/* Skip PCIe ALT module (convert byte size to word count) */
-		if (i == (u32)pcie_alt_module) {
-			i += (I40E_SR_PCIE_ALT_MODULE_MAX_SIZE / 2);
-			if (i >= hw->nvm.sr_size)
-				break;
+		if ((i >= (u32)pcie_alt_module) &&
+		    (i < ((u32)pcie_alt_module +
+		     (I40E_SR_PCIE_ALT_MODULE_MAX_SIZE / 2)))) {
+			continue;
 		}
 
-		ret_code = i40e_read_nvm_word(hw, (u16)i, &word);
-		if (ret_code) {
-			ret_code = I40E_ERR_NVM_CHECKSUM;
-			goto i40e_calc_nvm_checksum_exit;
-		}
-		checksum_local += word;
+		checksum_local += data[i % I40E_SR_SECTOR_SIZE_IN_WORDS];
 	}
 
 	*checksum = (u16)I40E_SR_SW_CHECKSUM_BASE - checksum_local;
 
 i40e_calc_nvm_checksum_exit:
+	i40e_free_virt_mem(hw, &vmem);
 	return ret_code;
 }
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_prototype.h b/drivers/net/ethernet/intel/i40e/i40e_prototype.h
index 68e852a96680..fea0d37ecc72 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_prototype.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_prototype.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -66,6 +66,7 @@ void i40e_led_set(struct i40e_hw *hw, u32 mode, bool blink);
 
 i40e_status i40e_aq_get_firmware_version(struct i40e_hw *hw,
 				u16 *fw_major_version, u16 *fw_minor_version,
+				u32 *fw_build,
 				u16 *api_major_version, u16 *api_minor_version,
 				struct i40e_asq_cmd_details *cmd_details);
 i40e_status i40e_aq_debug_write_register(struct i40e_hw *hw,
@@ -97,7 +98,6 @@ i40e_status i40e_aq_set_link_restart_an(struct i40e_hw *hw,
 i40e_status i40e_aq_get_link_info(struct i40e_hw *hw,
 				bool enable_lse, struct i40e_link_status *link,
 				struct i40e_asq_cmd_details *cmd_details);
-i40e_status i40e_update_link_info(struct i40e_hw *hw, bool enable_lse);
 i40e_status i40e_aq_set_local_advt_reg(struct i40e_hw *hw,
 				u64 advt_reg,
 				struct i40e_asq_cmd_details *cmd_details);
@@ -247,6 +247,12 @@ void i40e_clear_hw(struct i40e_hw *hw);
 void i40e_clear_pxe_mode(struct i40e_hw *hw);
 bool i40e_get_link_status(struct i40e_hw *hw);
 i40e_status i40e_get_mac_addr(struct i40e_hw *hw, u8 *mac_addr);
+i40e_status i40e_read_bw_from_alt_ram(struct i40e_hw *hw,
+				      u32 *max_bw, u32 *min_bw, bool *min_valid,
+				      bool *max_valid);
+i40e_status i40e_aq_configure_partition_bw(struct i40e_hw *hw,
+			struct i40e_aqc_configure_partition_bw_data *bw_data,
+			struct i40e_asq_cmd_details *cmd_details);
 i40e_status i40e_get_port_mac_addr(struct i40e_hw *hw, u8 *mac_addr);
 i40e_status i40e_read_pba_string(struct i40e_hw *hw, u8 *pba_num,
 				 u32 pba_num_size);
@@ -260,8 +266,6 @@ i40e_status i40e_init_nvm(struct i40e_hw *hw);
 i40e_status i40e_acquire_nvm(struct i40e_hw *hw,
 				      enum i40e_aq_resource_access_type access);
 void i40e_release_nvm(struct i40e_hw *hw);
-i40e_status i40e_read_nvm_srrd(struct i40e_hw *hw, u16 offset,
-					 u16 *data);
 i40e_status i40e_read_nvm_word(struct i40e_hw *hw, u16 offset,
 					 u16 *data);
 i40e_status i40e_read_nvm_buffer(struct i40e_hw *hw, u16 offset,
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ptp.c b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
index fabcfa1b45b2..a92b7725dec3 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ptp.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
@@ -57,7 +57,7 @@
  * timespec. However, since the registers are 64 bits of nanoseconds, we must
  * convert the result to a timespec before we can return.
  **/
-static void i40e_ptp_read(struct i40e_pf *pf, struct timespec *ts)
+static void i40e_ptp_read(struct i40e_pf *pf, struct timespec64 *ts)
 {
 	struct i40e_hw *hw = &pf->hw;
 	u32 hi, lo;
@@ -69,7 +69,7 @@ static void i40e_ptp_read(struct i40e_pf *pf, struct timespec *ts)
 
 	ns = (((u64)hi) << 32) | lo;
 
-	*ts = ns_to_timespec(ns);
+	*ts = ns_to_timespec64(ns);
 }
 
 /**
@@ -81,10 +81,10 @@ static void i40e_ptp_read(struct i40e_pf *pf, struct timespec *ts)
  * we receive a timespec from the stack, we must convert that timespec into
  * nanoseconds before programming the registers.
  **/
-static void i40e_ptp_write(struct i40e_pf *pf, const struct timespec *ts)
+static void i40e_ptp_write(struct i40e_pf *pf, const struct timespec64 *ts)
 {
 	struct i40e_hw *hw = &pf->hw;
-	u64 ns = timespec_to_ns(ts);
+	u64 ns = timespec64_to_ns(ts);
 
 	/* The timer will not update until the high register is written, so
 	 * write the low register first.
@@ -159,14 +159,14 @@ static int i40e_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 static int i40e_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 {
 	struct i40e_pf *pf = container_of(ptp, struct i40e_pf, ptp_caps);
-	struct timespec now, then = ns_to_timespec(delta);
+	struct timespec64 now, then = ns_to_timespec64(delta);
 	unsigned long flags;
 
 	spin_lock_irqsave(&pf->tmreg_lock, flags);
 
 	i40e_ptp_read(pf, &now);
-	now = timespec_add(now, then);
-	i40e_ptp_write(pf, (const struct timespec *)&now);
+	now = timespec64_add(now, then);
+	i40e_ptp_write(pf, (const struct timespec64 *)&now);
 
 	spin_unlock_irqrestore(&pf->tmreg_lock, flags);
 
@@ -181,7 +181,7 @@ static int i40e_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
  * Read the device clock and return the correct value on ns, after converting it
  * into a timespec struct.
  **/
-static int i40e_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int i40e_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct i40e_pf *pf = container_of(ptp, struct i40e_pf, ptp_caps);
 	unsigned long flags;
@@ -202,7 +202,7 @@ static int i40e_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
  * to ns happens in the write function.
  **/
 static int i40e_ptp_settime(struct ptp_clock_info *ptp,
-			    const struct timespec *ts)
+			    const struct timespec64 *ts)
 {
 	struct i40e_pf *pf = container_of(ptp, struct i40e_pf, ptp_caps);
 	unsigned long flags;
@@ -613,8 +613,8 @@ static long i40e_ptp_create_clock(struct i40e_pf *pf)
 	pf->ptp_caps.pps = 0;
 	pf->ptp_caps.adjfreq = i40e_ptp_adjfreq;
 	pf->ptp_caps.adjtime = i40e_ptp_adjtime;
-	pf->ptp_caps.gettime = i40e_ptp_gettime;
-	pf->ptp_caps.settime = i40e_ptp_settime;
+	pf->ptp_caps.gettime64 = i40e_ptp_gettime;
+	pf->ptp_caps.settime64 = i40e_ptp_settime;
 	pf->ptp_caps.enable = i40e_ptp_feature_enable;
 
 	/* Attempt to register the clock before enabling the hardware. */
@@ -673,7 +673,7 @@ void i40e_ptp_init(struct i40e_pf *pf)
 		dev_err(&pf->pdev->dev, "%s: ptp_clock_register failed\n",
 			__func__);
 	} else {
-		struct timespec ts;
+		struct timespec64 ts;
 		u32 regval;
 
 		dev_info(&pf->pdev->dev, "%s: added PHC on %s\n", __func__,
@@ -695,7 +695,7 @@ void i40e_ptp_init(struct i40e_pf *pf)
 		i40e_ptp_set_timestamp_mode(pf, &pf->tstamp_config);
 
 		/* Set the clock value. */
-		ts = ktime_to_timespec(ktime_get_real());
+		ts = ktime_to_timespec64(ktime_get_real());
 		i40e_ptp_settime(&pf->ptp_caps, &ts);
 	}
 }
diff --git a/drivers/net/ethernet/intel/i40e/i40e_register.h b/drivers/net/ethernet/intel/i40e/i40e_register.h
index 65d3c8bb2d5b..522d6df51330 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_register.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_register.h
@@ -310,6 +310,10 @@
 #define I40E_PRTDCB_RUP2TC_UP6TC_MASK I40E_MASK(0x7, I40E_PRTDCB_RUP2TC_UP6TC_SHIFT)
 #define I40E_PRTDCB_RUP2TC_UP7TC_SHIFT 21
 #define I40E_PRTDCB_RUP2TC_UP7TC_MASK I40E_MASK(0x7, I40E_PRTDCB_RUP2TC_UP7TC_SHIFT)
+#define I40E_PRTDCB_RUPTQ(_i) (0x00122400 + ((_i) * 32)) /* _i=0...7 */ /* Reset: CORER */
+#define I40E_PRTDCB_RUPTQ_MAX_INDEX 7
+#define I40E_PRTDCB_RUPTQ_RXQNUM_SHIFT 0
+#define I40E_PRTDCB_RUPTQ_RXQNUM_MASK I40E_MASK(0x3FFF, I40E_PRTDCB_RUPTQ_RXQNUM_SHIFT)
 #define I40E_PRTDCB_TC2PFC 0x001C0980 /* Reset: CORER */
 #define I40E_PRTDCB_TC2PFC_TC2PFC_SHIFT 0
 #define I40E_PRTDCB_TC2PFC_TC2PFC_MASK I40E_MASK(0xFF, I40E_PRTDCB_TC2PFC_TC2PFC_SHIFT)
@@ -421,6 +425,8 @@
 #define I40E_GLGEN_GPIO_CTL_OUT_DEFAULT_MASK I40E_MASK(0x1, I40E_GLGEN_GPIO_CTL_OUT_DEFAULT_SHIFT)
 #define I40E_GLGEN_GPIO_CTL_PHY_PIN_NAME_SHIFT 20
 #define I40E_GLGEN_GPIO_CTL_PHY_PIN_NAME_MASK I40E_MASK(0x3F, I40E_GLGEN_GPIO_CTL_PHY_PIN_NAME_SHIFT)
+#define I40E_GLGEN_GPIO_CTL_PRT_BIT_MAP_SHIFT 26
+#define I40E_GLGEN_GPIO_CTL_PRT_BIT_MAP_MASK I40E_MASK(0xF, I40E_GLGEN_GPIO_CTL_PRT_BIT_MAP_SHIFT)
 #define I40E_GLGEN_GPIO_SET 0x00088184 /* Reset: POR */
 #define I40E_GLGEN_GPIO_SET_GPIO_INDX_SHIFT 0
 #define I40E_GLGEN_GPIO_SET_GPIO_INDX_MASK I40E_MASK(0x1F, I40E_GLGEN_GPIO_SET_GPIO_INDX_SHIFT)
@@ -484,7 +490,9 @@
 #define I40E_GLGEN_MDIO_CTRL_CONTMDC_SHIFT 17
 #define I40E_GLGEN_MDIO_CTRL_CONTMDC_MASK I40E_MASK(0x1, I40E_GLGEN_MDIO_CTRL_CONTMDC_SHIFT)
 #define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_SHIFT 18
-#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_MASK I40E_MASK(0x3FFF, I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_SHIFT)
+#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_MASK I40E_MASK(0x7FF, I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_SHIFT)
+#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD0_SHIFT 29
+#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD0_MASK I40E_MASK(0x7, I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD0_SHIFT)
 #define I40E_GLGEN_MDIO_I2C_SEL(_i) (0x000881C0 + ((_i) * 4)) /* _i=0...3 */ /* Reset: POR */
 #define I40E_GLGEN_MDIO_I2C_SEL_MAX_INDEX 3
 #define I40E_GLGEN_MDIO_I2C_SEL_MDIO_I2C_SEL_SHIFT 0
@@ -548,9 +556,6 @@
 #define I40E_GLGEN_RSTCTL_GRSTDEL_MASK I40E_MASK(0x3F, I40E_GLGEN_RSTCTL_GRSTDEL_SHIFT)
 #define I40E_GLGEN_RSTCTL_ECC_RST_ENA_SHIFT 8
 #define I40E_GLGEN_RSTCTL_ECC_RST_ENA_MASK I40E_MASK(0x1, I40E_GLGEN_RSTCTL_ECC_RST_ENA_SHIFT)
-#define I40E_GLGEN_RSTENA_EMP 0x000B818C /* Reset: POR */
-#define I40E_GLGEN_RSTENA_EMP_EMP_RST_ENA_SHIFT 0
-#define I40E_GLGEN_RSTENA_EMP_EMP_RST_ENA_MASK I40E_MASK(0x1, I40E_GLGEN_RSTENA_EMP_EMP_RST_ENA_SHIFT)
 #define I40E_GLGEN_RTRIG 0x000B8190 /* Reset: CORER */
 #define I40E_GLGEN_RTRIG_CORER_SHIFT 0
 #define I40E_GLGEN_RTRIG_CORER_MASK I40E_MASK(0x1, I40E_GLGEN_RTRIG_CORER_SHIFT)
@@ -1066,7 +1071,7 @@
 #define I40E_PFINT_RATEN_INTERVAL_MASK I40E_MASK(0x3F, I40E_PFINT_RATEN_INTERVAL_SHIFT)
 #define I40E_PFINT_RATEN_INTRL_ENA_SHIFT 6
 #define I40E_PFINT_RATEN_INTRL_ENA_MASK I40E_MASK(0x1, I40E_PFINT_RATEN_INTRL_ENA_SHIFT)
-#define I40E_PFINT_STAT_CTL0 0x00038400 /* Reset: PFR */
+#define I40E_PFINT_STAT_CTL0 0x00038400 /* Reset: CORER */
 #define I40E_PFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT 2
 #define I40E_PFINT_STAT_CTL0_OTHER_ITR_INDX_MASK I40E_MASK(0x3, I40E_PFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT)
 #define I40E_QINT_RQCTL(_Q) (0x0003A000 + ((_Q) * 4)) /* _i=0...1535 */ /* Reset: CORER */
@@ -1171,7 +1176,7 @@
 #define I40E_VFINT_ITRN_MAX_INDEX 2
 #define I40E_VFINT_ITRN_INTERVAL_SHIFT 0
 #define I40E_VFINT_ITRN_INTERVAL_MASK I40E_MASK(0xFFF, I40E_VFINT_ITRN_INTERVAL_SHIFT)
-#define I40E_VFINT_STAT_CTL0(_VF) (0x0002A000 + ((_VF) * 4)) /* _i=0...127 */ /* Reset: VFR */
+#define I40E_VFINT_STAT_CTL0(_VF) (0x0002A000 + ((_VF) * 4)) /* _i=0...127 */ /* Reset: CORER */
 #define I40E_VFINT_STAT_CTL0_MAX_INDEX 127
 #define I40E_VFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT 2
 #define I40E_VFINT_STAT_CTL0_OTHER_ITR_INDX_MASK I40E_MASK(0x3, I40E_VFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT)
@@ -1803,9 +1808,6 @@
 #define I40E_GLPCI_GSCN_0_3_MAX_INDEX 3
 #define I40E_GLPCI_GSCN_0_3_EVENT_COUNTER_SHIFT 0
 #define I40E_GLPCI_GSCN_0_3_EVENT_COUNTER_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPCI_GSCN_0_3_EVENT_COUNTER_SHIFT)
-#define I40E_GLPCI_LATCT 0x0009C4B4 /* Reset: PCIR */
-#define I40E_GLPCI_LATCT_PCI_COUNT_LAT_CT_SHIFT 0
-#define I40E_GLPCI_LATCT_PCI_COUNT_LAT_CT_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPCI_LATCT_PCI_COUNT_LAT_CT_SHIFT)
 #define I40E_GLPCI_LBARCTRL 0x000BE484 /* Reset: POR */
 #define I40E_GLPCI_LBARCTRL_PREFBAR_SHIFT 0
 #define I40E_GLPCI_LBARCTRL_PREFBAR_MASK I40E_MASK(0x1, I40E_GLPCI_LBARCTRL_PREFBAR_SHIFT)
@@ -1902,6 +1904,11 @@
 #define I40E_GLPCI_VFSUP_VF_PREFETCH_MASK I40E_MASK(0x1, I40E_GLPCI_VFSUP_VF_PREFETCH_SHIFT)
 #define I40E_GLPCI_VFSUP_VR_BAR_TYPE_SHIFT 1
 #define I40E_GLPCI_VFSUP_VR_BAR_TYPE_MASK I40E_MASK(0x1, I40E_GLPCI_VFSUP_VR_BAR_TYPE_SHIFT)
+#define I40E_GLTPH_CTRL 0x000BE480 /* Reset: PCIR */
+#define I40E_GLTPH_CTRL_DESC_PH_SHIFT 9
+#define I40E_GLTPH_CTRL_DESC_PH_MASK I40E_MASK(0x3, I40E_GLTPH_CTRL_DESC_PH_SHIFT)
+#define I40E_GLTPH_CTRL_DATA_PH_SHIFT 11
+#define I40E_GLTPH_CTRL_DATA_PH_MASK I40E_MASK(0x3, I40E_GLTPH_CTRL_DATA_PH_SHIFT)
 #define I40E_PF_FUNC_RID 0x0009C000 /* Reset: PCIR */
 #define I40E_PF_FUNC_RID_FUNCTION_NUMBER_SHIFT 0
 #define I40E_PF_FUNC_RID_FUNCTION_NUMBER_MASK I40E_MASK(0x7, I40E_PF_FUNC_RID_FUNCTION_NUMBER_SHIFT)
@@ -2374,20 +2381,20 @@
 #define I40E_GL_RXERR2_L_FCOEDIXAC_MASK I40E_MASK(0xFFFFFFFF, I40E_GL_RXERR2_L_FCOEDIXAC_SHIFT)
 #define I40E_GLPRT_BPRCH(_i) (0x003005E4 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPRCH_MAX_INDEX 3
-#define I40E_GLPRT_BPRCH_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPRCH_UPRCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPRCH_UPRCH_SHIFT)
+#define I40E_GLPRT_BPRCH_BPRCH_SHIFT 0
+#define I40E_GLPRT_BPRCH_BPRCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPRCH_BPRCH_SHIFT)
 #define I40E_GLPRT_BPRCL(_i) (0x003005E0 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPRCL_MAX_INDEX 3
-#define I40E_GLPRT_BPRCL_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPRCL_UPRCH_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPRCL_UPRCH_SHIFT)
+#define I40E_GLPRT_BPRCL_BPRCL_SHIFT 0
+#define I40E_GLPRT_BPRCL_BPRCL_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPRCL_BPRCL_SHIFT)
 #define I40E_GLPRT_BPTCH(_i) (0x00300A04 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPTCH_MAX_INDEX 3
-#define I40E_GLPRT_BPTCH_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPTCH_UPRCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPTCH_UPRCH_SHIFT)
+#define I40E_GLPRT_BPTCH_BPTCH_SHIFT 0
+#define I40E_GLPRT_BPTCH_BPTCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPTCH_BPTCH_SHIFT)
 #define I40E_GLPRT_BPTCL(_i) (0x00300A00 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPTCL_MAX_INDEX 3
-#define I40E_GLPRT_BPTCL_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPTCL_UPRCH_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPTCL_UPRCH_SHIFT)
+#define I40E_GLPRT_BPTCL_BPTCL_SHIFT 0
+#define I40E_GLPRT_BPTCL_BPTCL_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPTCL_BPTCL_SHIFT)
 #define I40E_GLPRT_CRCERRS(_i) (0x00300080 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_CRCERRS_MAX_INDEX 3
 #define I40E_GLPRT_CRCERRS_CRCERRS_SHIFT 0
@@ -2620,10 +2627,6 @@
 #define I40E_GLPRT_TDOLD_MAX_INDEX 3
 #define I40E_GLPRT_TDOLD_GLPRT_TDOLD_SHIFT 0
 #define I40E_GLPRT_TDOLD_GLPRT_TDOLD_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_TDOLD_GLPRT_TDOLD_SHIFT)
-#define I40E_GLPRT_TDPC(_i) (0x00375400 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
-#define I40E_GLPRT_TDPC_MAX_INDEX 3
-#define I40E_GLPRT_TDPC_TDPC_SHIFT 0
-#define I40E_GLPRT_TDPC_TDPC_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_TDPC_TDPC_SHIFT)
 #define I40E_GLPRT_UPRCH(_i) (0x003005A4 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_UPRCH_MAX_INDEX 3
 #define I40E_GLPRT_UPRCH_UPRCH_SHIFT 0
@@ -2990,9 +2993,6 @@
 #define I40E_PRTTSYN_TXTIME_L 0x001E41C0 /* Reset: GLOBR */
 #define I40E_PRTTSYN_TXTIME_L_TXTIEM_L_SHIFT 0
 #define I40E_PRTTSYN_TXTIME_L_TXTIEM_L_MASK I40E_MASK(0xFFFFFFFF, I40E_PRTTSYN_TXTIME_L_TXTIEM_L_SHIFT)
-#define I40E_GLSCD_QUANTA 0x000B2080 /* Reset: CORER */
-#define I40E_GLSCD_QUANTA_TSCDQUANTA_SHIFT 0
-#define I40E_GLSCD_QUANTA_TSCDQUANTA_MASK I40E_MASK(0x7, I40E_GLSCD_QUANTA_TSCDQUANTA_SHIFT)
 #define I40E_GL_MDET_RX 0x0012A510 /* Reset: CORER */
 #define I40E_GL_MDET_RX_FUNCTION_SHIFT 0
 #define I40E_GL_MDET_RX_FUNCTION_MASK I40E_MASK(0xFF, I40E_GL_MDET_RX_FUNCTION_SHIFT)
@@ -3258,7 +3258,7 @@
 #define I40E_VFINT_ITRN1_MAX_INDEX 2
 #define I40E_VFINT_ITRN1_INTERVAL_SHIFT 0
 #define I40E_VFINT_ITRN1_INTERVAL_MASK I40E_MASK(0xFFF, I40E_VFINT_ITRN1_INTERVAL_SHIFT)
-#define I40E_VFINT_STAT_CTL01 0x00005400 /* Reset: VFR */
+#define I40E_VFINT_STAT_CTL01 0x00005400 /* Reset: CORER */
 #define I40E_VFINT_STAT_CTL01_OTHER_ITR_INDX_SHIFT 2
 #define I40E_VFINT_STAT_CTL01_OTHER_ITR_INDX_MASK I40E_MASK(0x3, I40E_VFINT_STAT_CTL01_OTHER_ITR_INDX_SHIFT)
 #define I40E_QRX_TAIL1(_Q) (0x00002000 + ((_Q) * 4)) /* _i=0...15 */ /* Reset: CORER */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index bbf1b1247ac4..4bd3a80aba82 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -25,6 +25,7 @@
  ******************************************************************************/
 
 #include <linux/prefetch.h>
+#include <net/busy_poll.h>
 #include "i40e.h"
 #include "i40e_prototype.h"
 
@@ -44,7 +45,7 @@ static inline __le64 build_ctob(u32 td_cmd, u32 td_offset, unsigned int size,
  * i40e_program_fdir_filter - Program a Flow Director filter
  * @fdir_data: Packet data that will be filter parameters
  * @raw_packet: the pre-allocated packet buffer for FDir
- * @pf: The pf pointer
+ * @pf: The PF pointer
  * @add: True for add/update, False for remove
  **/
 int i40e_program_fdir_filter(struct i40e_fdir_filter *fdir_data, u8 *raw_packet,
@@ -227,7 +228,7 @@ static int i40e_add_del_fdir_udpv4(struct i40e_vsi *vsi,
 			 "PCTYPE:%d, Filter command send failed for fd_id:%d (ret = %d)\n",
 			 fd_data->pctype, fd_data->fd_id, ret);
 		err = true;
-	} else {
+	} else if (I40E_DEBUG_FD & pf->hw.debug_mask) {
 		if (add)
 			dev_info(&pf->pdev->dev,
 				 "Filter OK for PCTYPE %d loc = %d\n",
@@ -302,7 +303,7 @@ static int i40e_add_del_fdir_tcpv4(struct i40e_vsi *vsi,
 			 "PCTYPE:%d, Filter command send failed for fd_id:%d (ret = %d)\n",
 			 fd_data->pctype, fd_data->fd_id, ret);
 		err = true;
-	} else {
+	} else if (I40E_DEBUG_FD & pf->hw.debug_mask) {
 		if (add)
 			dev_info(&pf->pdev->dev, "Filter OK for PCTYPE %d loc = %d)\n",
 				 fd_data->pctype, fd_data->fd_id);
@@ -375,7 +376,7 @@ static int i40e_add_del_fdir_ipv4(struct i40e_vsi *vsi,
 				 "PCTYPE:%d, Filter command send failed for fd_id:%d (ret = %d)\n",
 				 fd_data->pctype, fd_data->fd_id, ret);
 			err = true;
-		} else {
+		} else if (I40E_DEBUG_FD & pf->hw.debug_mask) {
 			if (add)
 				dev_info(&pf->pdev->dev,
 					 "Filter OK for PCTYPE %d loc = %d\n",
@@ -470,12 +471,27 @@ static void i40e_fd_handle_status(struct i40e_ring *rx_ring,
 			dev_warn(&pdev->dev, "ntuple filter loc = %d, could not be added\n",
 				 rx_desc->wb.qword0.hi_dword.fd_id);
 
+		/* Check if the programming error is for ATR.
+		 * If so, auto disable ATR and set a state for
+		 * flush in progress. Next time we come here if flush is in
+		 * progress do nothing, once flush is complete the state will
+		 * be cleared.
+		 */
+		if (test_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state))
+			return;
+
 		pf->fd_add_err++;
 		/* store the current atr filter count */
 		pf->fd_atr_cnt = i40e_get_current_atr_cnt(pf);
 
+		if ((rx_desc->wb.qword0.hi_dword.fd_id == 0) &&
+		    (pf->auto_disable_flags & I40E_FLAG_FD_SB_ENABLED)) {
+			pf->auto_disable_flags |= I40E_FLAG_FD_ATR_ENABLED;
+			set_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
+		}
+
 		/* filter programming failed most likely due to table full */
-		fcnt_prog = i40e_get_cur_guaranteed_fd_count(pf);
+		fcnt_prog = i40e_get_global_fd_count(pf);
 		fcnt_avail = pf->fdir_pf_filter_count;
 		/* If ATR is running fcnt_prog can quickly change,
 		 * if we are very close to full, it makes sense to disable
@@ -754,6 +770,8 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 			tx_desc = I40E_TX_DESC(tx_ring, 0);
 		}
 
+		prefetch(tx_desc);
+
 		/* update budget accounting */
 		budget--;
 	} while (likely(budget));
@@ -841,6 +859,7 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 static void i40e_force_wb(struct i40e_vsi *vsi, struct i40e_q_vector *q_vector)
 {
 	u32 val = I40E_PFINT_DYN_CTLN_INTENA_MASK |
+		  I40E_PFINT_DYN_CTLN_ITR_INDX_MASK | /* set noitr */
 		  I40E_PFINT_DYN_CTLN_SWINT_TRIG_MASK |
 		  I40E_PFINT_DYN_CTLN_SW_ITR_INDX_ENA_MASK;
 		  /* allow 00 to be written to the index */
@@ -1031,6 +1050,22 @@ void i40e_clean_rx_ring(struct i40e_ring *rx_ring)
 	if (!rx_ring->rx_bi)
 		return;
 
+	if (ring_is_ps_enabled(rx_ring)) {
+		int bufsz = ALIGN(rx_ring->rx_hdr_len, 256) * rx_ring->count;
+
+		rx_bi = &rx_ring->rx_bi[0];
+		if (rx_bi->hdr_buf) {
+			dma_free_coherent(dev,
+					  bufsz,
+					  rx_bi->hdr_buf,
+					  rx_bi->dma);
+			for (i = 0; i < rx_ring->count; i++) {
+				rx_bi = &rx_ring->rx_bi[i];
+				rx_bi->dma = 0;
+				rx_bi->hdr_buf = NULL;
+			}
+		}
+	}
 	/* Free all the Rx ring sk_buffs */
 	for (i = 0; i < rx_ring->count; i++) {
 		rx_bi = &rx_ring->rx_bi[i];
@@ -1089,6 +1124,37 @@ void i40e_free_rx_resources(struct i40e_ring *rx_ring)
 }
 
 /**
+ * i40e_alloc_rx_headers - allocate rx header buffers
+ * @rx_ring: ring to alloc buffers
+ *
+ * Allocate rx header buffers for the entire ring. As these are static,
+ * this is only called when setting up a new ring.
+ **/
+void i40e_alloc_rx_headers(struct i40e_ring *rx_ring)
+{
+	struct device *dev = rx_ring->dev;
+	struct i40e_rx_buffer *rx_bi;
+	dma_addr_t dma;
+	void *buffer;
+	int buf_size;
+	int i;
+
+	if (rx_ring->rx_bi[0].hdr_buf)
+		return;
+	/* Make sure the buffers don't cross cache line boundaries. */
+	buf_size = ALIGN(rx_ring->rx_hdr_len, 256);
+	buffer = dma_alloc_coherent(dev, buf_size * rx_ring->count,
+				    &dma, GFP_KERNEL);
+	if (!buffer)
+		return;
+	for (i = 0; i < rx_ring->count; i++) {
+		rx_bi = &rx_ring->rx_bi[i];
+		rx_bi->dma = dma + (i * buf_size);
+		rx_bi->hdr_buf = buffer + (i * buf_size);
+	}
+}
+
+/**
  * i40e_setup_rx_descriptors - Allocate Rx descriptors
  * @rx_ring: Rx descriptor ring (for a specific queue) to setup
  *
@@ -1148,11 +1214,76 @@ static inline void i40e_release_rx_desc(struct i40e_ring *rx_ring, u32 val)
 }
 
 /**
- * i40e_alloc_rx_buffers - Replace used receive buffers; packet split
+ * i40e_alloc_rx_buffers_ps - Replace used receive buffers; packet split
+ * @rx_ring: ring to place buffers on
+ * @cleaned_count: number of buffers to replace
+ **/
+void i40e_alloc_rx_buffers_ps(struct i40e_ring *rx_ring, u16 cleaned_count)
+{
+	u16 i = rx_ring->next_to_use;
+	union i40e_rx_desc *rx_desc;
+	struct i40e_rx_buffer *bi;
+
+	/* do nothing if no valid netdev defined */
+	if (!rx_ring->netdev || !cleaned_count)
+		return;
+
+	while (cleaned_count--) {
+		rx_desc = I40E_RX_DESC(rx_ring, i);
+		bi = &rx_ring->rx_bi[i];
+
+		if (bi->skb) /* desc is in use */
+			goto no_buffers;
+		if (!bi->page) {
+			bi->page = alloc_page(GFP_ATOMIC);
+			if (!bi->page) {
+				rx_ring->rx_stats.alloc_page_failed++;
+				goto no_buffers;
+			}
+		}
+
+		if (!bi->page_dma) {
+			/* use a half page if we're re-using */
+			bi->page_offset ^= PAGE_SIZE / 2;
+			bi->page_dma = dma_map_page(rx_ring->dev,
+						    bi->page,
+						    bi->page_offset,
+						    PAGE_SIZE / 2,
+						    DMA_FROM_DEVICE);
+			if (dma_mapping_error(rx_ring->dev,
+					      bi->page_dma)) {
+				rx_ring->rx_stats.alloc_page_failed++;
+				bi->page_dma = 0;
+				goto no_buffers;
+			}
+		}
+
+		dma_sync_single_range_for_device(rx_ring->dev,
+						 bi->dma,
+						 0,
+						 rx_ring->rx_hdr_len,
+						 DMA_FROM_DEVICE);
+		/* Refresh the desc even if buffer_addrs didn't change
+		 * because each write-back erases this info.
+		 */
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->page_dma);
+		rx_desc->read.hdr_addr = cpu_to_le64(bi->dma);
+		i++;
+		if (i == rx_ring->count)
+			i = 0;
+	}
+
+no_buffers:
+	if (rx_ring->next_to_use != i)
+		i40e_release_rx_desc(rx_ring, i);
+}
+
+/**
+ * i40e_alloc_rx_buffers_1buf - Replace used receive buffers; single buffer
  * @rx_ring: ring to place buffers on
  * @cleaned_count: number of buffers to replace
  **/
-void i40e_alloc_rx_buffers(struct i40e_ring *rx_ring, u16 cleaned_count)
+void i40e_alloc_rx_buffers_1buf(struct i40e_ring *rx_ring, u16 cleaned_count)
 {
 	u16 i = rx_ring->next_to_use;
 	union i40e_rx_desc *rx_desc;
@@ -1192,40 +1323,8 @@ void i40e_alloc_rx_buffers(struct i40e_ring *rx_ring, u16 cleaned_count)
 			}
 		}
 
-		if (ring_is_ps_enabled(rx_ring)) {
-			if (!bi->page) {
-				bi->page = alloc_page(GFP_ATOMIC);
-				if (!bi->page) {
-					rx_ring->rx_stats.alloc_page_failed++;
-					goto no_buffers;
-				}
-			}
-
-			if (!bi->page_dma) {
-				/* use a half page if we're re-using */
-				bi->page_offset ^= PAGE_SIZE / 2;
-				bi->page_dma = dma_map_page(rx_ring->dev,
-							    bi->page,
-							    bi->page_offset,
-							    PAGE_SIZE / 2,
-							    DMA_FROM_DEVICE);
-				if (dma_mapping_error(rx_ring->dev,
-						      bi->page_dma)) {
-					rx_ring->rx_stats.alloc_page_failed++;
-					bi->page_dma = 0;
-					goto no_buffers;
-				}
-			}
-
-			/* Refresh the desc even if buffer_addrs didn't change
-			 * because each write-back erases this info.
-			 */
-			rx_desc->read.pkt_addr = cpu_to_le64(bi->page_dma);
-			rx_desc->read.hdr_addr = cpu_to_le64(bi->dma);
-		} else {
-			rx_desc->read.pkt_addr = cpu_to_le64(bi->dma);
-			rx_desc->read.hdr_addr = 0;
-		}
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma);
+		rx_desc->read.hdr_addr = 0;
 		i++;
 		if (i == rx_ring->count)
 			i = 0;
@@ -1279,10 +1378,10 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	struct iphdr *iph;
 	__sum16 csum;
 
-	ipv4_tunnel = (rx_ptype > I40E_RX_PTYPE_GRENAT4_MAC_PAY3) &&
-		      (rx_ptype < I40E_RX_PTYPE_GRENAT4_MACVLAN_IPV6_ICMP_PAY4);
-	ipv6_tunnel = (rx_ptype > I40E_RX_PTYPE_GRENAT6_MAC_PAY3) &&
-		      (rx_ptype < I40E_RX_PTYPE_GRENAT6_MACVLAN_IPV6_ICMP_PAY4);
+	ipv4_tunnel = (rx_ptype >= I40E_RX_PTYPE_GRENAT4_MAC_PAY3) &&
+		     (rx_ptype <= I40E_RX_PTYPE_GRENAT4_MACVLAN_IPV6_ICMP_PAY4);
+	ipv6_tunnel = (rx_ptype >= I40E_RX_PTYPE_GRENAT6_MAC_PAY3) &&
+		     (rx_ptype <= I40E_RX_PTYPE_GRENAT6_MACVLAN_IPV6_ICMP_PAY4);
 
 	skb->ip_summed = CHECKSUM_NONE;
 
@@ -1410,13 +1509,13 @@ static inline enum pkt_hash_types i40e_ptype_to_hash(u8 ptype)
 }
 
 /**
- * i40e_clean_rx_irq - Reclaim resources after receive completes
+ * i40e_clean_rx_irq_ps - Reclaim resources after receive; packet split
  * @rx_ring:  rx ring to clean
  * @budget:   how many cleans we're allowed
  *
  * Returns true if there's any budget left (e.g. the clean is finished)
  **/
-static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
+static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 {
 	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
 	u16 rx_packet_len, rx_header_len, rx_sph, rx_hbo;
@@ -1432,25 +1531,54 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 	if (budget <= 0)
 		return 0;
 
-	rx_desc = I40E_RX_DESC(rx_ring, i);
-	qword = le64_to_cpu(rx_desc->wb.qword1.status_error_len);
-	rx_status = (qword & I40E_RXD_QW1_STATUS_MASK) >>
-		    I40E_RXD_QW1_STATUS_SHIFT;
-
-	while (rx_status & (1 << I40E_RX_DESC_STATUS_DD_SHIFT)) {
-		union i40e_rx_desc *next_rxd;
+	do {
 		struct i40e_rx_buffer *rx_bi;
 		struct sk_buff *skb;
 		u16 vlan_tag;
+		/* return some buffers to hardware, one at a time is too slow */
+		if (cleaned_count >= I40E_RX_BUFFER_WRITE) {
+			i40e_alloc_rx_buffers_ps(rx_ring, cleaned_count);
+			cleaned_count = 0;
+		}
+
+		i = rx_ring->next_to_clean;
+		rx_desc = I40E_RX_DESC(rx_ring, i);
+		qword = le64_to_cpu(rx_desc->wb.qword1.status_error_len);
+		rx_status = (qword & I40E_RXD_QW1_STATUS_MASK) >>
+			I40E_RXD_QW1_STATUS_SHIFT;
+
+		if (!(rx_status & (1 << I40E_RX_DESC_STATUS_DD_SHIFT)))
+			break;
+
+		/* This memory barrier is needed to keep us from reading
+		 * any other fields out of the rx_desc until we know the
+		 * DD bit is set.
+		 */
+		dma_rmb();
 		if (i40e_rx_is_programming_status(qword)) {
 			i40e_clean_programming_status(rx_ring, rx_desc);
-			I40E_RX_NEXT_DESC_PREFETCH(rx_ring, i, next_rxd);
-			goto next_desc;
+			I40E_RX_INCREMENT(rx_ring, i);
+			continue;
 		}
 		rx_bi = &rx_ring->rx_bi[i];
 		skb = rx_bi->skb;
-		prefetch(skb->data);
+		if (likely(!skb)) {
+			skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
+							rx_ring->rx_hdr_len);
+			if (!skb) {
+				rx_ring->rx_stats.alloc_buff_failed++;
+				break;
+			}
 
+			/* initialize queue mapping */
+			skb_record_rx_queue(skb, rx_ring->queue_index);
+			/* we are reusing so sync this buffer for CPU use */
+			dma_sync_single_range_for_cpu(rx_ring->dev,
+						      rx_bi->dma,
+						      0,
+						      rx_ring->rx_hdr_len,
+						      DMA_FROM_DEVICE);
+		}
 		rx_packet_len = (qword & I40E_RXD_QW1_LENGTH_PBUF_MASK) >>
 				I40E_RXD_QW1_LENGTH_PBUF_SHIFT;
 		rx_header_len = (qword & I40E_RXD_QW1_LENGTH_HBUF_MASK) >>
@@ -1465,40 +1593,30 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 
 		rx_ptype = (qword & I40E_RXD_QW1_PTYPE_MASK) >>
 			   I40E_RXD_QW1_PTYPE_SHIFT;
+		prefetch(rx_bi->page);
 		rx_bi->skb = NULL;
-
-		/* This memory barrier is needed to keep us from reading
-		 * any other fields out of the rx_desc until we know the
-		 * STATUS_DD bit is set
-		 */
-		rmb();
-
-		/* Get the header and possibly the whole packet
-		 * If this is an skb from previous receive dma will be 0
-		 */
-		if (rx_bi->dma) {
-			u16 len;
-
+		cleaned_count++;
+		if (rx_hbo || rx_sph) {
+			int len;
 			if (rx_hbo)
 				len = I40E_RX_HDR_SIZE;
-			else if (rx_sph)
-				len = rx_header_len;
-			else if (rx_packet_len)
-				len = rx_packet_len;   /* 1buf/no split found */
 			else
-				len = rx_header_len;   /* split always mode */
-
-			skb_put(skb, len);
-			dma_unmap_single(rx_ring->dev,
-					 rx_bi->dma,
-					 rx_ring->rx_buf_len,
-					 DMA_FROM_DEVICE);
-			rx_bi->dma = 0;
+				len = rx_header_len;
+			memcpy(__skb_put(skb, len), rx_bi->hdr_buf, len);
+		} else if (skb->len == 0) {
+			int len;
+
+			len = (rx_packet_len > skb_headlen(skb) ?
+				skb_headlen(skb) : rx_packet_len);
+			memcpy(__skb_put(skb, len),
+			       rx_bi->page + rx_bi->page_offset,
+			       len);
+			rx_bi->page_offset += len;
+			rx_packet_len -= len;
 		}
 
 		/* Get the rest of the data if this was a header split */
-		if (ring_is_ps_enabled(rx_ring) && rx_packet_len) {
-
+		if (rx_packet_len) {
 			skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
 					   rx_bi->page,
 					   rx_bi->page_offset,
@@ -1520,22 +1638,16 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 				       DMA_FROM_DEVICE);
 			rx_bi->page_dma = 0;
 		}
-		I40E_RX_NEXT_DESC_PREFETCH(rx_ring, i, next_rxd);
+		I40E_RX_INCREMENT(rx_ring, i);
 
 		if (unlikely(
 		    !(rx_status & (1 << I40E_RX_DESC_STATUS_EOF_SHIFT)))) {
 			struct i40e_rx_buffer *next_buffer;
 
 			next_buffer = &rx_ring->rx_bi[i];
-
-			if (ring_is_ps_enabled(rx_ring)) {
-				rx_bi->skb = next_buffer->skb;
-				rx_bi->dma = next_buffer->dma;
-				next_buffer->skb = skb;
-				next_buffer->dma = 0;
-			}
+			next_buffer->skb = skb;
 			rx_ring->rx_stats.non_eop_descs++;
-			goto next_desc;
+			continue;
 		}
 
 		/* ERR_MASK will only have valid bits if EOP set */
@@ -1544,7 +1656,7 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 			/* TODO: shouldn't we increment a counter indicating the
 			 * drop?
 			 */
-			goto next_desc;
+			continue;
 		}
 
 		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
@@ -1570,33 +1682,149 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 #ifdef I40E_FCOE
 		if (!i40e_fcoe_handle_offload(rx_ring, rx_desc, skb)) {
 			dev_kfree_skb_any(skb);
-			goto next_desc;
+			continue;
 		}
 #endif
+		skb_mark_napi_id(skb, &rx_ring->q_vector->napi);
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
 		rx_ring->netdev->last_rx = jiffies;
-		budget--;
-next_desc:
 		rx_desc->wb.qword1.status_error_len = 0;
-		if (!budget)
-			break;
 
-		cleaned_count++;
+	} while (likely(total_rx_packets < budget));
+
+	u64_stats_update_begin(&rx_ring->syncp);
+	rx_ring->stats.packets += total_rx_packets;
+	rx_ring->stats.bytes += total_rx_bytes;
+	u64_stats_update_end(&rx_ring->syncp);
+	rx_ring->q_vector->rx.total_packets += total_rx_packets;
+	rx_ring->q_vector->rx.total_bytes += total_rx_bytes;
+
+	return total_rx_packets;
+}
+
+/**
+ * i40e_clean_rx_irq_1buf - Reclaim resources after receive; single buffer
+ * @rx_ring:  rx ring to clean
+ * @budget:   how many cleans we're allowed
+ *
+ * Returns number of packets cleaned
+ **/
+static int i40e_clean_rx_irq_1buf(struct i40e_ring *rx_ring, int budget)
+{
+	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
+	u16 cleaned_count = I40E_DESC_UNUSED(rx_ring);
+	struct i40e_vsi *vsi = rx_ring->vsi;
+	union i40e_rx_desc *rx_desc;
+	u32 rx_error, rx_status;
+	u16 rx_packet_len;
+	u8 rx_ptype;
+	u64 qword;
+	u16 i;
+
+	do {
+		struct i40e_rx_buffer *rx_bi;
+		struct sk_buff *skb;
+		u16 vlan_tag;
 		/* return some buffers to hardware, one at a time is too slow */
 		if (cleaned_count >= I40E_RX_BUFFER_WRITE) {
-			i40e_alloc_rx_buffers(rx_ring, cleaned_count);
+			i40e_alloc_rx_buffers_1buf(rx_ring, cleaned_count);
 			cleaned_count = 0;
 		}
 
-		/* use prefetched values */
-		rx_desc = next_rxd;
+		i = rx_ring->next_to_clean;
+		rx_desc = I40E_RX_DESC(rx_ring, i);
 		qword = le64_to_cpu(rx_desc->wb.qword1.status_error_len);
 		rx_status = (qword & I40E_RXD_QW1_STATUS_MASK) >>
-			    I40E_RXD_QW1_STATUS_SHIFT;
-	}
+			I40E_RXD_QW1_STATUS_SHIFT;
+
+		if (!(rx_status & (1 << I40E_RX_DESC_STATUS_DD_SHIFT)))
+			break;
+
+		/* This memory barrier is needed to keep us from reading
+		 * any other fields out of the rx_desc until we know the
+		 * DD bit is set.
+		 */
+		dma_rmb();
+
+		if (i40e_rx_is_programming_status(qword)) {
+			i40e_clean_programming_status(rx_ring, rx_desc);
+			I40E_RX_INCREMENT(rx_ring, i);
+			continue;
+		}
+		rx_bi = &rx_ring->rx_bi[i];
+		skb = rx_bi->skb;
+		prefetch(skb->data);
+
+		rx_packet_len = (qword & I40E_RXD_QW1_LENGTH_PBUF_MASK) >>
+				I40E_RXD_QW1_LENGTH_PBUF_SHIFT;
+
+		rx_error = (qword & I40E_RXD_QW1_ERROR_MASK) >>
+			   I40E_RXD_QW1_ERROR_SHIFT;
+		rx_error &= ~(1 << I40E_RX_DESC_ERROR_HBO_SHIFT);
+
+		rx_ptype = (qword & I40E_RXD_QW1_PTYPE_MASK) >>
+			   I40E_RXD_QW1_PTYPE_SHIFT;
+		rx_bi->skb = NULL;
+		cleaned_count++;
+
+		/* Get the header and possibly the whole packet
+		 * If this is an skb from previous receive dma will be 0
+		 */
+		skb_put(skb, rx_packet_len);
+		dma_unmap_single(rx_ring->dev, rx_bi->dma, rx_ring->rx_buf_len,
+				 DMA_FROM_DEVICE);
+		rx_bi->dma = 0;
+
+		I40E_RX_INCREMENT(rx_ring, i);
+
+		if (unlikely(
+		    !(rx_status & (1 << I40E_RX_DESC_STATUS_EOF_SHIFT)))) {
+			rx_ring->rx_stats.non_eop_descs++;
+			continue;
+		}
+
+		/* ERR_MASK will only have valid bits if EOP set */
+		if (unlikely(rx_error & (1 << I40E_RX_DESC_ERROR_RXE_SHIFT))) {
+			dev_kfree_skb_any(skb);
+			/* TODO: shouldn't we increment a counter indicating the
+			 * drop?
+			 */
+			continue;
+		}
+
+		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
+			     i40e_ptype_to_hash(rx_ptype));
+		if (unlikely(rx_status & I40E_RXD_QW1_STATUS_TSYNVALID_MASK)) {
+			i40e_ptp_rx_hwtstamp(vsi->back, skb, (rx_status &
+					   I40E_RXD_QW1_STATUS_TSYNINDX_MASK) >>
+					   I40E_RXD_QW1_STATUS_TSYNINDX_SHIFT);
+			rx_ring->last_rx_timestamp = jiffies;
+		}
+
+		/* probably a little skewed due to removing CRC */
+		total_rx_bytes += skb->len;
+		total_rx_packets++;
+
+		skb->protocol = eth_type_trans(skb, rx_ring->netdev);
+
+		i40e_rx_checksum(vsi, skb, rx_status, rx_error, rx_ptype);
+
+		vlan_tag = rx_status & (1 << I40E_RX_DESC_STATUS_L2TAG1P_SHIFT)
+			 ? le16_to_cpu(rx_desc->wb.qword0.lo_dword.l2tag1)
+			 : 0;
+#ifdef I40E_FCOE
+		if (!i40e_fcoe_handle_offload(rx_ring, rx_desc, skb)) {
+			dev_kfree_skb_any(skb);
+			continue;
+		}
+#endif
+		i40e_receive_skb(rx_ring, skb, vlan_tag);
+
+		rx_ring->netdev->last_rx = jiffies;
+		rx_desc->wb.qword1.status_error_len = 0;
+	} while (likely(total_rx_packets < budget));
 
-	rx_ring->next_to_clean = i;
 	u64_stats_update_begin(&rx_ring->syncp);
 	rx_ring->stats.packets += total_rx_packets;
 	rx_ring->stats.bytes += total_rx_bytes;
@@ -1604,10 +1832,7 @@ next_desc:
 	rx_ring->q_vector->rx.total_packets += total_rx_packets;
 	rx_ring->q_vector->rx.total_bytes += total_rx_bytes;
 
-	if (cleaned_count)
-		i40e_alloc_rx_buffers(rx_ring, cleaned_count);
-
-	return budget > 0;
+	return total_rx_packets;
 }
 
 /**
@@ -1628,6 +1853,7 @@ int i40e_napi_poll(struct napi_struct *napi, int budget)
 	bool clean_complete = true;
 	bool arm_wb = false;
 	int budget_per_ring;
+	int cleaned;
 
 	if (test_bit(__I40E_DOWN, &vsi->state)) {
 		napi_complete(napi);
@@ -1647,8 +1873,14 @@ int i40e_napi_poll(struct napi_struct *napi, int budget)
 	 */
 	budget_per_ring = max(budget/q_vector->num_ringpairs, 1);
 
-	i40e_for_each_ring(ring, q_vector->rx)
-		clean_complete &= i40e_clean_rx_irq(ring, budget_per_ring);
+	i40e_for_each_ring(ring, q_vector->rx) {
+		if (ring_is_ps_enabled(ring))
+			cleaned = i40e_clean_rx_irq_ps(ring, budget_per_ring);
+		else
+			cleaned = i40e_clean_rx_irq_1buf(ring, budget_per_ring);
+		/* if we didn't clean as many as budgeted, we must be done */
+		clean_complete &= (budget_per_ring != cleaned);
+	}
 
 	/* If work not completed, return budget and polling will return */
 	if (!clean_complete) {
@@ -1715,6 +1947,9 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	if (!(pf->flags & I40E_FLAG_FD_ATR_ENABLED))
 		return;
 
+	if ((pf->auto_disable_flags & I40E_FLAG_FD_ATR_ENABLED))
+		return;
+
 	/* if sampling is disabled do nothing */
 	if (!tx_ring->atr_sample_rate)
 		return;
@@ -1822,6 +2057,19 @@ static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
 	__be16 protocol = skb->protocol;
 	u32  tx_flags = 0;
 
+	if (protocol == htons(ETH_P_8021Q) &&
+	    !(tx_ring->netdev->features & NETIF_F_HW_VLAN_CTAG_TX)) {
+		/* When HW VLAN acceleration is turned off by the user the
+		 * stack sets the protocol to 8021q so that the driver
+		 * can take any steps required to support the SW only
+		 * VLAN handling.  In our case the driver doesn't need
+		 * to take any further steps so just set the protocol
+		 * to the encapsulated ethertype.
+		 */
+		skb->protocol = vlan_get_protocol(skb);
+		goto out;
+	}
+
 	/* if we have a HW VLAN tag being added, default to the HW one */
 	if (skb_vlan_tag_present(skb)) {
 		tx_flags |= skb_vlan_tag_get(skb) << I40E_TX_FLAGS_VLAN_SHIFT;
@@ -1838,6 +2086,9 @@ static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
 		tx_flags |= I40E_TX_FLAGS_SW_VLAN;
 	}
 
+	if (!(tx_ring->vsi->back->flags & I40E_FLAG_DCB_ENABLED))
+		goto out;
+
 	/* Insert 802.1p priority into VLAN header */
 	if ((tx_flags & (I40E_TX_FLAGS_HW_VLAN | I40E_TX_FLAGS_SW_VLAN)) ||
 	    (skb->priority != TC_PRIO_CONTROL)) {
@@ -1858,6 +2109,8 @@ static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
 			tx_flags |= I40E_TX_FLAGS_HW_VLAN;
 		}
 	}
+
+out:
 	*flags = tx_flags;
 	return 0;
 }
@@ -1982,8 +2235,16 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 	struct iphdr *this_ip_hdr;
 	u32 network_hdr_len;
 	u8 l4_hdr = 0;
+	u32 l4_tunnel = 0;
 
 	if (skb->encapsulation) {
+		switch (ip_hdr(skb)->protocol) {
+		case IPPROTO_UDP:
+			l4_tunnel = I40E_TXD_CTX_UDP_TUNNELING;
+			break;
+		default:
+			return;
+		}
 		network_hdr_len = skb_inner_network_header_len(skb);
 		this_ip_hdr = inner_ip_hdr(skb);
 		this_ipv6_hdr = inner_ipv6_hdr(skb);
@@ -2006,8 +2267,8 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 
 		/* Now set the ctx descriptor fields */
 		*cd_tunneling |= (skb_network_header_len(skb) >> 2) <<
-					I40E_TXD_CTX_QW0_EXT_IPLEN_SHIFT |
-				   I40E_TXD_CTX_UDP_TUNNELING            |
+				   I40E_TXD_CTX_QW0_EXT_IPLEN_SHIFT      |
+				   l4_tunnel                             |
 				   ((skb_inner_network_offset(skb) -
 					skb_transport_offset(skb)) >> 1) <<
 				   I40E_TXD_CTX_QW0_NATLEN_SHIFT;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.h b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
index dff0baeb1ecc..4b0b8102cdc3 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
@@ -96,6 +96,14 @@ enum i40e_dyn_idx_t {
 
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
 #define I40E_RX_BUFFER_WRITE	16	/* Must be power of 2 */
+#define I40E_RX_INCREMENT(r, i) \
+	do {					\
+		(i)++;				\
+		if ((i) == (r)->count)		\
+			i = 0;			\
+		r->next_to_clean = i;		\
+	} while (0)
+
 #define I40E_RX_NEXT_DESC(r, i, n)		\
 	do {					\
 		(i)++;				\
@@ -152,6 +160,7 @@ struct i40e_tx_buffer {
 
 struct i40e_rx_buffer {
 	struct sk_buff *skb;
+	void *hdr_buf;
 	dma_addr_t dma;
 	struct page *page;
 	dma_addr_t page_dma;
@@ -224,8 +233,8 @@ struct i40e_ring {
 	u16 rx_buf_len;
 	u8  dtype;
 #define I40E_RX_DTYPE_NO_SPLIT      0
-#define I40E_RX_DTYPE_SPLIT_ALWAYS  1
-#define I40E_RX_DTYPE_HEADER_SPLIT  2
+#define I40E_RX_DTYPE_HEADER_SPLIT  1
+#define I40E_RX_DTYPE_SPLIT_ALWAYS  2
 	u8  hsplit;
 #define I40E_RX_SPLIT_L2      0x1
 #define I40E_RX_SPLIT_IP      0x2
@@ -281,7 +290,9 @@ struct i40e_ring_container {
 #define i40e_for_each_ring(pos, head) \
 	for (pos = (head).ring; pos != NULL; pos = pos->next)
 
-void i40e_alloc_rx_buffers(struct i40e_ring *rxr, u16 cleaned_count);
+void i40e_alloc_rx_buffers_ps(struct i40e_ring *rxr, u16 cleaned_count);
+void i40e_alloc_rx_buffers_1buf(struct i40e_ring *rxr, u16 cleaned_count);
+void i40e_alloc_rx_headers(struct i40e_ring *rxr);
 netdev_tx_t i40e_lan_xmit_frame(struct sk_buff *skb, struct net_device *netdev);
 void i40e_clean_tx_ring(struct i40e_ring *tx_ring);
 void i40e_clean_rx_ring(struct i40e_ring *rx_ring);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_type.h b/drivers/net/ethernet/intel/i40e/i40e_type.h
index e9901ef06a63..67c7bc9e9c21 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_type.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_type.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -44,6 +44,7 @@
 #define I40E_DEV_ID_QSFP_B		0x1584
 #define I40E_DEV_ID_QSFP_C		0x1585
 #define I40E_DEV_ID_10G_BASE_T		0x1586
+#define I40E_DEV_ID_20G_KR2		0x1587
 #define I40E_DEV_ID_VF			0x154C
 #define I40E_DEV_ID_VF_HV		0x1571
 
@@ -175,12 +176,12 @@ struct i40e_link_status {
 	u8 an_info;
 	u8 ext_info;
 	u8 loopback;
-	bool an_enabled;
 	/* is Link Status Event notification to SW enabled */
 	bool lse_enable;
 	u16 max_frame_size;
 	bool crc_enable;
 	u8 pacing;
+	u8 requested_speeds;
 };
 
 struct i40e_phy_info {
@@ -1143,7 +1144,7 @@ struct i40e_hw_port_stats {
 #define I40E_SR_EMP_MODULE_PTR			0x0F
 #define I40E_SR_PBA_FLAGS			0x15
 #define I40E_SR_PBA_BLOCK_PTR			0x16
-#define I40E_SR_NVM_IMAGE_VERSION		0x18
+#define I40E_SR_NVM_DEV_STARTER_VERSION		0x18
 #define I40E_SR_NVM_WAKE_ON_LAN			0x19
 #define I40E_SR_ALTERNATE_SAN_MAC_ADDRESS_PTR	0x27
 #define I40E_SR_NVM_EETRACK_LO			0x2D
@@ -1401,6 +1402,19 @@ struct i40e_lldp_variables {
 	u16 crc8;
 };
 
+/* Offsets into Alternate Ram */
+#define I40E_ALT_STRUCT_FIRST_PF_OFFSET		0   /* in dwords */
+#define I40E_ALT_STRUCT_DWORDS_PER_PF		64   /* in dwords */
+#define I40E_ALT_STRUCT_OUTER_VLAN_TAG_OFFSET	0xD  /* in dwords */
+#define I40E_ALT_STRUCT_USER_PRIORITY_OFFSET	0xC  /* in dwords */
+#define I40E_ALT_STRUCT_MIN_BW_OFFSET		0xE  /* in dwords */
+#define I40E_ALT_STRUCT_MAX_BW_OFFSET		0xF  /* in dwords */
+
+/* Alternate Ram Bandwidth Masks */
+#define I40E_ALT_BW_VALUE_MASK		0xFF
+#define I40E_ALT_BW_RELATIVE_MASK	0x40000000
+#define I40E_ALT_BW_VALID_MASK		0x80000000
+
 /* RSS Hash Table Size */
 #define I40E_PFQF_CTL_0_HASHLUTSIZE_512	0x00010000
 #endif /* _I40E_TYPE_H_ */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
index 61dd1b187624..2d20af290fbf 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
@@ -59,31 +59,29 @@
  * of the virtchnl_msg structure.
  */
 enum i40e_virtchnl_ops {
-/* VF sends req. to pf for the following
- * ops.
+/* The PF sends status change events to VFs using
+ * the I40E_VIRTCHNL_OP_EVENT opcode.
+ * VFs send requests to the PF using the other ops.
  */
 	I40E_VIRTCHNL_OP_UNKNOWN = 0,
 	I40E_VIRTCHNL_OP_VERSION = 1, /* must ALWAYS be 1 */
-	I40E_VIRTCHNL_OP_RESET_VF,
-	I40E_VIRTCHNL_OP_GET_VF_RESOURCES,
-	I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE,
-	I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE,
-	I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES,
-	I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP,
-	I40E_VIRTCHNL_OP_ENABLE_QUEUES,
-	I40E_VIRTCHNL_OP_DISABLE_QUEUES,
-	I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS,
-	I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS,
-	I40E_VIRTCHNL_OP_ADD_VLAN,
-	I40E_VIRTCHNL_OP_DEL_VLAN,
-	I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE,
-	I40E_VIRTCHNL_OP_GET_STATS,
-	I40E_VIRTCHNL_OP_FCOE,
-	I40E_VIRTCHNL_OP_CONFIG_RSS,
-/* PF sends status change events to vfs using
- * the following op.
- */
-	I40E_VIRTCHNL_OP_EVENT,
+	I40E_VIRTCHNL_OP_RESET_VF = 2,
+	I40E_VIRTCHNL_OP_GET_VF_RESOURCES = 3,
+	I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE = 4,
+	I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE = 5,
+	I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES = 6,
+	I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP = 7,
+	I40E_VIRTCHNL_OP_ENABLE_QUEUES = 8,
+	I40E_VIRTCHNL_OP_DISABLE_QUEUES = 9,
+	I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS = 10,
+	I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS = 11,
+	I40E_VIRTCHNL_OP_ADD_VLAN = 12,
+	I40E_VIRTCHNL_OP_DEL_VLAN = 13,
+	I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE = 14,
+	I40E_VIRTCHNL_OP_GET_STATS = 15,
+	I40E_VIRTCHNL_OP_FCOE = 16,
+	I40E_VIRTCHNL_OP_EVENT = 17,
+	I40E_VIRTCHNL_OP_CONFIG_RSS = 18,
 };
 
 /* Virtual channel message descriptor. This overlays the admin queue
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
index 40f042af4131..4d69e1f04901 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -30,8 +30,8 @@
 
 /**
  * i40e_vc_disable_vf
- * @pf: pointer to the pf info
- * @vf: pointer to the vf info
+ * @pf: pointer to the PF info
+ * @vf: pointer to the VF info
  *
  * Disable the VF through a SW reset
  **/
@@ -48,38 +48,40 @@ static inline void i40e_vc_disable_vf(struct i40e_pf *pf, struct i40e_vf *vf)
 
 /**
  * i40e_vc_isvalid_vsi_id
- * @vf: pointer to the vf info
- * @vsi_id: vf relative vsi id
+ * @vf: pointer to the VF info
+ * @vsi_id: VF relative VSI id
  *
- * check for the valid vsi id
+ * check for the valid VSI id
  **/
-static inline bool i40e_vc_isvalid_vsi_id(struct i40e_vf *vf, u8 vsi_id)
+static inline bool i40e_vc_isvalid_vsi_id(struct i40e_vf *vf, u16 vsi_id)
 {
 	struct i40e_pf *pf = vf->pf;
+	struct i40e_vsi *vsi = i40e_find_vsi_from_id(pf, vsi_id);
 
-	return pf->vsi[vsi_id]->vf_id == vf->vf_id;
+	return (vsi && (vsi->vf_id == vf->vf_id));
 }
 
 /**
  * i40e_vc_isvalid_queue_id
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @vsi_id: vsi id
  * @qid: vsi relative queue id
  *
  * check for the valid queue id
  **/
-static inline bool i40e_vc_isvalid_queue_id(struct i40e_vf *vf, u8 vsi_id,
+static inline bool i40e_vc_isvalid_queue_id(struct i40e_vf *vf, u16 vsi_id,
 					    u8 qid)
 {
 	struct i40e_pf *pf = vf->pf;
+	struct i40e_vsi *vsi = i40e_find_vsi_from_id(pf, vsi_id);
 
-	return qid < pf->vsi[vsi_id]->alloc_queue_pairs;
+	return (vsi && (qid < vsi->alloc_queue_pairs));
 }
 
 /**
  * i40e_vc_isvalid_vector_id
- * @vf: pointer to the vf info
- * @vector_id: vf relative vector id
+ * @vf: pointer to the VF info
+ * @vector_id: VF relative vector id
  *
  * check for the valid vector id
  **/
@@ -94,19 +96,22 @@ static inline bool i40e_vc_isvalid_vector_id(struct i40e_vf *vf, u8 vector_id)
 
 /**
  * i40e_vc_get_pf_queue_id
- * @vf: pointer to the vf info
- * @vsi_idx: index of VSI in PF struct
+ * @vf: pointer to the VF info
+ * @vsi_id: id of VSI as provided by the FW
  * @vsi_queue_id: vsi relative queue id
  *
- * return pf relative queue id
+ * return PF relative queue id
  **/
-static u16 i40e_vc_get_pf_queue_id(struct i40e_vf *vf, u8 vsi_idx,
+static u16 i40e_vc_get_pf_queue_id(struct i40e_vf *vf, u16 vsi_id,
 				   u8 vsi_queue_id)
 {
 	struct i40e_pf *pf = vf->pf;
-	struct i40e_vsi *vsi = pf->vsi[vsi_idx];
+	struct i40e_vsi *vsi = i40e_find_vsi_from_id(pf, vsi_id);
 	u16 pf_queue_id = I40E_QUEUE_END_OF_LIST;
 
+	if (!vsi)
+		return pf_queue_id;
+
 	if (le16_to_cpu(vsi->info.mapping_flags) &
 	    I40E_AQ_VSI_QUE_MAP_NONCONTIG)
 		pf_queue_id =
@@ -120,13 +125,13 @@ static u16 i40e_vc_get_pf_queue_id(struct i40e_vf *vf, u8 vsi_idx,
 
 /**
  * i40e_config_irq_link_list
- * @vf: pointer to the vf info
- * @vsi_idx: index of VSI in PF struct
+ * @vf: pointer to the VF info
+ * @vsi_id: id of VSI as given by the FW
  * @vecmap: irq map info
  *
  * configure irq link list from the map
  **/
-static void i40e_config_irq_link_list(struct i40e_vf *vf, u16 vsi_idx,
+static void i40e_config_irq_link_list(struct i40e_vf *vf, u16 vsi_id,
 				      struct i40e_virtchnl_vector_map *vecmap)
 {
 	unsigned long linklistmap = 0, tempmap;
@@ -171,7 +176,7 @@ static void i40e_config_irq_link_list(struct i40e_vf *vf, u16 vsi_idx,
 				 I40E_VIRTCHNL_SUPPORTED_QTYPES));
 	vsi_queue_id = next_q/I40E_VIRTCHNL_SUPPORTED_QTYPES;
 	qtype = next_q%I40E_VIRTCHNL_SUPPORTED_QTYPES;
-	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_idx, vsi_queue_id);
+	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_id, vsi_queue_id);
 	reg = ((qtype << I40E_VPINT_LNKLSTN_FIRSTQ_TYPE_SHIFT) | pf_queue_id);
 
 	wr32(hw, reg_idx, reg);
@@ -198,7 +203,7 @@ static void i40e_config_irq_link_list(struct i40e_vf *vf, u16 vsi_idx,
 		    (I40E_MAX_VSI_QP * I40E_VIRTCHNL_SUPPORTED_QTYPES)) {
 			vsi_queue_id = next_q / I40E_VIRTCHNL_SUPPORTED_QTYPES;
 			qtype = next_q % I40E_VIRTCHNL_SUPPORTED_QTYPES;
-			pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_idx,
+			pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_id,
 							      vsi_queue_id);
 		} else {
 			pf_queue_id = I40E_QUEUE_END_OF_LIST;
@@ -220,25 +225,27 @@ irq_list_done:
 
 /**
  * i40e_config_vsi_tx_queue
- * @vf: pointer to the vf info
- * @vsi_idx: index of VSI in PF struct
+ * @vf: pointer to the VF info
+ * @vsi_id: id of VSI as provided by the FW
  * @vsi_queue_id: vsi relative queue index
  * @info: config. info
  *
  * configure tx queue
  **/
-static int i40e_config_vsi_tx_queue(struct i40e_vf *vf, u16 vsi_idx,
+static int i40e_config_vsi_tx_queue(struct i40e_vf *vf, u16 vsi_id,
 				    u16 vsi_queue_id,
 				    struct i40e_virtchnl_txq_info *info)
 {
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_hw *hw = &pf->hw;
 	struct i40e_hmc_obj_txq tx_ctx;
+	struct i40e_vsi *vsi;
 	u16 pf_queue_id;
 	u32 qtx_ctl;
 	int ret = 0;
 
-	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_idx, vsi_queue_id);
+	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_id, vsi_queue_id);
+	vsi = i40e_find_vsi_from_id(pf, vsi_id);
 
 	/* clear the context structure first */
 	memset(&tx_ctx, 0, sizeof(struct i40e_hmc_obj_txq));
@@ -246,7 +253,7 @@ static int i40e_config_vsi_tx_queue(struct i40e_vf *vf, u16 vsi_idx,
 	/* only set the required fields */
 	tx_ctx.base = info->dma_ring_addr / 128;
 	tx_ctx.qlen = info->ring_len;
-	tx_ctx.rdylist = le16_to_cpu(pf->vsi[vsi_idx]->info.qs_handle[0]);
+	tx_ctx.rdylist = le16_to_cpu(vsi->info.qs_handle[0]);
 	tx_ctx.rdylist_act = 0;
 	tx_ctx.head_wb_ena = info->headwb_enabled;
 	tx_ctx.head_wb_addr = info->dma_headwb_addr;
@@ -287,14 +294,14 @@ error_context:
 
 /**
  * i40e_config_vsi_rx_queue
- * @vf: pointer to the vf info
- * @vsi_idx: index of VSI in PF struct
+ * @vf: pointer to the VF info
+ * @vsi_id: id of VSI  as provided by the FW
  * @vsi_queue_id: vsi relative queue index
  * @info: config. info
  *
  * configure rx queue
  **/
-static int i40e_config_vsi_rx_queue(struct i40e_vf *vf, u16 vsi_idx,
+static int i40e_config_vsi_rx_queue(struct i40e_vf *vf, u16 vsi_id,
 				    u16 vsi_queue_id,
 				    struct i40e_virtchnl_rxq_info *info)
 {
@@ -304,7 +311,7 @@ static int i40e_config_vsi_rx_queue(struct i40e_vf *vf, u16 vsi_idx,
 	u16 pf_queue_id;
 	int ret = 0;
 
-	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_idx, vsi_queue_id);
+	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_id, vsi_queue_id);
 
 	/* clear the context structure first */
 	memset(&rx_ctx, 0, sizeof(struct i40e_hmc_obj_rxq));
@@ -378,10 +385,10 @@ error_param:
 
 /**
  * i40e_alloc_vsi_res
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @type: type of VSI to allocate
  *
- * alloc vf vsi context & resources
+ * alloc VF vsi context & resources
  **/
 static int i40e_alloc_vsi_res(struct i40e_vf *vf, enum i40e_vsi_type type)
 {
@@ -394,18 +401,15 @@ static int i40e_alloc_vsi_res(struct i40e_vf *vf, enum i40e_vsi_type type)
 
 	if (!vsi) {
 		dev_err(&pf->pdev->dev,
-			"add vsi failed for vf %d, aq_err %d\n",
+			"add vsi failed for VF %d, aq_err %d\n",
 			vf->vf_id, pf->hw.aq.asq_last_status);
 		ret = -ENOENT;
 		goto error_alloc_vsi_res;
 	}
 	if (type == I40E_VSI_SRIOV) {
 		u8 brdcast[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-		vf->lan_vsi_index = vsi->idx;
+		vf->lan_vsi_idx = vsi->idx;
 		vf->lan_vsi_id = vsi->id;
-		dev_info(&pf->pdev->dev,
-			 "VF %d assigned LAN VSI index %d, VSI id %d\n",
-			 vf->vf_id, vsi->idx, vsi->id);
 		/* If the port VLAN has been configured and then the
 		 * VF driver was removed then the VSI port VLAN
 		 * configuration was destroyed.  Check if there is
@@ -446,9 +450,9 @@ error_alloc_vsi_res:
 
 /**
  * i40e_enable_vf_mappings
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  *
- * enable vf mappings
+ * enable VF mappings
  **/
 static void i40e_enable_vf_mappings(struct i40e_vf *vf)
 {
@@ -469,8 +473,8 @@ static void i40e_enable_vf_mappings(struct i40e_vf *vf)
 	wr32(hw, I40E_VPLAN_MAPENA(vf->vf_id), reg);
 
 	/* map PF queues to VF queues */
-	for (j = 0; j < pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs; j++) {
-		u16 qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_index, j);
+	for (j = 0; j < pf->vsi[vf->lan_vsi_idx]->alloc_queue_pairs; j++) {
+		u16 qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_id, j);
 		reg = (qid & I40E_VPLAN_QTABLE_QINDEX_MASK);
 		wr32(hw, I40E_VPLAN_QTABLE(total_queue_pairs, vf->vf_id), reg);
 		total_queue_pairs++;
@@ -478,13 +482,13 @@ static void i40e_enable_vf_mappings(struct i40e_vf *vf)
 
 	/* map PF queues to VSI */
 	for (j = 0; j < 7; j++) {
-		if (j * 2 >= pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs) {
+		if (j * 2 >= pf->vsi[vf->lan_vsi_idx]->alloc_queue_pairs) {
 			reg = 0x07FF07FF;	/* unused */
 		} else {
-			u16 qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_index,
+			u16 qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_id,
 							  j * 2);
 			reg = qid;
-			qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_index,
+			qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_id,
 						      (j * 2) + 1);
 			reg |= qid << 16;
 		}
@@ -496,9 +500,9 @@ static void i40e_enable_vf_mappings(struct i40e_vf *vf)
 
 /**
  * i40e_disable_vf_mappings
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  *
- * disable vf mappings
+ * disable VF mappings
  **/
 static void i40e_disable_vf_mappings(struct i40e_vf *vf)
 {
@@ -516,9 +520,9 @@ static void i40e_disable_vf_mappings(struct i40e_vf *vf)
 
 /**
  * i40e_free_vf_res
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  *
- * free vf resources
+ * free VF resources
  **/
 static void i40e_free_vf_res(struct i40e_vf *vf)
 {
@@ -528,9 +532,9 @@ static void i40e_free_vf_res(struct i40e_vf *vf)
 	int i, msix_vf;
 
 	/* free vsi & disconnect it from the parent uplink */
-	if (vf->lan_vsi_index) {
-		i40e_vsi_release(pf->vsi[vf->lan_vsi_index]);
-		vf->lan_vsi_index = 0;
+	if (vf->lan_vsi_idx) {
+		i40e_vsi_release(pf->vsi[vf->lan_vsi_idx]);
+		vf->lan_vsi_idx = 0;
 		vf->lan_vsi_id = 0;
 	}
 	msix_vf = pf->hw.func_caps.num_msix_vectors_vf;
@@ -571,9 +575,9 @@ static void i40e_free_vf_res(struct i40e_vf *vf)
 
 /**
  * i40e_alloc_vf_res
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  *
- * allocate vf resources
+ * allocate VF resources
  **/
 static int i40e_alloc_vf_res(struct i40e_vf *vf)
 {
@@ -585,15 +589,15 @@ static int i40e_alloc_vf_res(struct i40e_vf *vf)
 	ret = i40e_alloc_vsi_res(vf, I40E_VSI_SRIOV);
 	if (ret)
 		goto error_alloc;
-	total_queue_pairs += pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs;
+	total_queue_pairs += pf->vsi[vf->lan_vsi_idx]->alloc_queue_pairs;
 	set_bit(I40E_VIRTCHNL_VF_CAP_PRIVILEGE, &vf->vf_caps);
 
 	/* store the total qps number for the runtime
-	 * vf req validation
+	 * VF req validation
 	 */
 	vf->num_queue_pairs = total_queue_pairs;
 
-	/* vf is now completely initialized */
+	/* VF is now completely initialized */
 	set_bit(I40E_VF_STAT_INIT, &vf->vf_states);
 
 error_alloc:
@@ -607,7 +611,7 @@ error_alloc:
 #define VF_TRANS_PENDING_MASK 0x20
 /**
  * i40e_quiesce_vf_pci
- * @vf: pointer to the vf structure
+ * @vf: pointer to the VF structure
  *
  * Wait for VF PCI transactions to be cleared after reset. Returns -EIO
  * if the transactions never clear.
@@ -634,10 +638,10 @@ static int i40e_quiesce_vf_pci(struct i40e_vf *vf)
 
 /**
  * i40e_reset_vf
- * @vf: pointer to the vf structure
+ * @vf: pointer to the VF structure
  * @flr: VFLR was issued or not
  *
- * reset the vf
+ * reset the VF
  **/
 void i40e_reset_vf(struct i40e_vf *vf, bool flr)
 {
@@ -657,7 +661,7 @@ void i40e_reset_vf(struct i40e_vf *vf, bool flr)
 	 * just need to clean up, so don't hit the VFRTRIG register.
 	 */
 	if (!flr) {
-		/* reset vf using VPGEN_VFRTRIG reg */
+		/* reset VF using VPGEN_VFRTRIG reg */
 		reg = rd32(hw, I40E_VPGEN_VFRTRIG(vf->vf_id));
 		reg |= I40E_VPGEN_VFRTRIG_VFSWR_MASK;
 		wr32(hw, I40E_VPGEN_VFRTRIG(vf->vf_id), reg);
@@ -695,12 +699,12 @@ void i40e_reset_vf(struct i40e_vf *vf, bool flr)
 	wr32(hw, I40E_VPGEN_VFRTRIG(vf->vf_id), reg);
 
 	/* On initial reset, we won't have any queues */
-	if (vf->lan_vsi_index == 0)
+	if (vf->lan_vsi_idx == 0)
 		goto complete_reset;
 
-	i40e_vsi_control_rings(pf->vsi[vf->lan_vsi_index], false);
+	i40e_vsi_control_rings(pf->vsi[vf->lan_vsi_idx], false);
 complete_reset:
-	/* reallocate vf resources to reset the VSI state */
+	/* reallocate VF resources to reset the VSI state */
 	i40e_free_vf_res(vf);
 	i40e_alloc_vf_res(vf);
 	i40e_enable_vf_mappings(vf);
@@ -713,78 +717,10 @@ complete_reset:
 }
 
 /**
- * i40e_enable_pf_switch_lb
- * @pf: pointer to the pf structure
- *
- * enable switch loop back or die - no point in a return value
- **/
-void i40e_enable_pf_switch_lb(struct i40e_pf *pf)
-{
-	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
-	struct i40e_vsi_context ctxt;
-	int aq_ret;
-
-	ctxt.seid = pf->main_vsi_seid;
-	ctxt.pf_num = pf->hw.pf_id;
-	ctxt.vf_num = 0;
-	aq_ret = i40e_aq_get_vsi_params(&pf->hw, &ctxt, NULL);
-	if (aq_ret) {
-		dev_info(&pf->pdev->dev,
-			 "%s couldn't get pf vsi config, err %d, aq_err %d\n",
-			 __func__, aq_ret, pf->hw.aq.asq_last_status);
-		return;
-	}
-	ctxt.flags = I40E_AQ_VSI_TYPE_PF;
-	ctxt.info.valid_sections = cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
-	ctxt.info.switch_id |= cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
-
-	aq_ret = i40e_aq_update_vsi_params(&vsi->back->hw, &ctxt, NULL);
-	if (aq_ret) {
-		dev_info(&pf->pdev->dev,
-			 "%s: update vsi switch failed, aq_err=%d\n",
-			 __func__, vsi->back->hw.aq.asq_last_status);
-	}
-}
-
-/**
- * i40e_disable_pf_switch_lb
- * @pf: pointer to the pf structure
- *
- * disable switch loop back or die - no point in a return value
- **/
-static void i40e_disable_pf_switch_lb(struct i40e_pf *pf)
-{
-	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
-	struct i40e_vsi_context ctxt;
-	int aq_ret;
-
-	ctxt.seid = pf->main_vsi_seid;
-	ctxt.pf_num = pf->hw.pf_id;
-	ctxt.vf_num = 0;
-	aq_ret = i40e_aq_get_vsi_params(&pf->hw, &ctxt, NULL);
-	if (aq_ret) {
-		dev_info(&pf->pdev->dev,
-			 "%s couldn't get pf vsi config, err %d, aq_err %d\n",
-			 __func__, aq_ret, pf->hw.aq.asq_last_status);
-		return;
-	}
-	ctxt.flags = I40E_AQ_VSI_TYPE_PF;
-	ctxt.info.valid_sections = cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
-	ctxt.info.switch_id &= ~cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
-
-	aq_ret = i40e_aq_update_vsi_params(&vsi->back->hw, &ctxt, NULL);
-	if (aq_ret) {
-		dev_info(&pf->pdev->dev,
-			 "%s: update vsi switch failed, aq_err=%d\n",
-			 __func__, vsi->back->hw.aq.asq_last_status);
-	}
-}
-
-/**
  * i40e_free_vfs
- * @pf: pointer to the pf structure
+ * @pf: pointer to the PF structure
  *
- * free vf resources
+ * free VF resources
  **/
 void i40e_free_vfs(struct i40e_pf *pf)
 {
@@ -803,10 +739,12 @@ void i40e_free_vfs(struct i40e_pf *pf)
 	 */
 	if (!pci_vfs_assigned(pf->pdev))
 		pci_disable_sriov(pf->pdev);
+	else
+		dev_warn(&pf->pdev->dev, "VFs are assigned - not disabling SR-IOV\n");
 
 	msleep(20); /* let any messages in transit get finished up */
 
-	/* free up vf resources */
+	/* free up VF resources */
 	tmp = pf->num_alloc_vfs;
 	pf->num_alloc_vfs = 0;
 	for (i = 0; i < tmp; i++) {
@@ -832,10 +770,6 @@ void i40e_free_vfs(struct i40e_pf *pf)
 			bit_idx = (hw->func_caps.vf_base_id + vf_id) % 32;
 			wr32(hw, I40E_GLGEN_VFLRSTAT(reg_idx), (1 << bit_idx));
 		}
-		i40e_disable_pf_switch_lb(pf);
-	} else {
-		dev_warn(&pf->pdev->dev,
-			 "unable to disable SR-IOV because VFs are assigned.\n");
 	}
 	clear_bit(__I40E_VF_DISABLE, &pf->state);
 }
@@ -843,10 +777,10 @@ void i40e_free_vfs(struct i40e_pf *pf)
 #ifdef CONFIG_PCI_IOV
 /**
  * i40e_alloc_vfs
- * @pf: pointer to the pf structure
- * @num_alloc_vfs: number of vfs to allocate
+ * @pf: pointer to the PF structure
+ * @num_alloc_vfs: number of VFs to allocate
  *
- * allocate vf resources
+ * allocate VF resources
  **/
 int i40e_alloc_vfs(struct i40e_pf *pf, u16 num_alloc_vfs)
 {
@@ -883,15 +817,14 @@ int i40e_alloc_vfs(struct i40e_pf *pf, u16 num_alloc_vfs)
 		/* assign default capabilities */
 		set_bit(I40E_VIRTCHNL_VF_CAP_L2, &vfs[i].vf_caps);
 		vfs[i].spoofchk = true;
-		/* vf resources get allocated during reset */
+		/* VF resources get allocated during reset */
 		i40e_reset_vf(&vfs[i], false);
 
-		/* enable vf vplan_qtable mappings */
+		/* enable VF vplan_qtable mappings */
 		i40e_enable_vf_mappings(&vfs[i]);
 	}
 	pf->num_alloc_vfs = num_alloc_vfs;
 
-	i40e_enable_pf_switch_lb(pf);
 err_alloc:
 	if (ret)
 		i40e_free_vfs(pf);
@@ -905,7 +838,7 @@ err_iov:
 /**
  * i40e_pci_sriov_enable
  * @pdev: pointer to a pci_dev structure
- * @num_vfs: number of vfs to allocate
+ * @num_vfs: number of VFs to allocate
  *
  * Enable or change the number of VFs
  **/
@@ -945,7 +878,7 @@ err_out:
 /**
  * i40e_pci_sriov_configure
  * @pdev: pointer to a pci_dev structure
- * @num_vfs: number of vfs to allocate
+ * @num_vfs: number of VFs to allocate
  *
  * Enable or change the number of VFs. Called when the user updates the number
  * of VFs in sysfs.
@@ -970,13 +903,13 @@ int i40e_pci_sriov_configure(struct pci_dev *pdev, int num_vfs)
 
 /**
  * i40e_vc_send_msg_to_vf
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @v_opcode: virtual channel opcode
  * @v_retval: virtual channel return value
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * send msg to vf
+ * send msg to VF
  **/
 static int i40e_vc_send_msg_to_vf(struct i40e_vf *vf, u32 v_opcode,
 				  u32 v_retval, u8 *msg, u16 msglen)
@@ -1025,11 +958,11 @@ static int i40e_vc_send_msg_to_vf(struct i40e_vf *vf, u32 v_opcode,
 
 /**
  * i40e_vc_send_resp_to_vf
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @opcode: operation code
  * @retval: return value
  *
- * send resp msg to vf
+ * send resp msg to VF
  **/
 static int i40e_vc_send_resp_to_vf(struct i40e_vf *vf,
 				   enum i40e_virtchnl_ops opcode,
@@ -1040,9 +973,9 @@ static int i40e_vc_send_resp_to_vf(struct i40e_vf *vf,
 
 /**
  * i40e_vc_get_version_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  *
- * called from the vf to request the API version used by the PF
+ * called from the VF to request the API version used by the PF
  **/
 static int i40e_vc_get_version_msg(struct i40e_vf *vf)
 {
@@ -1058,11 +991,11 @@ static int i40e_vc_get_version_msg(struct i40e_vf *vf)
 
 /**
  * i40e_vc_get_vf_resources_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to request its resources
+ * called from the VF to request its resources
  **/
 static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf)
 {
@@ -1090,18 +1023,18 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf)
 	}
 
 	vfres->vf_offload_flags = I40E_VIRTCHNL_VF_OFFLOAD_L2;
-	vsi = pf->vsi[vf->lan_vsi_index];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!vsi->info.pvid)
 		vfres->vf_offload_flags |= I40E_VIRTCHNL_VF_OFFLOAD_VLAN;
 
 	vfres->num_vsis = num_vsis;
 	vfres->num_queue_pairs = vf->num_queue_pairs;
 	vfres->max_vectors = pf->hw.func_caps.num_msix_vectors_vf;
-	if (vf->lan_vsi_index) {
-		vfres->vsi_res[i].vsi_id = vf->lan_vsi_index;
+	if (vf->lan_vsi_idx) {
+		vfres->vsi_res[i].vsi_id = vf->lan_vsi_id;
 		vfres->vsi_res[i].vsi_type = I40E_VSI_SRIOV;
 		vfres->vsi_res[i].num_queue_pairs =
-		    pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs;
+		    pf->vsi[vf->lan_vsi_idx]->alloc_queue_pairs;
 		memcpy(vfres->vsi_res[i].default_mac_addr,
 		       vf->default_lan_addr.addr, ETH_ALEN);
 		i++;
@@ -1109,7 +1042,7 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf)
 	set_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states);
 
 err:
-	/* send the response back to the vf */
+	/* send the response back to the VF */
 	ret = i40e_vc_send_msg_to_vf(vf, I40E_VIRTCHNL_OP_GET_VF_RESOURCES,
 				     aq_ret, (u8 *)vfres, len);
 
@@ -1119,13 +1052,13 @@ err:
 
 /**
  * i40e_vc_reset_vf_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to reset itself,
- * unlike other virtchnl messages, pf driver
- * doesn't send the response back to the vf
+ * called from the VF to reset itself,
+ * unlike other virtchnl messages, PF driver
+ * doesn't send the response back to the VF
  **/
 static void i40e_vc_reset_vf_msg(struct i40e_vf *vf)
 {
@@ -1135,12 +1068,12 @@ static void i40e_vc_reset_vf_msg(struct i40e_vf *vf)
 
 /**
  * i40e_vc_config_promiscuous_mode_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to configure the promiscuous mode of
- * vf vsis
+ * called from the VF to configure the promiscuous mode of
+ * VF vsis
  **/
 static int i40e_vc_config_promiscuous_mode_msg(struct i40e_vf *vf,
 					       u8 *msg, u16 msglen)
@@ -1153,21 +1086,21 @@ static int i40e_vc_config_promiscuous_mode_msg(struct i40e_vf *vf,
 	bool allmulti = false;
 	i40e_status aq_ret;
 
+	vsi = i40e_find_vsi_from_id(pf, info->vsi_id);
 	if (!test_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states) ||
 	    !test_bit(I40E_VIRTCHNL_VF_CAP_PRIVILEGE, &vf->vf_caps) ||
 	    !i40e_vc_isvalid_vsi_id(vf, info->vsi_id) ||
-	    (pf->vsi[info->vsi_id]->type != I40E_VSI_FCOE)) {
+	    (vsi->type != I40E_VSI_FCOE)) {
 		aq_ret = I40E_ERR_PARAM;
 		goto error_param;
 	}
-	vsi = pf->vsi[info->vsi_id];
 	if (info->flags & I40E_FLAG_VF_MULTICAST_PROMISC)
 		allmulti = true;
 	aq_ret = i40e_aq_set_vsi_multicast_promiscuous(hw, vsi->seid,
 						       allmulti, NULL);
 
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf,
 				       I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE,
 				       aq_ret);
@@ -1175,11 +1108,11 @@ error_param:
 
 /**
  * i40e_vc_config_queues_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to configure the rx/tx
+ * called from the VF to configure the rx/tx
  * queues
  **/
 static int i40e_vc_config_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
@@ -1221,22 +1154,22 @@ static int i40e_vc_config_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 			goto error_param;
 		}
 	}
-	/* set vsi num_queue_pairs in use to num configured by vf */
-	pf->vsi[vf->lan_vsi_index]->num_queue_pairs = qci->num_queue_pairs;
+	/* set vsi num_queue_pairs in use to num configured by VF */
+	pf->vsi[vf->lan_vsi_idx]->num_queue_pairs = qci->num_queue_pairs;
 
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES,
 				       aq_ret);
 }
 
 /**
  * i40e_vc_config_irq_map_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to configure the irq to
+ * called from the VF to configure the irq to
  * queue map
  **/
 static int i40e_vc_config_irq_map_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
@@ -1288,18 +1221,18 @@ static int i40e_vc_config_irq_map_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		i40e_config_irq_link_list(vf, vsi_id, map);
 	}
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP,
 				       aq_ret);
 }
 
 /**
  * i40e_vc_enable_queues_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to enable all or specific queue(s)
+ * called from the VF to enable all or specific queue(s)
  **/
 static int i40e_vc_enable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
@@ -1323,21 +1256,22 @@ static int i40e_vc_enable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		aq_ret = I40E_ERR_PARAM;
 		goto error_param;
 	}
-	if (i40e_vsi_control_rings(pf->vsi[vsi_id], true))
+
+	if (i40e_vsi_control_rings(pf->vsi[vf->lan_vsi_idx], true))
 		aq_ret = I40E_ERR_TIMEOUT;
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_ENABLE_QUEUES,
 				       aq_ret);
 }
 
 /**
  * i40e_vc_disable_queues_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to disable all or specific
+ * called from the VF to disable all or specific
  * queue(s)
  **/
 static int i40e_vc_disable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
@@ -1345,7 +1279,6 @@ static int i40e_vc_disable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	struct i40e_virtchnl_queue_select *vqs =
 	    (struct i40e_virtchnl_queue_select *)msg;
 	struct i40e_pf *pf = vf->pf;
-	u16 vsi_id = vqs->vsi_id;
 	i40e_status aq_ret = 0;
 
 	if (!test_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states)) {
@@ -1362,22 +1295,23 @@ static int i40e_vc_disable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		aq_ret = I40E_ERR_PARAM;
 		goto error_param;
 	}
-	if (i40e_vsi_control_rings(pf->vsi[vsi_id], false))
+
+	if (i40e_vsi_control_rings(pf->vsi[vf->lan_vsi_idx], false))
 		aq_ret = I40E_ERR_TIMEOUT;
 
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_DISABLE_QUEUES,
 				       aq_ret);
 }
 
 /**
  * i40e_vc_get_stats_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
- * called from the vf to get vsi stats
+ * called from the VF to get vsi stats
  **/
 static int i40e_vc_get_stats_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
@@ -1400,7 +1334,7 @@ static int i40e_vc_get_stats_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		goto error_param;
 	}
 
-	vsi = pf->vsi[vqs->vsi_id];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!vsi) {
 		aq_ret = I40E_ERR_PARAM;
 		goto error_param;
@@ -1409,14 +1343,14 @@ static int i40e_vc_get_stats_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	stats = vsi->eth_stats;
 
 error_param:
-	/* send the response back to the vf */
+	/* send the response back to the VF */
 	return i40e_vc_send_msg_to_vf(vf, I40E_VIRTCHNL_OP_GET_STATS, aq_ret,
 				      (u8 *)&stats, sizeof(stats));
 }
 
 /**
  * i40e_check_vf_permission
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @macaddr: pointer to the MAC Address being checked
  *
  * Check if the VF has permission to add or delete unicast MAC address
@@ -1450,7 +1384,7 @@ static inline int i40e_check_vf_permission(struct i40e_vf *vf, u8 *macaddr)
 
 /**
  * i40e_vc_add_mac_addr_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
@@ -1478,7 +1412,7 @@ static int i40e_vc_add_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		if (ret)
 			goto error_param;
 	}
-	vsi = pf->vsi[vsi_id];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 
 	/* add new addresses to the list */
 	for (i = 0; i < al->num_elements; i++) {
@@ -1507,14 +1441,14 @@ static int i40e_vc_add_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		dev_err(&pf->pdev->dev, "Unable to program VF MAC filters\n");
 
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS,
 				       ret);
 }
 
 /**
  * i40e_vc_del_mac_addr_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
@@ -1546,7 +1480,7 @@ static int i40e_vc_del_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 			goto error_param;
 		}
 	}
-	vsi = pf->vsi[vsi_id];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 
 	/* delete addresses from the list */
 	for (i = 0; i < al->num_elements; i++)
@@ -1558,14 +1492,14 @@ static int i40e_vc_del_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		dev_err(&pf->pdev->dev, "Unable to program VF MAC filters\n");
 
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS,
 				       ret);
 }
 
 /**
  * i40e_vc_add_vlan_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
@@ -1596,7 +1530,7 @@ static int i40e_vc_add_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 			goto error_param;
 		}
 	}
-	vsi = pf->vsi[vsi_id];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (vsi->info.pvid) {
 		aq_ret = I40E_ERR_PARAM;
 		goto error_param;
@@ -1613,13 +1547,13 @@ static int i40e_vc_add_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	}
 
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_ADD_VLAN, aq_ret);
 }
 
 /**
  * i40e_vc_remove_vlan_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  *
@@ -1649,7 +1583,7 @@ static int i40e_vc_remove_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		}
 	}
 
-	vsi = pf->vsi[vsi_id];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (vsi->info.pvid) {
 		aq_ret = I40E_ERR_PARAM;
 		goto error_param;
@@ -1664,13 +1598,13 @@ static int i40e_vc_remove_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	}
 
 error_param:
-	/* send the response to the vf */
+	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_DEL_VLAN, aq_ret);
 }
 
 /**
  * i40e_vc_validate_vf_msg
- * @vf: pointer to the vf info
+ * @vf: pointer to the VF info
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  * @msghndl: msg handle
@@ -1776,14 +1710,14 @@ static int i40e_vc_validate_vf_msg(struct i40e_vf *vf, u32 v_opcode,
 
 /**
  * i40e_vc_process_vf_msg
- * @pf: pointer to the pf structure
- * @vf_id: source vf id
+ * @pf: pointer to the PF structure
+ * @vf_id: source VF id
  * @msg: pointer to the msg buffer
  * @msglen: msg length
  * @msghndl: msg handle
  *
  * called from the common aeq/arq handler to
- * process request from vf
+ * process request from VF
  **/
 int i40e_vc_process_vf_msg(struct i40e_pf *pf, u16 vf_id, u32 v_opcode,
 			   u32 v_retval, u8 *msg, u16 msglen)
@@ -1801,7 +1735,7 @@ int i40e_vc_process_vf_msg(struct i40e_pf *pf, u16 vf_id, u32 v_opcode,
 	ret = i40e_vc_validate_vf_msg(vf, v_opcode, v_retval, msg, msglen);
 
 	if (ret) {
-		dev_err(&pf->pdev->dev, "Invalid message from vf %d, opcode %d, len %d\n",
+		dev_err(&pf->pdev->dev, "Invalid message from VF %d, opcode %d, len %d\n",
 			local_vf_id, v_opcode, msglen);
 		return ret;
 	}
@@ -1849,7 +1783,7 @@ int i40e_vc_process_vf_msg(struct i40e_pf *pf, u16 vf_id, u32 v_opcode,
 		break;
 	case I40E_VIRTCHNL_OP_UNKNOWN:
 	default:
-		dev_err(&pf->pdev->dev, "Unsupported opcode %d from vf %d\n",
+		dev_err(&pf->pdev->dev, "Unsupported opcode %d from VF %d\n",
 			v_opcode, local_vf_id);
 		ret = i40e_vc_send_resp_to_vf(vf, v_opcode,
 					      I40E_ERR_NOT_IMPLEMENTED);
@@ -1861,10 +1795,10 @@ int i40e_vc_process_vf_msg(struct i40e_pf *pf, u16 vf_id, u32 v_opcode,
 
 /**
  * i40e_vc_process_vflr_event
- * @pf: pointer to the pf structure
+ * @pf: pointer to the PF structure
  *
  * called from the vlfr irq handler to
- * free up vf resources and state variables
+ * free up VF resources and state variables
  **/
 int i40e_vc_process_vflr_event(struct i40e_pf *pf)
 {
@@ -1885,7 +1819,7 @@ int i40e_vc_process_vflr_event(struct i40e_pf *pf)
 	for (vf_id = 0; vf_id < pf->num_alloc_vfs; vf_id++) {
 		reg_idx = (hw->func_caps.vf_base_id + vf_id) / 32;
 		bit_idx = (hw->func_caps.vf_base_id + vf_id) % 32;
-		/* read GLGEN_VFLRSTAT register to find out the flr vfs */
+		/* read GLGEN_VFLRSTAT register to find out the flr VFs */
 		vf = &pf->vf[vf_id];
 		reg = rd32(hw, I40E_GLGEN_VFLRSTAT(reg_idx));
 		if (reg & (1 << bit_idx)) {
@@ -1902,7 +1836,7 @@ int i40e_vc_process_vflr_event(struct i40e_pf *pf)
 
 /**
  * i40e_vc_vf_broadcast
- * @pf: pointer to the pf structure
+ * @pf: pointer to the PF structure
  * @opcode: operation code
  * @retval: return value
  * @msg: pointer to the msg buffer
@@ -1921,7 +1855,7 @@ static void i40e_vc_vf_broadcast(struct i40e_pf *pf,
 
 	for (i = 0; i < pf->num_alloc_vfs; i++, vf++) {
 		int abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
-		/* Not all vfs are enabled so skip the ones that are not */
+		/* Not all VFs are enabled so skip the ones that are not */
 		if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states) &&
 		    !test_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states))
 			continue;
@@ -1936,7 +1870,7 @@ static void i40e_vc_vf_broadcast(struct i40e_pf *pf,
 
 /**
  * i40e_vc_notify_link_state
- * @pf: pointer to the pf structure
+ * @pf: pointer to the PF structure
  *
  * send a link status message to all VFs on a given PF
  **/
@@ -1969,7 +1903,7 @@ void i40e_vc_notify_link_state(struct i40e_pf *pf)
 
 /**
  * i40e_vc_notify_reset
- * @pf: pointer to the pf structure
+ * @pf: pointer to the PF structure
  *
  * indicate a pending reset to all VFs on a given PF
  **/
@@ -1985,7 +1919,7 @@ void i40e_vc_notify_reset(struct i40e_pf *pf)
 
 /**
  * i40e_vc_notify_vf_reset
- * @vf: pointer to the vf structure
+ * @vf: pointer to the VF structure
  *
  * indicate a pending reset to the given VF
  **/
@@ -2015,10 +1949,10 @@ void i40e_vc_notify_vf_reset(struct i40e_vf *vf)
 /**
  * i40e_ndo_set_vf_mac
  * @netdev: network interface device structure
- * @vf_id: vf identifier
+ * @vf_id: VF identifier
  * @mac: mac address
  *
- * program vf mac address
+ * program VF mac address
  **/
 int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac)
 {
@@ -2038,7 +1972,7 @@ int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac)
 	}
 
 	vf = &(pf->vf[vf_id]);
-	vsi = pf->vsi[vf->lan_vsi_index];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
 		dev_err(&pf->pdev->dev,
 			"Uninitialized VF %d\n", vf_id);
@@ -2083,11 +2017,11 @@ error_param:
 /**
  * i40e_ndo_set_vf_port_vlan
  * @netdev: network interface device structure
- * @vf_id: vf identifier
+ * @vf_id: VF identifier
  * @vlan_id: mac address
  * @qos: priority setting
  *
- * program vf vlan id and/or qos
+ * program VF vlan id and/or qos
  **/
 int i40e_ndo_set_vf_port_vlan(struct net_device *netdev,
 			      int vf_id, u16 vlan_id, u8 qos)
@@ -2112,7 +2046,7 @@ int i40e_ndo_set_vf_port_vlan(struct net_device *netdev,
 	}
 
 	vf = &(pf->vf[vf_id]);
-	vsi = pf->vsi[vf->lan_vsi_index];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
 		dev_err(&pf->pdev->dev, "Uninitialized VF %d\n", vf_id);
 		ret = -EINVAL;
@@ -2196,10 +2130,10 @@ error_pvid:
 /**
  * i40e_ndo_set_vf_bw
  * @netdev: network interface device structure
- * @vf_id: vf identifier
- * @tx_rate: tx rate
+ * @vf_id: VF identifier
+ * @tx_rate: Tx rate
  *
- * configure vf tx rate
+ * configure VF Tx rate
  **/
 int i40e_ndo_set_vf_bw(struct net_device *netdev, int vf_id, int min_tx_rate,
 		       int max_tx_rate)
@@ -2219,13 +2153,13 @@ int i40e_ndo_set_vf_bw(struct net_device *netdev, int vf_id, int min_tx_rate,
 	}
 
 	if (min_tx_rate) {
-		dev_err(&pf->pdev->dev, "Invalid min tx rate (%d) (greater than 0) specified for vf %d.\n",
+		dev_err(&pf->pdev->dev, "Invalid min tx rate (%d) (greater than 0) specified for VF %d.\n",
 			min_tx_rate, vf_id);
 		return -EINVAL;
 	}
 
 	vf = &(pf->vf[vf_id]);
-	vsi = pf->vsi[vf->lan_vsi_index];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
 		dev_err(&pf->pdev->dev, "Uninitialized VF %d.\n", vf_id);
 		ret = -EINVAL;
@@ -2247,7 +2181,7 @@ int i40e_ndo_set_vf_bw(struct net_device *netdev, int vf_id, int min_tx_rate,
 	}
 
 	if (max_tx_rate > speed) {
-		dev_err(&pf->pdev->dev, "Invalid max tx rate %d specified for vf %d.",
+		dev_err(&pf->pdev->dev, "Invalid max tx rate %d specified for VF %d.",
 			max_tx_rate, vf->vf_id);
 		ret = -EINVAL;
 		goto error;
@@ -2276,10 +2210,10 @@ error:
 /**
  * i40e_ndo_get_vf_config
  * @netdev: network interface device structure
- * @vf_id: vf identifier
- * @ivi: vf configuration structure
+ * @vf_id: VF identifier
+ * @ivi: VF configuration structure
  *
- * return vf configuration
+ * return VF configuration
  **/
 int i40e_ndo_get_vf_config(struct net_device *netdev,
 			   int vf_id, struct ifla_vf_info *ivi)
@@ -2299,7 +2233,7 @@ int i40e_ndo_get_vf_config(struct net_device *netdev,
 
 	vf = &(pf->vf[vf_id]);
 	/* first vsi is always the LAN vsi */
-	vsi = pf->vsi[vf->lan_vsi_index];
+	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
 		dev_err(&pf->pdev->dev, "Uninitialized VF %d\n", vf_id);
 		ret = -EINVAL;
@@ -2331,7 +2265,7 @@ error_param:
 /**
  * i40e_ndo_set_vf_link_state
  * @netdev: network interface device structure
- * @vf_id: vf identifier
+ * @vf_id: VF identifier
  * @link: required link state
  *
  * Set the link state of a specified VF, regardless of physical link state
@@ -2394,7 +2328,7 @@ error_out:
 /**
  * i40e_ndo_set_vf_spoofchk
  * @netdev: network interface device structure
- * @vf_id: vf identifier
+ * @vf_id: VF identifier
  * @enable: flag to enable or disable feature
  *
  * Enable or disable VF spoof checking
@@ -2423,11 +2357,12 @@ int i40e_ndo_set_vf_spoofchk(struct net_device *netdev, int vf_id, bool enable)
 
 	vf->spoofchk = enable;
 	memset(&ctxt, 0, sizeof(ctxt));
-	ctxt.seid = pf->vsi[vf->lan_vsi_index]->seid;
+	ctxt.seid = pf->vsi[vf->lan_vsi_idx]->seid;
 	ctxt.pf_num = pf->hw.pf_id;
 	ctxt.info.valid_sections = cpu_to_le16(I40E_AQ_VSI_PROP_SECURITY_VALID);
 	if (enable)
-		ctxt.info.sec_flags |= I40E_AQ_VSI_SEC_FLAG_ENABLE_MAC_CHK;
+		ctxt.info.sec_flags |= (I40E_AQ_VSI_SEC_FLAG_ENABLE_VLAN_CHK |
+					I40E_AQ_VSI_SEC_FLAG_ENABLE_MAC_CHK);
 	ret = i40e_aq_update_vsi_params(hw, &ctxt, NULL);
 	if (ret) {
 		dev_err(&pf->pdev->dev, "Error %d updating VSI parameters\n",
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
index 9452f5247cff..09043c1aae54 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -71,12 +71,12 @@ enum i40e_vf_capabilities {
 struct i40e_vf {
 	struct i40e_pf *pf;
 
-	/* vf id in the pf space */
+	/* VF id in the PF space */
 	u16 vf_id;
-	/* all vf vsis connect to the same parent */
+	/* all VF vsis connect to the same parent */
 	enum i40e_switch_element_types parent_type;
 
-	/* vf Port Extender (PE) stag if used */
+	/* VF Port Extender (PE) stag if used */
 	u16 stag;
 
 	struct i40e_virtchnl_ether_addr default_lan_addr;
@@ -88,10 +88,10 @@ struct i40e_vf {
 	 * When assigned, these will be non-zero, because VSI 0 is always
 	 * the main LAN VSI for the PF.
 	 */
-	u8 lan_vsi_index;	/* index into PF struct */
+	u8 lan_vsi_idx;	        /* index into PF struct */
 	u8 lan_vsi_id;		/* ID as used by firmware */
 
-	u8 num_queue_pairs;	/* num of qps assigned to vf vsis */
+	u8 num_queue_pairs;	/* num of qps assigned to VF vsis */
 	u64 num_mdd_events;	/* num of mdd events detected */
 	u64 num_invalid_msgs;	/* num of malformed or invalid msgs detected */
 	u64 num_valid_msgs;	/* num of valid msgs detected */
@@ -100,7 +100,7 @@ struct i40e_vf {
 	unsigned long vf_states;	/* vf's runtime states */
 	unsigned int tx_rate;	/* Tx bandwidth limit in Mbps */
 	bool link_forced;
-	bool link_up;		/* only valid if vf link is forced */
+	bool link_up;		/* only valid if VF link is forced */
 	bool spoofchk;
 };
 
@@ -113,7 +113,7 @@ int i40e_vc_process_vflr_event(struct i40e_pf *pf);
 void i40e_reset_vf(struct i40e_vf *vf, bool flr);
 void i40e_vc_notify_vf_reset(struct i40e_vf *vf);
 
-/* vf configuration related iplink handlers */
+/* VF configuration related iplink handlers */
 int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac);
 int i40e_ndo_set_vf_port_vlan(struct net_device *netdev,
 			      int vf_id, u16 vlan_id, u8 qos);
@@ -126,6 +126,5 @@ int i40e_ndo_set_vf_spoofchk(struct net_device *netdev, int vf_id, bool enable);
 
 void i40e_vc_notify_link_state(struct i40e_pf *pf);
 void i40e_vc_notify_reset(struct i40e_pf *pf);
-void i40e_enable_pf_switch_lb(struct i40e_pf *pf);
 
 #endif /* _I40E_VIRTCHNL_PF_H_ */
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_adminq.h b/drivers/net/ethernet/intel/i40evf/i40e_adminq.h
index 60f04e96a80e..ef43d68f67b3 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_adminq.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_adminq.h
@@ -93,6 +93,7 @@ struct i40e_adminq_info {
 	u16 asq_buf_size;               /* send queue buffer size */
 	u16 fw_maj_ver;                 /* firmware major version */
 	u16 fw_min_ver;                 /* firmware minor version */
+	u32 fw_build;                   /* firmware build number */
 	u16 api_maj_ver;                /* api major version */
 	u16 api_min_ver;                /* api minor version */
 	bool nvm_release_on_done;
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_common.c b/drivers/net/ethernet/intel/i40evf/i40e_common.c
index 28c40c57d4f5..39fcb1dc4ea6 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_common.c
@@ -51,6 +51,7 @@ i40e_status i40e_set_mac_type(struct i40e_hw *hw)
 		case I40E_DEV_ID_QSFP_B:
 		case I40E_DEV_ID_QSFP_C:
 		case I40E_DEV_ID_10G_BASE_T:
+		case I40E_DEV_ID_20G_KR2:
 			hw->mac.type = I40E_MAC_XL710;
 			break;
 		case I40E_DEV_ID_VF:
@@ -85,46 +86,53 @@ void i40evf_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
 {
 	struct i40e_aq_desc *aq_desc = (struct i40e_aq_desc *)desc;
 	u16 len = le16_to_cpu(aq_desc->datalen);
-	u8 *aq_buffer = (u8 *)buffer;
-	u32 data[4];
-	u32 i = 0;
+	u8 *buf = (u8 *)buffer;
+	u16 i = 0;
 
 	if ((!(mask & hw->debug_mask)) || (desc == NULL))
 		return;
 
 	i40e_debug(hw, mask,
 		   "AQ CMD: opcode 0x%04X, flags 0x%04X, datalen 0x%04X, retval 0x%04X\n",
-		   aq_desc->opcode, aq_desc->flags, aq_desc->datalen,
-		   aq_desc->retval);
+		   le16_to_cpu(aq_desc->opcode),
+		   le16_to_cpu(aq_desc->flags),
+		   le16_to_cpu(aq_desc->datalen),
+		   le16_to_cpu(aq_desc->retval));
 	i40e_debug(hw, mask, "\tcookie (h,l) 0x%08X 0x%08X\n",
-		   aq_desc->cookie_high, aq_desc->cookie_low);
+		   le32_to_cpu(aq_desc->cookie_high),
+		   le32_to_cpu(aq_desc->cookie_low));
 	i40e_debug(hw, mask, "\tparam (0,1)  0x%08X 0x%08X\n",
-		   aq_desc->params.internal.param0,
-		   aq_desc->params.internal.param1);
+		   le32_to_cpu(aq_desc->params.internal.param0),
+		   le32_to_cpu(aq_desc->params.internal.param1));
 	i40e_debug(hw, mask, "\taddr (h,l)   0x%08X 0x%08X\n",
-		   aq_desc->params.external.addr_high,
-		   aq_desc->params.external.addr_low);
+		   le32_to_cpu(aq_desc->params.external.addr_high),
+		   le32_to_cpu(aq_desc->params.external.addr_low));
 
 	if ((buffer != NULL) && (aq_desc->datalen != 0)) {
-		memset(data, 0, sizeof(data));
 		i40e_debug(hw, mask, "AQ CMD Buffer:\n");
 		if (buf_len < len)
 			len = buf_len;
-		for (i = 0; i < len; i++) {
-			data[((i % 16) / 4)] |=
-				((u32)aq_buffer[i]) << (8 * (i % 4));
-			if ((i % 16) == 15) {
-				i40e_debug(hw, mask,
-					   "\t0x%04X  %08X %08X %08X %08X\n",
-					   i - 15, data[0], data[1], data[2],
-					   data[3]);
-				memset(data, 0, sizeof(data));
-			}
+		/* write the full 16-byte chunks */
+		for (i = 0; i < (len - 16); i += 16)
+			i40e_debug(hw, mask,
+				   "\t0x%04X  %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X\n",
+				   i, buf[i], buf[i + 1], buf[i + 2],
+				   buf[i + 3], buf[i + 4], buf[i + 5],
+				   buf[i + 6], buf[i + 7], buf[i + 8],
+				   buf[i + 9], buf[i + 10], buf[i + 11],
+				   buf[i + 12], buf[i + 13], buf[i + 14],
+				   buf[i + 15]);
+		/* write whatever's left over without overrunning the buffer */
+		if (i < len) {
+			char d_buf[80];
+			int j = 0;
+
+			memset(d_buf, 0, sizeof(d_buf));
+			j += sprintf(d_buf, "\t0x%04X ", i);
+			while (i < len)
+				j += sprintf(&d_buf[j], " %02X", buf[i++]);
+			i40e_debug(hw, mask, "%s\n", d_buf);
 		}
-		if ((i % 16) != 0)
-			i40e_debug(hw, mask, "\t0x%04X  %08X %08X %08X %08X\n",
-				   i - (i % 16), data[0], data[1], data[2],
-				   data[3]);
 	}
 }
 
@@ -535,7 +543,6 @@ struct i40e_rx_ptype_decoded i40evf_ptype_lookup[] = {
 	I40E_PTT_UNUSED_ENTRY(255)
 };
 
-
 /**
  * i40e_aq_send_msg_to_pf
  * @hw: pointer to the hardware structure
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_prototype.h b/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
index 9173834825ac..58e37a44b80a 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
@@ -59,8 +59,7 @@ void i40evf_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask,
 void i40e_idle_aq(struct i40e_hw *hw);
 void i40evf_resume_aq(struct i40e_hw *hw);
 bool i40evf_check_asq_alive(struct i40e_hw *hw);
-i40e_status i40evf_aq_queue_shutdown(struct i40e_hw *hw,
-					     bool unloading);
+i40e_status i40evf_aq_queue_shutdown(struct i40e_hw *hw, bool unloading);
 
 i40e_status i40e_set_mac_type(struct i40e_hw *hw);
 
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_register.h b/drivers/net/ethernet/intel/i40evf/i40e_register.h
index c1f6a59bfea0..3cc737629bf7 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_register.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_register.h
@@ -310,6 +310,10 @@
 #define I40E_PRTDCB_RUP2TC_UP6TC_MASK I40E_MASK(0x7, I40E_PRTDCB_RUP2TC_UP6TC_SHIFT)
 #define I40E_PRTDCB_RUP2TC_UP7TC_SHIFT 21
 #define I40E_PRTDCB_RUP2TC_UP7TC_MASK I40E_MASK(0x7, I40E_PRTDCB_RUP2TC_UP7TC_SHIFT)
+#define I40E_PRTDCB_RUPTQ(_i) (0x00122400 + ((_i) * 32)) /* _i=0...7 */ /* Reset: CORER */
+#define I40E_PRTDCB_RUPTQ_MAX_INDEX 7
+#define I40E_PRTDCB_RUPTQ_RXQNUM_SHIFT 0
+#define I40E_PRTDCB_RUPTQ_RXQNUM_MASK I40E_MASK(0x3FFF, I40E_PRTDCB_RUPTQ_RXQNUM_SHIFT)
 #define I40E_PRTDCB_TC2PFC 0x001C0980 /* Reset: CORER */
 #define I40E_PRTDCB_TC2PFC_TC2PFC_SHIFT 0
 #define I40E_PRTDCB_TC2PFC_TC2PFC_MASK I40E_MASK(0xFF, I40E_PRTDCB_TC2PFC_TC2PFC_SHIFT)
@@ -421,6 +425,8 @@
 #define I40E_GLGEN_GPIO_CTL_OUT_DEFAULT_MASK I40E_MASK(0x1, I40E_GLGEN_GPIO_CTL_OUT_DEFAULT_SHIFT)
 #define I40E_GLGEN_GPIO_CTL_PHY_PIN_NAME_SHIFT 20
 #define I40E_GLGEN_GPIO_CTL_PHY_PIN_NAME_MASK I40E_MASK(0x3F, I40E_GLGEN_GPIO_CTL_PHY_PIN_NAME_SHIFT)
+#define I40E_GLGEN_GPIO_CTL_PRT_BIT_MAP_SHIFT 26
+#define I40E_GLGEN_GPIO_CTL_PRT_BIT_MAP_MASK I40E_MASK(0xF, I40E_GLGEN_GPIO_CTL_PRT_BIT_MAP_SHIFT)
 #define I40E_GLGEN_GPIO_SET 0x00088184 /* Reset: POR */
 #define I40E_GLGEN_GPIO_SET_GPIO_INDX_SHIFT 0
 #define I40E_GLGEN_GPIO_SET_GPIO_INDX_MASK I40E_MASK(0x1F, I40E_GLGEN_GPIO_SET_GPIO_INDX_SHIFT)
@@ -484,7 +490,9 @@
 #define I40E_GLGEN_MDIO_CTRL_CONTMDC_SHIFT 17
 #define I40E_GLGEN_MDIO_CTRL_CONTMDC_MASK I40E_MASK(0x1, I40E_GLGEN_MDIO_CTRL_CONTMDC_SHIFT)
 #define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_SHIFT 18
-#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_MASK I40E_MASK(0x3FFF, I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_SHIFT)
+#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_MASK I40E_MASK(0x7FF, I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD1_SHIFT)
+#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD0_SHIFT 29
+#define I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD0_MASK I40E_MASK(0x7, I40E_GLGEN_MDIO_CTRL_LEGACY_RSVD0_SHIFT)
 #define I40E_GLGEN_MDIO_I2C_SEL(_i) (0x000881C0 + ((_i) * 4)) /* _i=0...3 */ /* Reset: POR */
 #define I40E_GLGEN_MDIO_I2C_SEL_MAX_INDEX 3
 #define I40E_GLGEN_MDIO_I2C_SEL_MDIO_I2C_SEL_SHIFT 0
@@ -548,9 +556,6 @@
 #define I40E_GLGEN_RSTCTL_GRSTDEL_MASK I40E_MASK(0x3F, I40E_GLGEN_RSTCTL_GRSTDEL_SHIFT)
 #define I40E_GLGEN_RSTCTL_ECC_RST_ENA_SHIFT 8
 #define I40E_GLGEN_RSTCTL_ECC_RST_ENA_MASK I40E_MASK(0x1, I40E_GLGEN_RSTCTL_ECC_RST_ENA_SHIFT)
-#define I40E_GLGEN_RSTENA_EMP 0x000B818C /* Reset: POR */
-#define I40E_GLGEN_RSTENA_EMP_EMP_RST_ENA_SHIFT 0
-#define I40E_GLGEN_RSTENA_EMP_EMP_RST_ENA_MASK I40E_MASK(0x1, I40E_GLGEN_RSTENA_EMP_EMP_RST_ENA_SHIFT)
 #define I40E_GLGEN_RTRIG 0x000B8190 /* Reset: CORER */
 #define I40E_GLGEN_RTRIG_CORER_SHIFT 0
 #define I40E_GLGEN_RTRIG_CORER_MASK I40E_MASK(0x1, I40E_GLGEN_RTRIG_CORER_SHIFT)
@@ -1066,7 +1071,7 @@
 #define I40E_PFINT_RATEN_INTERVAL_MASK I40E_MASK(0x3F, I40E_PFINT_RATEN_INTERVAL_SHIFT)
 #define I40E_PFINT_RATEN_INTRL_ENA_SHIFT 6
 #define I40E_PFINT_RATEN_INTRL_ENA_MASK I40E_MASK(0x1, I40E_PFINT_RATEN_INTRL_ENA_SHIFT)
-#define I40E_PFINT_STAT_CTL0 0x00038400 /* Reset: PFR */
+#define I40E_PFINT_STAT_CTL0 0x00038400 /* Reset: CORER */
 #define I40E_PFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT 2
 #define I40E_PFINT_STAT_CTL0_OTHER_ITR_INDX_MASK I40E_MASK(0x3, I40E_PFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT)
 #define I40E_QINT_RQCTL(_Q) (0x0003A000 + ((_Q) * 4)) /* _i=0...1535 */ /* Reset: CORER */
@@ -1171,7 +1176,7 @@
 #define I40E_VFINT_ITRN_MAX_INDEX 2
 #define I40E_VFINT_ITRN_INTERVAL_SHIFT 0
 #define I40E_VFINT_ITRN_INTERVAL_MASK I40E_MASK(0xFFF, I40E_VFINT_ITRN_INTERVAL_SHIFT)
-#define I40E_VFINT_STAT_CTL0(_VF) (0x0002A000 + ((_VF) * 4)) /* _i=0...127 */ /* Reset: VFR */
+#define I40E_VFINT_STAT_CTL0(_VF) (0x0002A000 + ((_VF) * 4)) /* _i=0...127 */ /* Reset: CORER */
 #define I40E_VFINT_STAT_CTL0_MAX_INDEX 127
 #define I40E_VFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT 2
 #define I40E_VFINT_STAT_CTL0_OTHER_ITR_INDX_MASK I40E_MASK(0x3, I40E_VFINT_STAT_CTL0_OTHER_ITR_INDX_SHIFT)
@@ -1803,9 +1808,6 @@
 #define I40E_GLPCI_GSCN_0_3_MAX_INDEX 3
 #define I40E_GLPCI_GSCN_0_3_EVENT_COUNTER_SHIFT 0
 #define I40E_GLPCI_GSCN_0_3_EVENT_COUNTER_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPCI_GSCN_0_3_EVENT_COUNTER_SHIFT)
-#define I40E_GLPCI_LATCT 0x0009C4B4 /* Reset: PCIR */
-#define I40E_GLPCI_LATCT_PCI_COUNT_LAT_CT_SHIFT 0
-#define I40E_GLPCI_LATCT_PCI_COUNT_LAT_CT_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPCI_LATCT_PCI_COUNT_LAT_CT_SHIFT)
 #define I40E_GLPCI_LBARCTRL 0x000BE484 /* Reset: POR */
 #define I40E_GLPCI_LBARCTRL_PREFBAR_SHIFT 0
 #define I40E_GLPCI_LBARCTRL_PREFBAR_MASK I40E_MASK(0x1, I40E_GLPCI_LBARCTRL_PREFBAR_SHIFT)
@@ -1902,6 +1904,11 @@
 #define I40E_GLPCI_VFSUP_VF_PREFETCH_MASK I40E_MASK(0x1, I40E_GLPCI_VFSUP_VF_PREFETCH_SHIFT)
 #define I40E_GLPCI_VFSUP_VR_BAR_TYPE_SHIFT 1
 #define I40E_GLPCI_VFSUP_VR_BAR_TYPE_MASK I40E_MASK(0x1, I40E_GLPCI_VFSUP_VR_BAR_TYPE_SHIFT)
+#define I40E_GLTPH_CTRL 0x000BE480 /* Reset: PCIR */
+#define I40E_GLTPH_CTRL_DESC_PH_SHIFT 9
+#define I40E_GLTPH_CTRL_DESC_PH_MASK I40E_MASK(0x3, I40E_GLTPH_CTRL_DESC_PH_SHIFT)
+#define I40E_GLTPH_CTRL_DATA_PH_SHIFT 11
+#define I40E_GLTPH_CTRL_DATA_PH_MASK I40E_MASK(0x3, I40E_GLTPH_CTRL_DATA_PH_SHIFT)
 #define I40E_PF_FUNC_RID 0x0009C000 /* Reset: PCIR */
 #define I40E_PF_FUNC_RID_FUNCTION_NUMBER_SHIFT 0
 #define I40E_PF_FUNC_RID_FUNCTION_NUMBER_MASK I40E_MASK(0x7, I40E_PF_FUNC_RID_FUNCTION_NUMBER_SHIFT)
@@ -2374,20 +2381,20 @@
 #define I40E_GL_RXERR2_L_FCOEDIXAC_MASK I40E_MASK(0xFFFFFFFF, I40E_GL_RXERR2_L_FCOEDIXAC_SHIFT)
 #define I40E_GLPRT_BPRCH(_i) (0x003005E4 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPRCH_MAX_INDEX 3
-#define I40E_GLPRT_BPRCH_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPRCH_UPRCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPRCH_UPRCH_SHIFT)
+#define I40E_GLPRT_BPRCH_BPRCH_SHIFT 0
+#define I40E_GLPRT_BPRCH_BPRCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPRCH_BPRCH_SHIFT)
 #define I40E_GLPRT_BPRCL(_i) (0x003005E0 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPRCL_MAX_INDEX 3
-#define I40E_GLPRT_BPRCL_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPRCL_UPRCH_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPRCL_UPRCH_SHIFT)
+#define I40E_GLPRT_BPRCL_BPRCL_SHIFT 0
+#define I40E_GLPRT_BPRCL_BPRCL_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPRCL_BPRCL_SHIFT)
 #define I40E_GLPRT_BPTCH(_i) (0x00300A04 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPTCH_MAX_INDEX 3
-#define I40E_GLPRT_BPTCH_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPTCH_UPRCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPTCH_UPRCH_SHIFT)
+#define I40E_GLPRT_BPTCH_BPTCH_SHIFT 0
+#define I40E_GLPRT_BPTCH_BPTCH_MASK I40E_MASK(0xFFFF, I40E_GLPRT_BPTCH_BPTCH_SHIFT)
 #define I40E_GLPRT_BPTCL(_i) (0x00300A00 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_BPTCL_MAX_INDEX 3
-#define I40E_GLPRT_BPTCL_UPRCH_SHIFT 0
-#define I40E_GLPRT_BPTCL_UPRCH_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPTCL_UPRCH_SHIFT)
+#define I40E_GLPRT_BPTCL_BPTCL_SHIFT 0
+#define I40E_GLPRT_BPTCL_BPTCL_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_BPTCL_BPTCL_SHIFT)
 #define I40E_GLPRT_CRCERRS(_i) (0x00300080 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_CRCERRS_MAX_INDEX 3
 #define I40E_GLPRT_CRCERRS_CRCERRS_SHIFT 0
@@ -2620,10 +2627,6 @@
 #define I40E_GLPRT_TDOLD_MAX_INDEX 3
 #define I40E_GLPRT_TDOLD_GLPRT_TDOLD_SHIFT 0
 #define I40E_GLPRT_TDOLD_GLPRT_TDOLD_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_TDOLD_GLPRT_TDOLD_SHIFT)
-#define I40E_GLPRT_TDPC(_i) (0x00375400 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
-#define I40E_GLPRT_TDPC_MAX_INDEX 3
-#define I40E_GLPRT_TDPC_TDPC_SHIFT 0
-#define I40E_GLPRT_TDPC_TDPC_MASK I40E_MASK(0xFFFFFFFF, I40E_GLPRT_TDPC_TDPC_SHIFT)
 #define I40E_GLPRT_UPRCH(_i) (0x003005A4 + ((_i) * 8)) /* _i=0...3 */ /* Reset: CORER */
 #define I40E_GLPRT_UPRCH_MAX_INDEX 3
 #define I40E_GLPRT_UPRCH_UPRCH_SHIFT 0
@@ -2990,9 +2993,6 @@
 #define I40E_PRTTSYN_TXTIME_L 0x001E41C0 /* Reset: GLOBR */
 #define I40E_PRTTSYN_TXTIME_L_TXTIEM_L_SHIFT 0
 #define I40E_PRTTSYN_TXTIME_L_TXTIEM_L_MASK I40E_MASK(0xFFFFFFFF, I40E_PRTTSYN_TXTIME_L_TXTIEM_L_SHIFT)
-#define I40E_GLSCD_QUANTA 0x000B2080 /* Reset: CORER */
-#define I40E_GLSCD_QUANTA_TSCDQUANTA_SHIFT 0
-#define I40E_GLSCD_QUANTA_TSCDQUANTA_MASK I40E_MASK(0x7, I40E_GLSCD_QUANTA_TSCDQUANTA_SHIFT)
 #define I40E_GL_MDET_RX 0x0012A510 /* Reset: CORER */
 #define I40E_GL_MDET_RX_FUNCTION_SHIFT 0
 #define I40E_GL_MDET_RX_FUNCTION_MASK I40E_MASK(0xFF, I40E_GL_MDET_RX_FUNCTION_SHIFT)
@@ -3258,7 +3258,7 @@
 #define I40E_VFINT_ITRN1_MAX_INDEX 2
 #define I40E_VFINT_ITRN1_INTERVAL_SHIFT 0
 #define I40E_VFINT_ITRN1_INTERVAL_MASK I40E_MASK(0xFFF, I40E_VFINT_ITRN1_INTERVAL_SHIFT)
-#define I40E_VFINT_STAT_CTL01 0x00005400 /* Reset: VFR */
+#define I40E_VFINT_STAT_CTL01 0x00005400 /* Reset: CORER */
 #define I40E_VFINT_STAT_CTL01_OTHER_ITR_INDX_SHIFT 2
 #define I40E_VFINT_STAT_CTL01_OTHER_ITR_INDX_MASK I40E_MASK(0x3, I40E_VFINT_STAT_CTL01_OTHER_ITR_INDX_SHIFT)
 #define I40E_QRX_TAIL1(_Q) (0x00002000 + ((_Q) * 4)) /* _i=0...15 */ /* Reset: CORER */
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
index 708891571dae..b077e02a0cc7 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
@@ -25,6 +25,7 @@
  ******************************************************************************/
 
 #include <linux/prefetch.h>
+#include <net/busy_poll.h>
 
 #include "i40evf.h"
 #include "i40e_prototype.h"
@@ -288,6 +289,8 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 			tx_desc = I40E_TX_DESC(tx_ring, 0);
 		}
 
+		prefetch(tx_desc);
+
 		/* update budget accounting */
 		budget--;
 	} while (likely(budget));
@@ -368,6 +371,7 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 static void i40e_force_wb(struct i40e_vsi *vsi, struct i40e_q_vector *q_vector)
 {
 	u32 val = I40E_VFINT_DYN_CTLN_INTENA_MASK |
+		  I40E_VFINT_DYN_CTLN1_ITR_INDX_MASK | /* set noitr */
 		  I40E_VFINT_DYN_CTLN_SWINT_TRIG_MASK |
 		  I40E_VFINT_DYN_CTLN_SW_ITR_INDX_ENA_MASK;
 		  /* allow 00 to be written to the index */
@@ -529,6 +533,22 @@ void i40evf_clean_rx_ring(struct i40e_ring *rx_ring)
 	if (!rx_ring->rx_bi)
 		return;
 
+	if (ring_is_ps_enabled(rx_ring)) {
+		int bufsz = ALIGN(rx_ring->rx_hdr_len, 256) * rx_ring->count;
+
+		rx_bi = &rx_ring->rx_bi[0];
+		if (rx_bi->hdr_buf) {
+			dma_free_coherent(dev,
+					  bufsz,
+					  rx_bi->hdr_buf,
+					  rx_bi->dma);
+			for (i = 0; i < rx_ring->count; i++) {
+				rx_bi = &rx_ring->rx_bi[i];
+				rx_bi->dma = 0;
+				rx_bi->hdr_buf = NULL;
+			}
+		}
+	}
 	/* Free all the Rx ring sk_buffs */
 	for (i = 0; i < rx_ring->count; i++) {
 		rx_bi = &rx_ring->rx_bi[i];
@@ -587,6 +607,37 @@ void i40evf_free_rx_resources(struct i40e_ring *rx_ring)
 }
 
 /**
+ * i40evf_alloc_rx_headers - allocate rx header buffers
+ * @rx_ring: ring to alloc buffers
+ *
+ * Allocate rx header buffers for the entire ring. As these are static,
+ * this is only called when setting up a new ring.
+ **/
+void i40evf_alloc_rx_headers(struct i40e_ring *rx_ring)
+{
+	struct device *dev = rx_ring->dev;
+	struct i40e_rx_buffer *rx_bi;
+	dma_addr_t dma;
+	void *buffer;
+	int buf_size;
+	int i;
+
+	if (rx_ring->rx_bi[0].hdr_buf)
+		return;
+	/* Make sure the buffers don't cross cache line boundaries. */
+	buf_size = ALIGN(rx_ring->rx_hdr_len, 256);
+	buffer = dma_alloc_coherent(dev, buf_size * rx_ring->count,
+				    &dma, GFP_KERNEL);
+	if (!buffer)
+		return;
+	for (i = 0; i < rx_ring->count; i++) {
+		rx_bi = &rx_ring->rx_bi[i];
+		rx_bi->dma = dma + (i * buf_size);
+		rx_bi->hdr_buf = buffer + (i * buf_size);
+	}
+}
+
+/**
  * i40evf_setup_rx_descriptors - Allocate Rx descriptors
  * @rx_ring: Rx descriptor ring (for a specific queue) to setup
  *
@@ -646,11 +697,76 @@ static inline void i40e_release_rx_desc(struct i40e_ring *rx_ring, u32 val)
 }
 
 /**
- * i40evf_alloc_rx_buffers - Replace used receive buffers; packet split
+ * i40evf_alloc_rx_buffers_ps - Replace used receive buffers; packet split
+ * @rx_ring: ring to place buffers on
+ * @cleaned_count: number of buffers to replace
+ **/
+void i40evf_alloc_rx_buffers_ps(struct i40e_ring *rx_ring, u16 cleaned_count)
+{
+	u16 i = rx_ring->next_to_use;
+	union i40e_rx_desc *rx_desc;
+	struct i40e_rx_buffer *bi;
+
+	/* do nothing if no valid netdev defined */
+	if (!rx_ring->netdev || !cleaned_count)
+		return;
+
+	while (cleaned_count--) {
+		rx_desc = I40E_RX_DESC(rx_ring, i);
+		bi = &rx_ring->rx_bi[i];
+
+		if (bi->skb) /* desc is in use */
+			goto no_buffers;
+		if (!bi->page) {
+			bi->page = alloc_page(GFP_ATOMIC);
+			if (!bi->page) {
+				rx_ring->rx_stats.alloc_page_failed++;
+				goto no_buffers;
+			}
+		}
+
+		if (!bi->page_dma) {
+			/* use a half page if we're re-using */
+			bi->page_offset ^= PAGE_SIZE / 2;
+			bi->page_dma = dma_map_page(rx_ring->dev,
+						    bi->page,
+						    bi->page_offset,
+						    PAGE_SIZE / 2,
+						    DMA_FROM_DEVICE);
+			if (dma_mapping_error(rx_ring->dev,
+					      bi->page_dma)) {
+				rx_ring->rx_stats.alloc_page_failed++;
+				bi->page_dma = 0;
+				goto no_buffers;
+			}
+		}
+
+		dma_sync_single_range_for_device(rx_ring->dev,
+						 bi->dma,
+						 0,
+						 rx_ring->rx_hdr_len,
+						 DMA_FROM_DEVICE);
+		/* Refresh the desc even if buffer_addrs didn't change
+		 * because each write-back erases this info.
+		 */
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->page_dma);
+		rx_desc->read.hdr_addr = cpu_to_le64(bi->dma);
+		i++;
+		if (i == rx_ring->count)
+			i = 0;
+	}
+
+no_buffers:
+	if (rx_ring->next_to_use != i)
+		i40e_release_rx_desc(rx_ring, i);
+}
+
+/**
+ * i40evf_alloc_rx_buffers_1buf - Replace used receive buffers; single buffer
  * @rx_ring: ring to place buffers on
  * @cleaned_count: number of buffers to replace
  **/
-void i40evf_alloc_rx_buffers(struct i40e_ring *rx_ring, u16 cleaned_count)
+void i40evf_alloc_rx_buffers_1buf(struct i40e_ring *rx_ring, u16 cleaned_count)
 {
 	u16 i = rx_ring->next_to_use;
 	union i40e_rx_desc *rx_desc;
@@ -690,40 +806,8 @@ void i40evf_alloc_rx_buffers(struct i40e_ring *rx_ring, u16 cleaned_count)
 			}
 		}
 
-		if (ring_is_ps_enabled(rx_ring)) {
-			if (!bi->page) {
-				bi->page = alloc_page(GFP_ATOMIC);
-				if (!bi->page) {
-					rx_ring->rx_stats.alloc_page_failed++;
-					goto no_buffers;
-				}
-			}
-
-			if (!bi->page_dma) {
-				/* use a half page if we're re-using */
-				bi->page_offset ^= PAGE_SIZE / 2;
-				bi->page_dma = dma_map_page(rx_ring->dev,
-							    bi->page,
-							    bi->page_offset,
-							    PAGE_SIZE / 2,
-							    DMA_FROM_DEVICE);
-				if (dma_mapping_error(rx_ring->dev,
-						      bi->page_dma)) {
-					rx_ring->rx_stats.alloc_page_failed++;
-					bi->page_dma = 0;
-					goto no_buffers;
-				}
-			}
-
-			/* Refresh the desc even if buffer_addrs didn't change
-			 * because each write-back erases this info.
-			 */
-			rx_desc->read.pkt_addr = cpu_to_le64(bi->page_dma);
-			rx_desc->read.hdr_addr = cpu_to_le64(bi->dma);
-		} else {
-			rx_desc->read.pkt_addr = cpu_to_le64(bi->dma);
-			rx_desc->read.hdr_addr = 0;
-		}
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma);
+		rx_desc->read.hdr_addr = 0;
 		i++;
 		if (i == rx_ring->count)
 			i = 0;
@@ -777,10 +861,10 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	struct iphdr *iph;
 	__sum16 csum;
 
-	ipv4_tunnel = (rx_ptype > I40E_RX_PTYPE_GRENAT4_MAC_PAY3) &&
-		      (rx_ptype < I40E_RX_PTYPE_GRENAT4_MACVLAN_IPV6_ICMP_PAY4);
-	ipv6_tunnel = (rx_ptype > I40E_RX_PTYPE_GRENAT6_MAC_PAY3) &&
-		      (rx_ptype < I40E_RX_PTYPE_GRENAT6_MACVLAN_IPV6_ICMP_PAY4);
+	ipv4_tunnel = (rx_ptype >= I40E_RX_PTYPE_GRENAT4_MAC_PAY3) &&
+		     (rx_ptype <= I40E_RX_PTYPE_GRENAT4_MACVLAN_IPV6_ICMP_PAY4);
+	ipv6_tunnel = (rx_ptype >= I40E_RX_PTYPE_GRENAT6_MAC_PAY3) &&
+		     (rx_ptype <= I40E_RX_PTYPE_GRENAT6_MACVLAN_IPV6_ICMP_PAY4);
 
 	skb->ip_summed = CHECKSUM_NONE;
 
@@ -831,9 +915,7 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	 * so the total length of IPv4 header is IHL*4 bytes
 	 * The UDP_0 bit *may* bet set if the *inner* header is UDP
 	 */
-	if (ipv4_tunnel &&
-	    (decoded.inner_prot != I40E_RX_PTYPE_INNER_PROT_UDP) &&
-	    !(rx_status & (1 << I40E_RX_DESC_STATUS_UDP_0_SHIFT))) {
+	if (ipv4_tunnel) {
 		skb->transport_header = skb->mac_header +
 					sizeof(struct ethhdr) +
 					(ip_hdr(skb)->ihl * 4);
@@ -843,15 +925,19 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 					  skb->protocol == htons(ETH_P_8021AD))
 					  ? VLAN_HLEN : 0;
 
-		rx_udp_csum = udp_csum(skb);
-		iph = ip_hdr(skb);
-		csum = csum_tcpudp_magic(
-				iph->saddr, iph->daddr,
-				(skb->len - skb_transport_offset(skb)),
-				IPPROTO_UDP, rx_udp_csum);
+		if ((ip_hdr(skb)->protocol == IPPROTO_UDP) &&
+		    (udp_hdr(skb)->check != 0)) {
+			rx_udp_csum = udp_csum(skb);
+			iph = ip_hdr(skb);
+			csum = csum_tcpudp_magic(iph->saddr, iph->daddr,
+						 (skb->len -
+						  skb_transport_offset(skb)),
+						 IPPROTO_UDP, rx_udp_csum);
+
+			if (udp_hdr(skb)->check != csum)
+				goto checksum_fail;
 
-		if (udp_hdr(skb)->check != csum)
-			goto checksum_fail;
+		} /* else its GRE and so no outer UDP header */
 	}
 
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
@@ -906,13 +992,13 @@ static inline enum pkt_hash_types i40e_ptype_to_hash(u8 ptype)
 }
 
 /**
- * i40e_clean_rx_irq - Reclaim resources after receive completes
+ * i40e_clean_rx_irq_ps - Reclaim resources after receive; packet split
  * @rx_ring:  rx ring to clean
  * @budget:   how many cleans we're allowed
  *
  * Returns true if there's any budget left (e.g. the clean is finished)
  **/
-static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
+static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 {
 	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
 	u16 rx_packet_len, rx_header_len, rx_sph, rx_hbo;
@@ -925,20 +1011,49 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 	u8 rx_ptype;
 	u64 qword;
 
-	rx_desc = I40E_RX_DESC(rx_ring, i);
-	qword = le64_to_cpu(rx_desc->wb.qword1.status_error_len);
-	rx_status = (qword & I40E_RXD_QW1_STATUS_MASK) >>
-		    I40E_RXD_QW1_STATUS_SHIFT;
-
-	while (rx_status & (1 << I40E_RX_DESC_STATUS_DD_SHIFT)) {
-		union i40e_rx_desc *next_rxd;
+	do {
 		struct i40e_rx_buffer *rx_bi;
 		struct sk_buff *skb;
 		u16 vlan_tag;
+		/* return some buffers to hardware, one at a time is too slow */
+		if (cleaned_count >= I40E_RX_BUFFER_WRITE) {
+			i40evf_alloc_rx_buffers_ps(rx_ring, cleaned_count);
+			cleaned_count = 0;
+		}
+
+		i = rx_ring->next_to_clean;
+		rx_desc = I40E_RX_DESC(rx_ring, i);
+		qword = le64_to_cpu(rx_desc->wb.qword1.status_error_len);
+		rx_status = (qword & I40E_RXD_QW1_STATUS_MASK) >>
+			I40E_RXD_QW1_STATUS_SHIFT;
+
+		if (!(rx_status & (1 << I40E_RX_DESC_STATUS_DD_SHIFT)))
+			break;
+
+		/* This memory barrier is needed to keep us from reading
+		 * any other fields out of the rx_desc until we know the
+		 * DD bit is set.
+		 */
+		dma_rmb();
 		rx_bi = &rx_ring->rx_bi[i];
 		skb = rx_bi->skb;
-		prefetch(skb->data);
+		if (likely(!skb)) {
+			skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
+							rx_ring->rx_hdr_len);
+			if (!skb) {
+				rx_ring->rx_stats.alloc_buff_failed++;
+				break;
+			}
 
+			/* initialize queue mapping */
+			skb_record_rx_queue(skb, rx_ring->queue_index);
+			/* we are reusing so sync this buffer for CPU use */
+			dma_sync_single_range_for_cpu(rx_ring->dev,
+						      rx_bi->dma,
+						      0,
+						      rx_ring->rx_hdr_len,
+						      DMA_FROM_DEVICE);
+		}
 		rx_packet_len = (qword & I40E_RXD_QW1_LENGTH_PBUF_MASK) >>
 				I40E_RXD_QW1_LENGTH_PBUF_SHIFT;
 		rx_header_len = (qword & I40E_RXD_QW1_LENGTH_HBUF_MASK) >>
@@ -953,40 +1068,30 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 
 		rx_ptype = (qword & I40E_RXD_QW1_PTYPE_MASK) >>
 			   I40E_RXD_QW1_PTYPE_SHIFT;
+		prefetch(rx_bi->page);
 		rx_bi->skb = NULL;
-
-		/* This memory barrier is needed to keep us from reading
-		 * any other fields out of the rx_desc until we know the
-		 * STATUS_DD bit is set
-		 */
-		rmb();
-
-		/* Get the header and possibly the whole packet
-		 * If this is an skb from previous receive dma will be 0
-		 */
-		if (rx_bi->dma) {
-			u16 len;
-
+		cleaned_count++;
+		if (rx_hbo || rx_sph) {
+			int len;
 			if (rx_hbo)
 				len = I40E_RX_HDR_SIZE;
-			else if (rx_sph)
-				len = rx_header_len;
-			else if (rx_packet_len)
-				len = rx_packet_len;   /* 1buf/no split found */
 			else
-				len = rx_header_len;   /* split always mode */
-
-			skb_put(skb, len);
-			dma_unmap_single(rx_ring->dev,
-					 rx_bi->dma,
-					 rx_ring->rx_buf_len,
-					 DMA_FROM_DEVICE);
-			rx_bi->dma = 0;
+				len = rx_header_len;
+			memcpy(__skb_put(skb, len), rx_bi->hdr_buf, len);
+		} else if (skb->len == 0) {
+			int len;
+
+			len = (rx_packet_len > skb_headlen(skb) ?
+				skb_headlen(skb) : rx_packet_len);
+			memcpy(__skb_put(skb, len),
+			       rx_bi->page + rx_bi->page_offset,
+			       len);
+			rx_bi->page_offset += len;
+			rx_packet_len -= len;
 		}
 
 		/* Get the rest of the data if this was a header split */
-		if (ring_is_ps_enabled(rx_ring) && rx_packet_len) {
-
+		if (rx_packet_len) {
 			skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
 					   rx_bi->page,
 					   rx_bi->page_offset,
@@ -1008,22 +1113,16 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 				       DMA_FROM_DEVICE);
 			rx_bi->page_dma = 0;
 		}
-		I40E_RX_NEXT_DESC_PREFETCH(rx_ring, i, next_rxd);
+		I40E_RX_INCREMENT(rx_ring, i);
 
 		if (unlikely(
 		    !(rx_status & (1 << I40E_RX_DESC_STATUS_EOF_SHIFT)))) {
 			struct i40e_rx_buffer *next_buffer;
 
 			next_buffer = &rx_ring->rx_bi[i];
-
-			if (ring_is_ps_enabled(rx_ring)) {
-				rx_bi->skb = next_buffer->skb;
-				rx_bi->dma = next_buffer->dma;
-				next_buffer->skb = skb;
-				next_buffer->dma = 0;
-			}
+			next_buffer->skb = skb;
 			rx_ring->rx_stats.non_eop_descs++;
-			goto next_desc;
+			continue;
 		}
 
 		/* ERR_MASK will only have valid bits if EOP set */
@@ -1032,7 +1131,7 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 			/* TODO: shouldn't we increment a counter indicating the
 			 * drop?
 			 */
-			goto next_desc;
+			continue;
 		}
 
 		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
@@ -1048,30 +1147,134 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 		vlan_tag = rx_status & (1 << I40E_RX_DESC_STATUS_L2TAG1P_SHIFT)
 			 ? le16_to_cpu(rx_desc->wb.qword0.lo_dword.l2tag1)
 			 : 0;
+#ifdef I40E_FCOE
+		if (!i40e_fcoe_handle_offload(rx_ring, rx_desc, skb)) {
+			dev_kfree_skb_any(skb);
+			continue;
+		}
+#endif
+		skb_mark_napi_id(skb, &rx_ring->q_vector->napi);
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
 		rx_ring->netdev->last_rx = jiffies;
-		budget--;
-next_desc:
 		rx_desc->wb.qword1.status_error_len = 0;
-		if (!budget)
-			break;
 
-		cleaned_count++;
+	} while (likely(total_rx_packets < budget));
+
+	u64_stats_update_begin(&rx_ring->syncp);
+	rx_ring->stats.packets += total_rx_packets;
+	rx_ring->stats.bytes += total_rx_bytes;
+	u64_stats_update_end(&rx_ring->syncp);
+	rx_ring->q_vector->rx.total_packets += total_rx_packets;
+	rx_ring->q_vector->rx.total_bytes += total_rx_bytes;
+
+	return total_rx_packets;
+}
+
+/**
+ * i40e_clean_rx_irq_1buf - Reclaim resources after receive; single buffer
+ * @rx_ring:  rx ring to clean
+ * @budget:   how many cleans we're allowed
+ *
+ * Returns number of packets cleaned
+ **/
+static int i40e_clean_rx_irq_1buf(struct i40e_ring *rx_ring, int budget)
+{
+	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
+	u16 cleaned_count = I40E_DESC_UNUSED(rx_ring);
+	struct i40e_vsi *vsi = rx_ring->vsi;
+	union i40e_rx_desc *rx_desc;
+	u32 rx_error, rx_status;
+	u16 rx_packet_len;
+	u8 rx_ptype;
+	u64 qword;
+	u16 i;
+
+	do {
+		struct i40e_rx_buffer *rx_bi;
+		struct sk_buff *skb;
+		u16 vlan_tag;
 		/* return some buffers to hardware, one at a time is too slow */
 		if (cleaned_count >= I40E_RX_BUFFER_WRITE) {
-			i40evf_alloc_rx_buffers(rx_ring, cleaned_count);
+			i40evf_alloc_rx_buffers_1buf(rx_ring, cleaned_count);
 			cleaned_count = 0;
 		}
 
-		/* use prefetched values */
-		rx_desc = next_rxd;
+		i = rx_ring->next_to_clean;
+		rx_desc = I40E_RX_DESC(rx_ring, i);
 		qword = le64_to_cpu(rx_desc->wb.qword1.status_error_len);
 		rx_status = (qword & I40E_RXD_QW1_STATUS_MASK) >>
-			    I40E_RXD_QW1_STATUS_SHIFT;
-	}
+			I40E_RXD_QW1_STATUS_SHIFT;
+
+		if (!(rx_status & (1 << I40E_RX_DESC_STATUS_DD_SHIFT)))
+			break;
+
+		/* This memory barrier is needed to keep us from reading
+		 * any other fields out of the rx_desc until we know the
+		 * DD bit is set.
+		 */
+		dma_rmb();
+
+		rx_bi = &rx_ring->rx_bi[i];
+		skb = rx_bi->skb;
+		prefetch(skb->data);
+
+		rx_packet_len = (qword & I40E_RXD_QW1_LENGTH_PBUF_MASK) >>
+				I40E_RXD_QW1_LENGTH_PBUF_SHIFT;
+
+		rx_error = (qword & I40E_RXD_QW1_ERROR_MASK) >>
+			   I40E_RXD_QW1_ERROR_SHIFT;
+		rx_error &= ~(1 << I40E_RX_DESC_ERROR_HBO_SHIFT);
+
+		rx_ptype = (qword & I40E_RXD_QW1_PTYPE_MASK) >>
+			   I40E_RXD_QW1_PTYPE_SHIFT;
+		rx_bi->skb = NULL;
+		cleaned_count++;
+
+		/* Get the header and possibly the whole packet
+		 * If this is an skb from previous receive dma will be 0
+		 */
+		skb_put(skb, rx_packet_len);
+		dma_unmap_single(rx_ring->dev, rx_bi->dma, rx_ring->rx_buf_len,
+				 DMA_FROM_DEVICE);
+		rx_bi->dma = 0;
+
+		I40E_RX_INCREMENT(rx_ring, i);
+
+		if (unlikely(
+		    !(rx_status & (1 << I40E_RX_DESC_STATUS_EOF_SHIFT)))) {
+			rx_ring->rx_stats.non_eop_descs++;
+			continue;
+		}
+
+		/* ERR_MASK will only have valid bits if EOP set */
+		if (unlikely(rx_error & (1 << I40E_RX_DESC_ERROR_RXE_SHIFT))) {
+			dev_kfree_skb_any(skb);
+			/* TODO: shouldn't we increment a counter indicating the
+			 * drop?
+			 */
+			continue;
+		}
+
+		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
+			     i40e_ptype_to_hash(rx_ptype));
+		/* probably a little skewed due to removing CRC */
+		total_rx_bytes += skb->len;
+		total_rx_packets++;
+
+		skb->protocol = eth_type_trans(skb, rx_ring->netdev);
+
+		i40e_rx_checksum(vsi, skb, rx_status, rx_error, rx_ptype);
+
+		vlan_tag = rx_status & (1 << I40E_RX_DESC_STATUS_L2TAG1P_SHIFT)
+			 ? le16_to_cpu(rx_desc->wb.qword0.lo_dword.l2tag1)
+			 : 0;
+		i40e_receive_skb(rx_ring, skb, vlan_tag);
+
+		rx_ring->netdev->last_rx = jiffies;
+		rx_desc->wb.qword1.status_error_len = 0;
+	} while (likely(total_rx_packets < budget));
 
-	rx_ring->next_to_clean = i;
 	u64_stats_update_begin(&rx_ring->syncp);
 	rx_ring->stats.packets += total_rx_packets;
 	rx_ring->stats.bytes += total_rx_bytes;
@@ -1079,10 +1282,7 @@ next_desc:
 	rx_ring->q_vector->rx.total_packets += total_rx_packets;
 	rx_ring->q_vector->rx.total_bytes += total_rx_bytes;
 
-	if (cleaned_count)
-		i40evf_alloc_rx_buffers(rx_ring, cleaned_count);
-
-	return budget > 0;
+	return total_rx_packets;
 }
 
 /**
@@ -1103,6 +1303,7 @@ int i40evf_napi_poll(struct napi_struct *napi, int budget)
 	bool clean_complete = true;
 	bool arm_wb = false;
 	int budget_per_ring;
+	int cleaned;
 
 	if (test_bit(__I40E_DOWN, &vsi->state)) {
 		napi_complete(napi);
@@ -1122,8 +1323,14 @@ int i40evf_napi_poll(struct napi_struct *napi, int budget)
 	 */
 	budget_per_ring = max(budget/q_vector->num_ringpairs, 1);
 
-	i40e_for_each_ring(ring, q_vector->rx)
-		clean_complete &= i40e_clean_rx_irq(ring, budget_per_ring);
+	i40e_for_each_ring(ring, q_vector->rx) {
+		if (ring_is_ps_enabled(ring))
+			cleaned = i40e_clean_rx_irq_ps(ring, budget_per_ring);
+		else
+			cleaned = i40e_clean_rx_irq_1buf(ring, budget_per_ring);
+		/* if we didn't clean as many as budgeted, we must be done */
+		clean_complete &= (budget_per_ring != cleaned);
+	}
 
 	/* If work not completed, return budget and polling will return */
 	if (!clean_complete) {
@@ -1163,6 +1370,19 @@ static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
 	__be16 protocol = skb->protocol;
 	u32  tx_flags = 0;
 
+	if (protocol == htons(ETH_P_8021Q) &&
+	    !(tx_ring->netdev->features & NETIF_F_HW_VLAN_CTAG_TX)) {
+		/* When HW VLAN acceleration is turned off by the user the
+		 * stack sets the protocol to 8021q so that the driver
+		 * can take any steps required to support the SW only
+		 * VLAN handling.  In our case the driver doesn't need
+		 * to take any further steps so just set the protocol
+		 * to the encapsulated ethertype.
+		 */
+		skb->protocol = vlan_get_protocol(skb);
+		goto out;
+	}
+
 	/* if we have a HW VLAN tag being added, default to the HW one */
 	if (skb_vlan_tag_present(skb)) {
 		tx_flags |= skb_vlan_tag_get(skb) << I40E_TX_FLAGS_VLAN_SHIFT;
@@ -1179,6 +1399,7 @@ static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
 		tx_flags |= I40E_TX_FLAGS_SW_VLAN;
 	}
 
+out:
 	*flags = tx_flags;
 	return 0;
 }
@@ -1262,8 +1483,16 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 	struct iphdr *this_ip_hdr;
 	u32 network_hdr_len;
 	u8 l4_hdr = 0;
+	u32 l4_tunnel = 0;
 
 	if (skb->encapsulation) {
+		switch (ip_hdr(skb)->protocol) {
+		case IPPROTO_UDP:
+			l4_tunnel = I40E_TXD_CTX_UDP_TUNNELING;
+			break;
+		default:
+			return;
+		}
 		network_hdr_len = skb_inner_network_header_len(skb);
 		this_ip_hdr = inner_ip_hdr(skb);
 		this_ipv6_hdr = inner_ipv6_hdr(skb);
@@ -1286,8 +1515,8 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 
 		/* Now set the ctx descriptor fields */
 		*cd_tunneling |= (skb_network_header_len(skb) >> 2) <<
-					I40E_TXD_CTX_QW0_EXT_IPLEN_SHIFT |
-				   I40E_TXD_CTX_UDP_TUNNELING            |
+				   I40E_TXD_CTX_QW0_EXT_IPLEN_SHIFT      |
+				   l4_tunnel                             |
 				   ((skb_inner_network_offset(skb) -
 					skb_transport_offset(skb)) >> 1) <<
 				   I40E_TXD_CTX_QW0_NATLEN_SHIFT;
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
index c950a038237c..1e49bb1fbac1 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
@@ -96,6 +96,14 @@ enum i40e_dyn_idx_t {
 
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
 #define I40E_RX_BUFFER_WRITE	16	/* Must be power of 2 */
+#define I40E_RX_INCREMENT(r, i) \
+	do {					\
+		(i)++;				\
+		if ((i) == (r)->count)		\
+			i = 0;			\
+		r->next_to_clean = i;		\
+	} while (0)
+
 #define I40E_RX_NEXT_DESC(r, i, n)		\
 	do {					\
 		(i)++;				\
@@ -151,6 +159,7 @@ struct i40e_tx_buffer {
 
 struct i40e_rx_buffer {
 	struct sk_buff *skb;
+	void *hdr_buf;
 	dma_addr_t dma;
 	struct page *page;
 	dma_addr_t page_dma;
@@ -223,8 +232,8 @@ struct i40e_ring {
 	u16 rx_buf_len;
 	u8  dtype;
 #define I40E_RX_DTYPE_NO_SPLIT      0
-#define I40E_RX_DTYPE_SPLIT_ALWAYS  1
-#define I40E_RX_DTYPE_HEADER_SPLIT  2
+#define I40E_RX_DTYPE_HEADER_SPLIT  1
+#define I40E_RX_DTYPE_SPLIT_ALWAYS  2
 	u8  hsplit;
 #define I40E_RX_SPLIT_L2      0x1
 #define I40E_RX_SPLIT_IP      0x2
@@ -278,7 +287,9 @@ struct i40e_ring_container {
 #define i40e_for_each_ring(pos, head) \
 	for (pos = (head).ring; pos != NULL; pos = pos->next)
 
-void i40evf_alloc_rx_buffers(struct i40e_ring *rxr, u16 cleaned_count);
+void i40evf_alloc_rx_buffers_ps(struct i40e_ring *rxr, u16 cleaned_count);
+void i40evf_alloc_rx_buffers_1buf(struct i40e_ring *rxr, u16 cleaned_count);
+void i40evf_alloc_rx_headers(struct i40e_ring *rxr);
 netdev_tx_t i40evf_xmit_frame(struct sk_buff *skb, struct net_device *netdev);
 void i40evf_clean_tx_ring(struct i40e_ring *tx_ring);
 void i40evf_clean_rx_ring(struct i40e_ring *rx_ring);
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_type.h b/drivers/net/ethernet/intel/i40evf/i40e_type.h
index 3d0fdaab5cc8..9c79cb6abb2b 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_type.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_type.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Virtual Function Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -44,7 +44,8 @@
 #define I40E_DEV_ID_QSFP_B		0x1584
 #define I40E_DEV_ID_QSFP_C		0x1585
 #define I40E_DEV_ID_10G_BASE_T		0x1586
-#define I40E_DEV_ID_VF		0x154C
+#define I40E_DEV_ID_20G_KR2		0x1587
+#define I40E_DEV_ID_VF			0x154C
 #define I40E_DEV_ID_VF_HV		0x1571
 
 #define i40e_is_40G_device(d)		((d) == I40E_DEV_ID_QSFP_A  || \
@@ -175,12 +176,12 @@ struct i40e_link_status {
 	u8 an_info;
 	u8 ext_info;
 	u8 loopback;
-	bool an_enabled;
 	/* is Link Status Event notification to SW enabled */
 	bool lse_enable;
 	u16 max_frame_size;
 	bool crc_enable;
 	u8 pacing;
+	u8 requested_speeds;
 };
 
 struct i40e_phy_info {
@@ -1116,7 +1117,7 @@ struct i40e_hw_port_stats {
 /* Checksum and Shadow RAM pointers */
 #define I40E_SR_NVM_CONTROL_WORD		0x00
 #define I40E_SR_EMP_MODULE_PTR			0x0F
-#define I40E_SR_NVM_IMAGE_VERSION		0x18
+#define I40E_SR_NVM_DEV_STARTER_VERSION		0x18
 #define I40E_SR_NVM_WAKE_ON_LAN			0x19
 #define I40E_SR_ALTERNATE_SAN_MAC_ADDRESS_PTR	0x27
 #define I40E_SR_NVM_EETRACK_LO			0x2D
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
index e0c8208138f4..59f62f0e65dd 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
@@ -59,31 +59,29 @@
  * of the virtchnl_msg structure.
  */
 enum i40e_virtchnl_ops {
-/* VF sends req. to pf for the following
- * ops.
+/* The PF sends status change events to VFs using
+ * the I40E_VIRTCHNL_OP_EVENT opcode.
+ * VFs send requests to the PF using the other ops.
  */
 	I40E_VIRTCHNL_OP_UNKNOWN = 0,
 	I40E_VIRTCHNL_OP_VERSION = 1, /* must ALWAYS be 1 */
-	I40E_VIRTCHNL_OP_RESET_VF,
-	I40E_VIRTCHNL_OP_GET_VF_RESOURCES,
-	I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE,
-	I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE,
-	I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES,
-	I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP,
-	I40E_VIRTCHNL_OP_ENABLE_QUEUES,
-	I40E_VIRTCHNL_OP_DISABLE_QUEUES,
-	I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS,
-	I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS,
-	I40E_VIRTCHNL_OP_ADD_VLAN,
-	I40E_VIRTCHNL_OP_DEL_VLAN,
-	I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE,
-	I40E_VIRTCHNL_OP_GET_STATS,
-	I40E_VIRTCHNL_OP_FCOE,
-	I40E_VIRTCHNL_OP_CONFIG_RSS,
-/* PF sends status change events to vfs using
- * the following op.
- */
-	I40E_VIRTCHNL_OP_EVENT,
+	I40E_VIRTCHNL_OP_RESET_VF = 2,
+	I40E_VIRTCHNL_OP_GET_VF_RESOURCES = 3,
+	I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE = 4,
+	I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE = 5,
+	I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES = 6,
+	I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP = 7,
+	I40E_VIRTCHNL_OP_ENABLE_QUEUES = 8,
+	I40E_VIRTCHNL_OP_DISABLE_QUEUES = 9,
+	I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS = 10,
+	I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS = 11,
+	I40E_VIRTCHNL_OP_ADD_VLAN = 12,
+	I40E_VIRTCHNL_OP_DEL_VLAN = 13,
+	I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE = 14,
+	I40E_VIRTCHNL_OP_GET_STATS = 15,
+	I40E_VIRTCHNL_OP_FCOE = 16,
+	I40E_VIRTCHNL_OP_EVENT = 17,
+	I40E_VIRTCHNL_OP_CONFIG_RSS = 18,
 };
 
 /* Virtual channel message descriptor. This overlays the admin queue
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf.h b/drivers/net/ethernet/intel/i40evf/i40evf.h
index 981224743c73..34c8565031f6 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf.h
+++ b/drivers/net/ethernet/intel/i40evf/i40evf.h
@@ -272,6 +272,8 @@ void i40evf_update_stats(struct i40evf_adapter *adapter);
 void i40evf_reset_interrupt_capability(struct i40evf_adapter *adapter);
 int i40evf_init_interrupt_scheme(struct i40evf_adapter *adapter);
 void i40evf_irq_enable_queues(struct i40evf_adapter *adapter, u32 mask);
+void i40evf_free_all_tx_resources(struct i40evf_adapter *adapter);
+void i40evf_free_all_rx_resources(struct i40evf_adapter *adapter);
 
 void i40e_napi_add_all(struct i40evf_adapter *adapter);
 void i40e_napi_del_all(struct i40evf_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
index 69b97bac182c..f4e77665bc54 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Virtual Function Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -29,7 +29,6 @@
 
 #include <linux/uaccess.h>
 
-
 struct i40evf_stats {
 	char stat_string[ETH_GSTRING_LEN];
 	int stat_offset;
@@ -180,7 +179,7 @@ static u32 i40evf_get_msglevel(struct net_device *netdev)
 }
 
 /**
- * i40evf_get_msglevel - Set debug message level
+ * i40evf_set_msglevel - Set debug message level
  * @netdev: network interface device structure
  * @data: message level
  *
@@ -191,6 +190,8 @@ static void i40evf_set_msglevel(struct net_device *netdev, u32 data)
 {
 	struct i40evf_adapter *adapter = netdev_priv(netdev);
 
+	if (I40E_DEBUG_USER & data)
+		adapter->hw.debug_mask = data;
 	adapter->msg_enable = data;
 }
 
@@ -208,7 +209,7 @@ static void i40evf_get_drvinfo(struct net_device *netdev,
 
 	strlcpy(drvinfo->driver, i40evf_driver_name, 32);
 	strlcpy(drvinfo->version, i40evf_driver_version, 32);
-
+	strlcpy(drvinfo->fw_version, "N/A", 4);
 	strlcpy(drvinfo->bus_info, pci_name(adapter->pdev), 32);
 }
 
@@ -640,12 +641,14 @@ static int i40evf_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
 	if (!indir)
 		return 0;
 
-	for (i = 0, j = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++) {
-		hlut_val = rd32(hw, I40E_VFQF_HLUT(i));
-		indir[j++] = hlut_val & 0xff;
-		indir[j++] = (hlut_val >> 8) & 0xff;
-		indir[j++] = (hlut_val >> 16) & 0xff;
-		indir[j++] = (hlut_val >> 24) & 0xff;
+	if (indir) {
+		for (i = 0, j = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++) {
+			hlut_val = rd32(hw, I40E_VFQF_HLUT(i));
+			indir[j++] = hlut_val & 0xff;
+			indir[j++] = (hlut_val >> 8) & 0xff;
+			indir[j++] = (hlut_val >> 16) & 0xff;
+			indir[j++] = (hlut_val >> 24) & 0xff;
+		}
 	}
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_main.c b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
index 8d8c201c63c1..6d5f3b21c68a 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_main.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Virtual Function Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 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,
@@ -28,15 +28,13 @@
 #include "i40e_prototype.h"
 static int i40evf_setup_all_tx_resources(struct i40evf_adapter *adapter);
 static int i40evf_setup_all_rx_resources(struct i40evf_adapter *adapter);
-static void i40evf_free_all_tx_resources(struct i40evf_adapter *adapter);
-static void i40evf_free_all_rx_resources(struct i40evf_adapter *adapter);
 static int i40evf_close(struct net_device *netdev);
 
 char i40evf_driver_name[] = "i40evf";
 static const char i40evf_driver_string[] =
 	"Intel(R) XL710/X710 Virtual Function Network Driver";
 
-#define DRV_VERSION "1.2.0"
+#define DRV_VERSION "1.2.25"
 const char i40evf_driver_version[] = DRV_VERSION;
 static const char i40evf_copyright[] =
 	"Copyright (c) 2013 - 2014 Intel Corporation.";
@@ -244,6 +242,7 @@ void i40evf_irq_enable_queues(struct i40evf_adapter *adapter, u32 mask)
 		if (mask & (1 << (i - 1))) {
 			wr32(hw, I40E_VFINT_DYN_CTLN1(i - 1),
 			     I40E_VFINT_DYN_CTLN1_INTENA_MASK |
+			     I40E_VFINT_DYN_CTLN1_ITR_INDX_MASK |
 			     I40E_VFINT_DYN_CTLN_CLEARPBA_MASK);
 		}
 	}
@@ -263,6 +262,7 @@ static void i40evf_fire_sw_int(struct i40evf_adapter *adapter, u32 mask)
 	if (mask & 1) {
 		dyn_ctl = rd32(hw, I40E_VFINT_DYN_CTL01);
 		dyn_ctl |= I40E_VFINT_DYN_CTLN_SWINT_TRIG_MASK |
+			   I40E_VFINT_DYN_CTLN1_ITR_INDX_MASK |
 			   I40E_VFINT_DYN_CTLN_CLEARPBA_MASK;
 		wr32(hw, I40E_VFINT_DYN_CTL01, dyn_ctl);
 	}
@@ -270,6 +270,7 @@ static void i40evf_fire_sw_int(struct i40evf_adapter *adapter, u32 mask)
 		if (mask & (1 << i)) {
 			dyn_ctl = rd32(hw, I40E_VFINT_DYN_CTLN1(i - 1));
 			dyn_ctl |= I40E_VFINT_DYN_CTLN_SWINT_TRIG_MASK |
+				   I40E_VFINT_DYN_CTLN1_ITR_INDX_MASK |
 				   I40E_VFINT_DYN_CTLN_CLEARPBA_MASK;
 			wr32(hw, I40E_VFINT_DYN_CTLN1(i - 1), dyn_ctl);
 		}
@@ -524,7 +525,8 @@ static int i40evf_request_misc_irq(struct i40evf_adapter *adapter)
 	int err;
 
 	snprintf(adapter->misc_vector_name,
-		 sizeof(adapter->misc_vector_name) - 1, "i40evf:mbx");
+		 sizeof(adapter->misc_vector_name) - 1, "i40evf-%s:mbx",
+		 dev_name(&adapter->pdev->dev));
 	err = request_irq(adapter->msix_entries[0].vector,
 			  &i40evf_msix_aq, 0,
 			  adapter->misc_vector_name, netdev);
@@ -662,13 +664,21 @@ i40evf_vlan_filter *i40evf_find_vlan(struct i40evf_adapter *adapter, u16 vlan)
 static struct
 i40evf_vlan_filter *i40evf_add_vlan(struct i40evf_adapter *adapter, u16 vlan)
 {
-	struct i40evf_vlan_filter *f;
+	struct i40evf_vlan_filter *f = NULL;
+	int count = 50;
+
+	while (test_and_set_bit(__I40EVF_IN_CRITICAL_TASK,
+				&adapter->crit_section)) {
+		udelay(1);
+		if (--count == 0)
+			goto out;
+	}
 
 	f = i40evf_find_vlan(adapter, vlan);
 	if (!f) {
 		f = kzalloc(sizeof(*f), GFP_ATOMIC);
 		if (!f)
-			return NULL;
+			goto clearout;
 
 		f->vlan = vlan;
 
@@ -678,6 +688,9 @@ i40evf_vlan_filter *i40evf_add_vlan(struct i40evf_adapter *adapter, u16 vlan)
 		adapter->aq_required |= I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
 	}
 
+clearout:
+	clear_bit(__I40EVF_IN_CRITICAL_TASK, &adapter->crit_section);
+out:
 	return f;
 }
 
@@ -689,12 +702,21 @@ i40evf_vlan_filter *i40evf_add_vlan(struct i40evf_adapter *adapter, u16 vlan)
 static void i40evf_del_vlan(struct i40evf_adapter *adapter, u16 vlan)
 {
 	struct i40evf_vlan_filter *f;
+	int count = 50;
+
+	while (test_and_set_bit(__I40EVF_IN_CRITICAL_TASK,
+				&adapter->crit_section)) {
+		udelay(1);
+		if (--count == 0)
+			return;
+	}
 
 	f = i40evf_find_vlan(adapter, vlan);
 	if (f) {
 		f->remove = true;
 		adapter->aq_required |= I40EVF_FLAG_AQ_DEL_VLAN_FILTER;
 	}
+	clear_bit(__I40EVF_IN_CRITICAL_TASK, &adapter->crit_section);
 }
 
 /**
@@ -761,13 +783,17 @@ i40evf_mac_filter *i40evf_add_filter(struct i40evf_adapter *adapter,
 				     u8 *macaddr)
 {
 	struct i40evf_mac_filter *f;
+	int count = 50;
 
 	if (!macaddr)
 		return NULL;
 
 	while (test_and_set_bit(__I40EVF_IN_CRITICAL_TASK,
-				&adapter->crit_section))
+				&adapter->crit_section)) {
 		udelay(1);
+		if (--count == 0)
+			return NULL;
+	}
 
 	f = i40evf_find_filter(adapter, macaddr);
 	if (!f) {
@@ -828,6 +854,7 @@ static void i40evf_set_rx_mode(struct net_device *netdev)
 	struct i40evf_mac_filter *f, *ftmp;
 	struct netdev_hw_addr *uca;
 	struct netdev_hw_addr *mca;
+	int count = 50;
 
 	/* add addr if not already in the filter list */
 	netdev_for_each_uc_addr(uca, netdev) {
@@ -838,8 +865,14 @@ static void i40evf_set_rx_mode(struct net_device *netdev)
 	}
 
 	while (test_and_set_bit(__I40EVF_IN_CRITICAL_TASK,
-				&adapter->crit_section))
+				&adapter->crit_section)) {
 		udelay(1);
+		if (--count == 0) {
+			dev_err(&adapter->pdev->dev,
+				"Failed to get lock in %s\n", __func__);
+			return;
+		}
+	}
 	/* remove filter if not in netdev list */
 	list_for_each_entry_safe(f, ftmp, &adapter->mac_filter_list, list) {
 		bool found = false;
@@ -920,7 +953,7 @@ static void i40evf_configure(struct i40evf_adapter *adapter)
 	for (i = 0; i < adapter->num_active_queues; i++) {
 		struct i40e_ring *ring = adapter->rx_rings[i];
 
-		i40evf_alloc_rx_buffers(ring, ring->count);
+		i40evf_alloc_rx_buffers_1buf(ring, ring->count);
 		ring->next_to_use = ring->count - 1;
 		writel(ring->next_to_use, ring->tail);
 	}
@@ -958,6 +991,9 @@ void i40evf_down(struct i40evf_adapter *adapter)
 				&adapter->crit_section))
 		usleep_range(500, 1000);
 
+	netif_carrier_off(netdev);
+	netif_tx_disable(netdev);
+	i40evf_napi_disable_all(adapter);
 	i40evf_irq_disable(adapter);
 
 	/* remove all MAC filters */
@@ -981,15 +1017,7 @@ void i40evf_down(struct i40evf_adapter *adapter)
 		adapter->aq_required |= I40EVF_FLAG_AQ_DEL_VLAN_FILTER;
 		adapter->aq_required |= I40EVF_FLAG_AQ_DISABLE_QUEUES;
 	}
-	netif_tx_disable(netdev);
-
-	netif_tx_stop_all_queues(netdev);
-
-	i40evf_napi_disable_all(adapter);
-
-	msleep(20);
 
-	netif_carrier_off(netdev);
 	clear_bit(__I40EVF_IN_CRITICAL_TASK, &adapter->crit_section);
 }
 
@@ -1344,6 +1372,11 @@ static void i40evf_watchdog_task(struct work_struct *work)
 		goto watchdog_done;
 	}
 
+	if (adapter->aq_required & I40EVF_FLAG_AQ_DISABLE_QUEUES) {
+		i40evf_disable_queues(adapter);
+		goto watchdog_done;
+	}
+
 	if (adapter->aq_required & I40EVF_FLAG_AQ_MAP_VECTORS) {
 		i40evf_map_queues(adapter);
 		goto watchdog_done;
@@ -1369,11 +1402,6 @@ static void i40evf_watchdog_task(struct work_struct *work)
 		goto watchdog_done;
 	}
 
-	if (adapter->aq_required & I40EVF_FLAG_AQ_DISABLE_QUEUES) {
-		i40evf_disable_queues(adapter);
-		goto watchdog_done;
-	}
-
 	if (adapter->aq_required & I40EVF_FLAG_AQ_CONFIGURE_QUEUES) {
 		i40evf_configure_queues(adapter);
 		goto watchdog_done;
@@ -1407,41 +1435,22 @@ restart_watchdog:
 }
 
 /**
- * next_queue - increment to next available tx queue
- * @adapter: board private structure
- * @j: queue counter
- *
- * Helper function for RSS programming to increment through available
- * queus. Returns the next queue value.
- **/
-static int next_queue(struct i40evf_adapter *adapter, int j)
-{
-	j += 1;
-
-	return j >= adapter->num_active_queues ? 0 : j;
-}
-
-/**
- * i40evf_configure_rss - Prepare for RSS if used
+ * i40evf_configure_rss - Prepare for RSS
  * @adapter: board private structure
  **/
 static void i40evf_configure_rss(struct i40evf_adapter *adapter)
 {
 	u32 rss_key[I40E_VFQF_HKEY_MAX_INDEX + 1];
 	struct i40e_hw *hw = &adapter->hw;
+	u32 cqueue = 0;
 	u32 lut = 0;
 	int i, j;
 	u64 hena;
 
-	/* No RSS for single queue. */
-	if (adapter->num_active_queues == 1) {
-		wr32(hw, I40E_VFQF_HENA(0), 0);
-		wr32(hw, I40E_VFQF_HENA(1), 0);
-		return;
-	}
-
 	/* Hash type is configured by the PF - we just supply the key */
 	netdev_rss_key_fill(rss_key, sizeof(rss_key));
+
+	/* Fill out hash function seed */
 	for (i = 0; i <= I40E_VFQF_HKEY_MAX_INDEX; i++)
 		wr32(hw, I40E_VFQF_HKEY(i), rss_key[i]);
 
@@ -1451,16 +1460,14 @@ static void i40evf_configure_rss(struct i40evf_adapter *adapter)
 	wr32(hw, I40E_VFQF_HENA(1), (u32)(hena >> 32));
 
 	/* Populate the LUT with max no. of queues in round robin fashion */
-	j = adapter->num_active_queues;
 	for (i = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++) {
-		j = next_queue(adapter, j);
-		lut = j;
-		j = next_queue(adapter, j);
-		lut |= j << 8;
-		j = next_queue(adapter, j);
-		lut |= j << 16;
-		j = next_queue(adapter, j);
-		lut |= j << 24;
+		lut = 0;
+		for (j = 0; j < 4; j++) {
+			if (cqueue == adapter->vsi_res->num_queue_pairs)
+				cqueue = 0;
+			lut |= ((cqueue) << (8 * j));
+			cqueue++;
+		}
 		wr32(hw, I40E_VFQF_HLUT(i), lut);
 	}
 	i40e_flush(hw);
@@ -1481,9 +1488,11 @@ static void i40evf_reset_task(struct work_struct *work)
 	struct i40evf_adapter *adapter = container_of(work,
 						      struct i40evf_adapter,
 						      reset_task);
+	struct net_device *netdev = adapter->netdev;
 	struct i40e_hw *hw = &adapter->hw;
-	int i = 0, err;
+	struct i40evf_mac_filter *f;
 	uint32_t rstat_val;
+	int i = 0, err;
 
 	while (test_and_set_bit(__I40EVF_IN_CRITICAL_TASK,
 				&adapter->crit_section))
@@ -1528,7 +1537,11 @@ static void i40evf_reset_task(struct work_struct *work)
 
 		if (netif_running(adapter->netdev)) {
 			set_bit(__I40E_DOWN, &adapter->vsi.state);
-			i40evf_down(adapter);
+			i40evf_irq_disable(adapter);
+			i40evf_napi_disable_all(adapter);
+			netif_tx_disable(netdev);
+			netif_tx_stop_all_queues(netdev);
+			netif_carrier_off(netdev);
 			i40evf_free_traffic_irqs(adapter);
 			i40evf_free_all_tx_resources(adapter);
 			i40evf_free_all_rx_resources(adapter);
@@ -1560,22 +1573,38 @@ static void i40evf_reset_task(struct work_struct *work)
 continue_reset:
 	adapter->flags &= ~I40EVF_FLAG_RESET_PENDING;
 
-	i40evf_down(adapter);
+	i40evf_irq_disable(adapter);
+
+	if (netif_running(adapter->netdev)) {
+		i40evf_napi_disable_all(adapter);
+		netif_tx_disable(netdev);
+		netif_tx_stop_all_queues(netdev);
+		netif_carrier_off(netdev);
+	}
+
 	adapter->state = __I40EVF_RESETTING;
 
 	/* kill and reinit the admin queue */
 	if (i40evf_shutdown_adminq(hw))
-		dev_warn(&adapter->pdev->dev,
-			 "%s: Failed to destroy the Admin Queue resources\n",
-			 __func__);
+		dev_warn(&adapter->pdev->dev, "Failed to shut down adminq\n");
+	adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
 	err = i40evf_init_adminq(hw);
 	if (err)
-		dev_info(&adapter->pdev->dev, "%s: init_adminq failed: %d\n",
-			 __func__, err);
+		dev_info(&adapter->pdev->dev, "Failed to init adminq: %d\n",
+			 err);
 
-	adapter->aq_pending = 0;
-	adapter->aq_required = 0;
 	i40evf_map_queues(adapter);
+
+	/* re-add all MAC filters */
+	list_for_each_entry(f, &adapter->mac_filter_list, list) {
+		f->add = true;
+	}
+	/* re-add all VLAN filters */
+	list_for_each_entry(f, &adapter->vlan_filter_list, list) {
+		f->add = true;
+	}
+	adapter->aq_required = I40EVF_FLAG_AQ_ADD_MAC_FILTER;
+	adapter->aq_required |= I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
 	clear_bit(__I40EVF_IN_CRITICAL_TASK, &adapter->crit_section);
 
 	mod_timer(&adapter->watchdog_timer, jiffies + 2);
@@ -1688,7 +1717,7 @@ out:
  *
  * Free all transmit software resources
  **/
-static void i40evf_free_all_tx_resources(struct i40evf_adapter *adapter)
+void i40evf_free_all_tx_resources(struct i40evf_adapter *adapter)
 {
 	int i;
 
@@ -1758,7 +1787,7 @@ static int i40evf_setup_all_rx_resources(struct i40evf_adapter *adapter)
  *
  * Free all receive software resources
  **/
-static void i40evf_free_all_rx_resources(struct i40evf_adapter *adapter)
+void i40evf_free_all_rx_resources(struct i40evf_adapter *adapter)
 {
 	int i;
 
@@ -1788,7 +1817,7 @@ static int i40evf_open(struct net_device *netdev)
 		dev_err(&adapter->pdev->dev, "Unable to open device due to PF driver failure.\n");
 		return -EIO;
 	}
-	if (adapter->state != __I40EVF_DOWN)
+	if (adapter->state != __I40EVF_DOWN || adapter->aq_required)
 		return -EBUSY;
 
 	/* allocate transmit descriptors */
@@ -1852,9 +1881,6 @@ static int i40evf_close(struct net_device *netdev)
 	adapter->state = __I40EVF_DOWN;
 	i40evf_free_traffic_irqs(adapter);
 
-	i40evf_free_all_tx_resources(adapter);
-	i40evf_free_all_rx_resources(adapter);
-
 	return 0;
 }
 
@@ -1977,7 +2003,7 @@ static int i40evf_check_reset_complete(struct i40e_hw *hw)
  *
  * This task completes the work that was begun in probe. Due to the nature
  * of VF-PF communications, we may need to wait tens of milliseconds to get
- * reponses back from the PF. Rather than busy-wait in probe and bog down the
+ * responses back from the PF. Rather than busy-wait in probe and bog down the
  * whole system, we'll do it in a task so we can sleep.
  * This task only runs during driver init. Once we've established
  * communications with the PF driver and set up our netdev, the watchdog
@@ -2368,7 +2394,7 @@ static int i40evf_suspend(struct pci_dev *pdev, pm_message_t state)
 }
 
 /**
- * i40evf_resume - Power managment resume routine
+ * i40evf_resume - Power management resume routine
  * @pdev: PCI device information struct
  *
  * Called when the system (VM) is resumed from sleep/suspend.
@@ -2468,6 +2494,8 @@ static void i40evf_remove(struct pci_dev *pdev)
 	iounmap(hw->hw_addr);
 	pci_release_regions(pdev);
 
+	i40evf_free_all_tx_resources(adapter);
+	i40evf_free_all_rx_resources(adapter);
 	i40evf_free_queues(adapter);
 	kfree(adapter->vf_res);
 	/* If we got removed before an up/down sequence, we've got a filter
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
index 3f0c85ecbca6..4240a496dc50 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
@@ -761,6 +761,8 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 		break;
 	case I40E_VIRTCHNL_OP_DISABLE_QUEUES:
 		adapter->aq_pending &= ~(I40EVF_FLAG_AQ_DISABLE_QUEUES);
+		i40evf_free_all_tx_resources(adapter);
+		i40evf_free_all_rx_resources(adapter);
 		break;
 	case I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES:
 		adapter->aq_pending &= ~(I40EVF_FLAG_AQ_CONFIGURE_QUEUES);
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index f366b3b96d03..8457d0306e3a 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -1776,6 +1776,7 @@ void igb_down(struct igb_adapter *adapter)
 	wr32(E1000_RCTL, rctl & ~E1000_RCTL_EN);
 	/* flush and sleep below */
 
+	netif_carrier_off(netdev);
 	netif_tx_stop_all_queues(netdev);
 
 	/* disable transmits in the hardware */
@@ -1797,12 +1798,9 @@ void igb_down(struct igb_adapter *adapter)
 		}
 	}
 
-
 	del_timer_sync(&adapter->watchdog_timer);
 	del_timer_sync(&adapter->phy_info_timer);
 
-	netif_carrier_off(netdev);
-
 	/* record the stats before reset*/
 	spin_lock(&adapter->stats64_lock);
 	igb_update_stats(adapter, &adapter->stats64);
@@ -2095,6 +2093,7 @@ static const struct net_device_ops igb_netdev_ops = {
 #endif
 	.ndo_fix_features	= igb_fix_features,
 	.ndo_set_features	= igb_set_features,
+	.ndo_features_check	= passthru_features_check,
 };
 
 /**
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index d20fc8ed11f1..e3b9b63ad010 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -30,7 +30,7 @@
  *
  * Neither the 82576 nor the 82580 offer registers wide enough to hold
  * nanoseconds time values for very long. For the 82580, SYSTIM always
- * counts nanoseconds, but the upper 24 bits are not availible. The
+ * counts nanoseconds, but the upper 24 bits are not available. The
  * frequency is adjusted by changing the 32 bit fractional nanoseconds
  * register, TIMINCA.
  *
@@ -116,7 +116,8 @@ static cycle_t igb_ptp_read_82580(const struct cyclecounter *cc)
 }
 
 /* SYSTIM read access for I210/I211 */
-static void igb_ptp_read_i210(struct igb_adapter *adapter, struct timespec *ts)
+static void igb_ptp_read_i210(struct igb_adapter *adapter,
+			      struct timespec64 *ts)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 sec, nsec;
@@ -134,7 +135,7 @@ static void igb_ptp_read_i210(struct igb_adapter *adapter, struct timespec *ts)
 }
 
 static void igb_ptp_write_i210(struct igb_adapter *adapter,
-			       const struct timespec *ts)
+			       const struct timespec64 *ts)
 {
 	struct e1000_hw *hw = &adapter->hw;
 
@@ -269,13 +270,13 @@ static int igb_ptp_adjtime_i210(struct ptp_clock_info *ptp, s64 delta)
 	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
 					       ptp_caps);
 	unsigned long flags;
-	struct timespec now, then = ns_to_timespec(delta);
+	struct timespec64 now, then = ns_to_timespec64(delta);
 
 	spin_lock_irqsave(&igb->tmreg_lock, flags);
 
 	igb_ptp_read_i210(igb, &now);
-	now = timespec_add(now, then);
-	igb_ptp_write_i210(igb, (const struct timespec *)&now);
+	now = timespec64_add(now, then);
+	igb_ptp_write_i210(igb, (const struct timespec64 *)&now);
 
 	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
 
@@ -283,13 +284,12 @@ static int igb_ptp_adjtime_i210(struct ptp_clock_info *ptp, s64 delta)
 }
 
 static int igb_ptp_gettime_82576(struct ptp_clock_info *ptp,
-				 struct timespec *ts)
+				 struct timespec64 *ts)
 {
 	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
 					       ptp_caps);
 	unsigned long flags;
 	u64 ns;
-	u32 remainder;
 
 	spin_lock_irqsave(&igb->tmreg_lock, flags);
 
@@ -297,14 +297,13 @@ static int igb_ptp_gettime_82576(struct ptp_clock_info *ptp,
 
 	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
 
 static int igb_ptp_gettime_i210(struct ptp_clock_info *ptp,
-				struct timespec *ts)
+				struct timespec64 *ts)
 {
 	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
 					       ptp_caps);
@@ -320,15 +319,14 @@ static int igb_ptp_gettime_i210(struct ptp_clock_info *ptp,
 }
 
 static int igb_ptp_settime_82576(struct ptp_clock_info *ptp,
-				 const struct timespec *ts)
+				 const struct timespec64 *ts)
 {
 	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
 					       ptp_caps);
 	unsigned long flags;
 	u64 ns;
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 
 	spin_lock_irqsave(&igb->tmreg_lock, flags);
 
@@ -340,7 +338,7 @@ static int igb_ptp_settime_82576(struct ptp_clock_info *ptp,
 }
 
 static int igb_ptp_settime_i210(struct ptp_clock_info *ptp,
-				const struct timespec *ts)
+				const struct timespec64 *ts)
 {
 	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
 					       ptp_caps);
@@ -358,7 +356,7 @@ static int igb_ptp_settime_i210(struct ptp_clock_info *ptp,
 static void igb_pin_direction(int pin, int input, u32 *ctrl, u32 *ctrl_ext)
 {
 	u32 *ptr = pin < 2 ? ctrl : ctrl_ext;
-	u32 mask[IGB_N_SDP] = {
+	static const u32 mask[IGB_N_SDP] = {
 		E1000_CTRL_SDP0_DIR,
 		E1000_CTRL_SDP1_DIR,
 		E1000_CTRL_EXT_SDP2_DIR,
@@ -373,16 +371,16 @@ static void igb_pin_direction(int pin, int input, u32 *ctrl, u32 *ctrl_ext)
 
 static void igb_pin_extts(struct igb_adapter *igb, int chan, int pin)
 {
-	struct e1000_hw *hw = &igb->hw;
-	u32 aux0_sel_sdp[IGB_N_SDP] = {
+	static const u32 aux0_sel_sdp[IGB_N_SDP] = {
 		AUX0_SEL_SDP0, AUX0_SEL_SDP1, AUX0_SEL_SDP2, AUX0_SEL_SDP3,
 	};
-	u32 aux1_sel_sdp[IGB_N_SDP] = {
+	static const u32 aux1_sel_sdp[IGB_N_SDP] = {
 		AUX1_SEL_SDP0, AUX1_SEL_SDP1, AUX1_SEL_SDP2, AUX1_SEL_SDP3,
 	};
-	u32 ts_sdp_en[IGB_N_SDP] = {
+	static const u32 ts_sdp_en[IGB_N_SDP] = {
 		TS_SDP0_EN, TS_SDP1_EN, TS_SDP2_EN, TS_SDP3_EN,
 	};
+	struct e1000_hw *hw = &igb->hw;
 	u32 ctrl, ctrl_ext, tssdp = 0;
 
 	ctrl = rd32(E1000_CTRL);
@@ -409,28 +407,28 @@ static void igb_pin_extts(struct igb_adapter *igb, int chan, int pin)
 
 static void igb_pin_perout(struct igb_adapter *igb, int chan, int pin)
 {
-	struct e1000_hw *hw = &igb->hw;
-	u32 aux0_sel_sdp[IGB_N_SDP] = {
+	static const u32 aux0_sel_sdp[IGB_N_SDP] = {
 		AUX0_SEL_SDP0, AUX0_SEL_SDP1, AUX0_SEL_SDP2, AUX0_SEL_SDP3,
 	};
-	u32 aux1_sel_sdp[IGB_N_SDP] = {
+	static const u32 aux1_sel_sdp[IGB_N_SDP] = {
 		AUX1_SEL_SDP0, AUX1_SEL_SDP1, AUX1_SEL_SDP2, AUX1_SEL_SDP3,
 	};
-	u32 ts_sdp_en[IGB_N_SDP] = {
+	static const u32 ts_sdp_en[IGB_N_SDP] = {
 		TS_SDP0_EN, TS_SDP1_EN, TS_SDP2_EN, TS_SDP3_EN,
 	};
-	u32 ts_sdp_sel_tt0[IGB_N_SDP] = {
+	static const u32 ts_sdp_sel_tt0[IGB_N_SDP] = {
 		TS_SDP0_SEL_TT0, TS_SDP1_SEL_TT0,
 		TS_SDP2_SEL_TT0, TS_SDP3_SEL_TT0,
 	};
-	u32 ts_sdp_sel_tt1[IGB_N_SDP] = {
+	static const u32 ts_sdp_sel_tt1[IGB_N_SDP] = {
 		TS_SDP0_SEL_TT1, TS_SDP1_SEL_TT1,
 		TS_SDP2_SEL_TT1, TS_SDP3_SEL_TT1,
 	};
-	u32 ts_sdp_sel_clr[IGB_N_SDP] = {
+	static const u32 ts_sdp_sel_clr[IGB_N_SDP] = {
 		TS_SDP0_SEL_FC1, TS_SDP1_SEL_FC1,
 		TS_SDP2_SEL_FC1, TS_SDP3_SEL_FC1,
 	};
+	struct e1000_hw *hw = &igb->hw;
 	u32 ctrl, ctrl_ext, tssdp = 0;
 
 	ctrl = rd32(E1000_CTRL);
@@ -468,7 +466,7 @@ static int igb_ptp_feature_enable_i210(struct ptp_clock_info *ptp,
 	u32 tsauxc, tsim, tsauxc_mask, tsim_mask, trgttiml, trgttimh;
 	unsigned long flags;
 	struct timespec ts;
-	int pin;
+	int pin = -1;
 	s64 ns;
 
 	switch (rq->type) {
@@ -627,11 +625,12 @@ static void igb_ptp_overflow_check(struct work_struct *work)
 {
 	struct igb_adapter *igb =
 		container_of(work, struct igb_adapter, ptp_overflow_work.work);
-	struct timespec ts;
+	struct timespec64 ts;
 
-	igb->ptp_caps.gettime(&igb->ptp_caps, &ts);
+	igb->ptp_caps.gettime64(&igb->ptp_caps, &ts);
 
-	pr_debug("igb overflow check at %ld.%09lu\n", ts.tv_sec, ts.tv_nsec);
+	pr_debug("igb overflow check at %lld.%09lu\n",
+		 (long long) ts.tv_sec, ts.tv_nsec);
 
 	schedule_delayed_work(&igb->ptp_overflow_work,
 			      IGB_SYSTIM_OVERFLOW_PERIOD);
@@ -989,8 +988,8 @@ void igb_ptp_init(struct igb_adapter *adapter)
 		adapter->ptp_caps.pps = 0;
 		adapter->ptp_caps.adjfreq = igb_ptp_adjfreq_82576;
 		adapter->ptp_caps.adjtime = igb_ptp_adjtime_82576;
-		adapter->ptp_caps.gettime = igb_ptp_gettime_82576;
-		adapter->ptp_caps.settime = igb_ptp_settime_82576;
+		adapter->ptp_caps.gettime64 = igb_ptp_gettime_82576;
+		adapter->ptp_caps.settime64 = igb_ptp_settime_82576;
 		adapter->ptp_caps.enable = igb_ptp_feature_enable;
 		adapter->cc.read = igb_ptp_read_82576;
 		adapter->cc.mask = CYCLECOUNTER_MASK(64);
@@ -1009,8 +1008,8 @@ void igb_ptp_init(struct igb_adapter *adapter)
 		adapter->ptp_caps.pps = 0;
 		adapter->ptp_caps.adjfreq = igb_ptp_adjfreq_82580;
 		adapter->ptp_caps.adjtime = igb_ptp_adjtime_82576;
-		adapter->ptp_caps.gettime = igb_ptp_gettime_82576;
-		adapter->ptp_caps.settime = igb_ptp_settime_82576;
+		adapter->ptp_caps.gettime64 = igb_ptp_gettime_82576;
+		adapter->ptp_caps.settime64 = igb_ptp_settime_82576;
 		adapter->ptp_caps.enable = igb_ptp_feature_enable;
 		adapter->cc.read = igb_ptp_read_82580;
 		adapter->cc.mask = CYCLECOUNTER_MASK(IGB_NBITS_82580);
@@ -1038,8 +1037,8 @@ void igb_ptp_init(struct igb_adapter *adapter)
 		adapter->ptp_caps.pin_config = adapter->sdp_config;
 		adapter->ptp_caps.adjfreq = igb_ptp_adjfreq_82580;
 		adapter->ptp_caps.adjtime = igb_ptp_adjtime_i210;
-		adapter->ptp_caps.gettime = igb_ptp_gettime_i210;
-		adapter->ptp_caps.settime = igb_ptp_settime_i210;
+		adapter->ptp_caps.gettime64 = igb_ptp_gettime_i210;
+		adapter->ptp_caps.settime64 = igb_ptp_settime_i210;
 		adapter->ptp_caps.enable = igb_ptp_feature_enable_i210;
 		adapter->ptp_caps.verify = igb_ptp_verify_pin;
 		/* Enable the timer functions by clearing bit 31. */
@@ -1057,7 +1056,7 @@ void igb_ptp_init(struct igb_adapter *adapter)
 
 	/* Initialize the clock and overflow work for devices that need it. */
 	if ((hw->mac.type == e1000_i210) || (hw->mac.type == e1000_i211)) {
-		struct timespec ts = ktime_to_timespec(ktime_get_real());
+		struct timespec64 ts = ktime_to_timespec64(ktime_get_real());
 
 		igb_ptp_settime_i210(&adapter->ptp_caps, &ts);
 	} else {
@@ -1171,7 +1170,7 @@ void igb_ptp_reset(struct igb_adapter *adapter)
 
 	/* Re-initialize the timer. */
 	if ((hw->mac.type == e1000_i210) || (hw->mac.type == e1000_i211)) {
-		struct timespec ts = ktime_to_timespec(ktime_get_real());
+		struct timespec64 ts = ktime_to_timespec64(ktime_get_real());
 
 		igb_ptp_write_i210(adapter, &ts);
 	} else {
diff --git a/drivers/net/ethernet/intel/igbvf/defines.h b/drivers/net/ethernet/intel/igbvf/defines.h
index d9fa999b1685..ae3f28332fa0 100644
--- a/drivers/net/ethernet/intel/igbvf/defines.h
+++ b/drivers/net/ethernet/intel/igbvf/defines.h
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -29,94 +28,93 @@
 #define _E1000_DEFINES_H_
 
 /* Number of Transmit and Receive Descriptors must be a multiple of 8 */
-#define REQ_TX_DESCRIPTOR_MULTIPLE  8
-#define REQ_RX_DESCRIPTOR_MULTIPLE  8
+#define REQ_TX_DESCRIPTOR_MULTIPLE	8
+#define REQ_RX_DESCRIPTOR_MULTIPLE	8
 
 /* IVAR valid bit */
-#define E1000_IVAR_VALID        0x80
+#define E1000_IVAR_VALID	0x80
 
 /* Receive Descriptor bit definitions */
-#define E1000_RXD_STAT_DD       0x01    /* Descriptor Done */
-#define E1000_RXD_STAT_EOP      0x02    /* End of Packet */
-#define E1000_RXD_STAT_IXSM     0x04    /* Ignore checksum */
-#define E1000_RXD_STAT_VP       0x08    /* IEEE VLAN Packet */
-#define E1000_RXD_STAT_UDPCS    0x10    /* UDP xsum calculated */
-#define E1000_RXD_STAT_TCPCS    0x20    /* TCP xsum calculated */
-#define E1000_RXD_STAT_IPCS     0x40    /* IP xsum calculated */
-#define E1000_RXD_ERR_SE        0x02    /* Symbol Error */
-#define E1000_RXD_SPC_VLAN_MASK 0x0FFF  /* VLAN ID is in lower 12 bits */
-
-#define E1000_RXDEXT_STATERR_LB    0x00040000
-#define E1000_RXDEXT_STATERR_CE    0x01000000
-#define E1000_RXDEXT_STATERR_SE    0x02000000
-#define E1000_RXDEXT_STATERR_SEQ   0x04000000
-#define E1000_RXDEXT_STATERR_CXE   0x10000000
-#define E1000_RXDEXT_STATERR_TCPE  0x20000000
-#define E1000_RXDEXT_STATERR_IPE   0x40000000
-#define E1000_RXDEXT_STATERR_RXE   0x80000000
-
+#define E1000_RXD_STAT_DD	0x01    /* Descriptor Done */
+#define E1000_RXD_STAT_EOP	0x02    /* End of Packet */
+#define E1000_RXD_STAT_IXSM	0x04    /* Ignore checksum */
+#define E1000_RXD_STAT_VP	0x08    /* IEEE VLAN Packet */
+#define E1000_RXD_STAT_UDPCS	0x10    /* UDP xsum calculated */
+#define E1000_RXD_STAT_TCPCS	0x20    /* TCP xsum calculated */
+#define E1000_RXD_STAT_IPCS	0x40    /* IP xsum calculated */
+#define E1000_RXD_ERR_SE	0x02    /* Symbol Error */
+#define E1000_RXD_SPC_VLAN_MASK	0x0FFF  /* VLAN ID is in lower 12 bits */
+
+#define E1000_RXDEXT_STATERR_LB	0x00040000
+#define E1000_RXDEXT_STATERR_CE	0x01000000
+#define E1000_RXDEXT_STATERR_SE	0x02000000
+#define E1000_RXDEXT_STATERR_SEQ	0x04000000
+#define E1000_RXDEXT_STATERR_CXE	0x10000000
+#define E1000_RXDEXT_STATERR_TCPE	0x20000000
+#define E1000_RXDEXT_STATERR_IPE	0x40000000
+#define E1000_RXDEXT_STATERR_RXE	0x80000000
 
 /* Same mask, but for extended and packet split descriptors */
 #define E1000_RXDEXT_ERR_FRAME_ERR_MASK ( \
-    E1000_RXDEXT_STATERR_CE  |            \
-    E1000_RXDEXT_STATERR_SE  |            \
-    E1000_RXDEXT_STATERR_SEQ |            \
-    E1000_RXDEXT_STATERR_CXE |            \
-    E1000_RXDEXT_STATERR_RXE)
+	E1000_RXDEXT_STATERR_CE  | \
+	E1000_RXDEXT_STATERR_SE  | \
+	E1000_RXDEXT_STATERR_SEQ | \
+	E1000_RXDEXT_STATERR_CXE | \
+	E1000_RXDEXT_STATERR_RXE)
 
 /* Device Control */
-#define E1000_CTRL_RST      0x04000000  /* Global reset */
+#define E1000_CTRL_RST		0x04000000  /* Global reset */
 
 /* Device Status */
-#define E1000_STATUS_FD         0x00000001      /* Full duplex.0=half,1=full */
-#define E1000_STATUS_LU         0x00000002      /* Link up.0=no,1=link */
-#define E1000_STATUS_TXOFF      0x00000010      /* transmission paused */
-#define E1000_STATUS_SPEED_10   0x00000000      /* Speed 10Mb/s */
-#define E1000_STATUS_SPEED_100  0x00000040      /* Speed 100Mb/s */
-#define E1000_STATUS_SPEED_1000 0x00000080      /* Speed 1000Mb/s */
-
-#define SPEED_10    10
-#define SPEED_100   100
-#define SPEED_1000  1000
-#define HALF_DUPLEX 1
-#define FULL_DUPLEX 2
+#define E1000_STATUS_FD		0x00000001      /* Full duplex.0=half,1=full */
+#define E1000_STATUS_LU		0x00000002      /* Link up.0=no,1=link */
+#define E1000_STATUS_TXOFF	0x00000010      /* transmission paused */
+#define E1000_STATUS_SPEED_10	0x00000000      /* Speed 10Mb/s */
+#define E1000_STATUS_SPEED_100	0x00000040      /* Speed 100Mb/s */
+#define E1000_STATUS_SPEED_1000	0x00000080      /* Speed 1000Mb/s */
+
+#define SPEED_10	10
+#define SPEED_100	100
+#define SPEED_1000	1000
+#define HALF_DUPLEX	1
+#define FULL_DUPLEX	2
 
 /* Transmit Descriptor bit definitions */
-#define E1000_TXD_POPTS_IXSM 0x01       /* Insert IP checksum */
-#define E1000_TXD_POPTS_TXSM 0x02       /* Insert TCP/UDP checksum */
-#define E1000_TXD_CMD_DEXT   0x20000000 /* Descriptor extension (0 = legacy) */
-#define E1000_TXD_STAT_DD    0x00000001 /* Descriptor Done */
+#define E1000_TXD_POPTS_IXSM	0x01       /* Insert IP checksum */
+#define E1000_TXD_POPTS_TXSM	0x02       /* Insert TCP/UDP checksum */
+#define E1000_TXD_CMD_DEXT	0x20000000 /* Desc extension (0 = legacy) */
+#define E1000_TXD_STAT_DD	0x00000001 /* Desc Done */
 
-#define MAX_JUMBO_FRAME_SIZE    0x3F00
+#define MAX_JUMBO_FRAME_SIZE	0x3F00
 
 /* 802.1q VLAN Packet Size */
-#define VLAN_TAG_SIZE              4    /* 802.3ac tag (not DMA'd) */
+#define VLAN_TAG_SIZE		4    /* 802.3ac tag (not DMA'd) */
 
 /* Error Codes */
-#define E1000_SUCCESS      0
-#define E1000_ERR_CONFIG   3
-#define E1000_ERR_MAC_INIT 5
-#define E1000_ERR_MBX      15
+#define E1000_SUCCESS		0
+#define E1000_ERR_CONFIG	3
+#define E1000_ERR_MAC_INIT	5
+#define E1000_ERR_MBX		15
 
 /* SRRCTL bit definitions */
-#define E1000_SRRCTL_BSIZEPKT_SHIFT                     10 /* Shift _right_ */
-#define E1000_SRRCTL_BSIZEHDRSIZE_MASK                  0x00000F00
-#define E1000_SRRCTL_BSIZEHDRSIZE_SHIFT                 2  /* Shift _left_ */
-#define E1000_SRRCTL_DESCTYPE_ADV_ONEBUF                0x02000000
-#define E1000_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS          0x0A000000
-#define E1000_SRRCTL_DESCTYPE_MASK                      0x0E000000
-#define E1000_SRRCTL_DROP_EN                            0x80000000
+#define E1000_SRRCTL_BSIZEPKT_SHIFT		10 /* Shift _right_ */
+#define E1000_SRRCTL_BSIZEHDRSIZE_MASK		0x00000F00
+#define E1000_SRRCTL_BSIZEHDRSIZE_SHIFT		2  /* Shift _left_ */
+#define E1000_SRRCTL_DESCTYPE_ADV_ONEBUF	0x02000000
+#define E1000_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS	0x0A000000
+#define E1000_SRRCTL_DESCTYPE_MASK		0x0E000000
+#define E1000_SRRCTL_DROP_EN			0x80000000
 
-#define E1000_SRRCTL_BSIZEPKT_MASK      0x0000007F
-#define E1000_SRRCTL_BSIZEHDR_MASK      0x00003F00
+#define E1000_SRRCTL_BSIZEPKT_MASK	0x0000007F
+#define E1000_SRRCTL_BSIZEHDR_MASK	0x00003F00
 
 /* Additional Descriptor Control definitions */
-#define E1000_TXDCTL_QUEUE_ENABLE  0x02000000 /* Enable specific Tx Queue */
-#define E1000_RXDCTL_QUEUE_ENABLE  0x02000000 /* Enable specific Rx Queue */
+#define E1000_TXDCTL_QUEUE_ENABLE	0x02000000 /* Enable specific Tx Que */
+#define E1000_RXDCTL_QUEUE_ENABLE	0x02000000 /* Enable specific Rx Que */
 
 /* Direct Cache Access (DCA) definitions */
-#define E1000_DCA_TXCTRL_TX_WB_RO_EN (1 << 11) /* Tx Desc writeback RO bit */
+#define E1000_DCA_TXCTRL_TX_WB_RO_EN	(1 << 11) /* Tx Desc writeback RO bit */
 
-#define E1000_VF_INIT_TIMEOUT 200 /* Number of retries to clear RSTI */
+#define E1000_VF_INIT_TIMEOUT	200 /* Number of retries to clear RSTI */
 
 #endif /* _E1000_DEFINES_H_ */
diff --git a/drivers/net/ethernet/intel/igbvf/ethtool.c b/drivers/net/ethernet/intel/igbvf/ethtool.c
index 2178f87e9f61..c6996feb1cb4 100644
--- a/drivers/net/ethernet/intel/igbvf/ethtool.c
+++ b/drivers/net/ethernet/intel/igbvf/ethtool.c
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -36,7 +35,6 @@
 #include "igbvf.h"
 #include <linux/if_vlan.h>
 
-
 struct igbvf_stats {
 	char stat_string[ETH_GSTRING_LEN];
 	int sizeof_stat;
@@ -74,7 +72,7 @@ static const char igbvf_gstrings_test[][ETH_GSTRING_LEN] = {
 #define IGBVF_TEST_LEN ARRAY_SIZE(igbvf_gstrings_test)
 
 static int igbvf_get_settings(struct net_device *netdev,
-                              struct ethtool_cmd *ecmd)
+			      struct ethtool_cmd *ecmd)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -111,18 +109,18 @@ static int igbvf_get_settings(struct net_device *netdev,
 }
 
 static int igbvf_set_settings(struct net_device *netdev,
-                              struct ethtool_cmd *ecmd)
+			      struct ethtool_cmd *ecmd)
 {
 	return -EOPNOTSUPP;
 }
 
 static void igbvf_get_pauseparam(struct net_device *netdev,
-                                 struct ethtool_pauseparam *pause)
+				 struct ethtool_pauseparam *pause)
 {
 }
 
 static int igbvf_set_pauseparam(struct net_device *netdev,
-                                struct ethtool_pauseparam *pause)
+				struct ethtool_pauseparam *pause)
 {
 	return -EOPNOTSUPP;
 }
@@ -130,12 +128,14 @@ static int igbvf_set_pauseparam(struct net_device *netdev,
 static u32 igbvf_get_msglevel(struct net_device *netdev)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
+
 	return adapter->msg_enable;
 }
 
 static void igbvf_set_msglevel(struct net_device *netdev, u32 data)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
+
 	adapter->msg_enable = data;
 }
 
@@ -146,7 +146,7 @@ static int igbvf_get_regs_len(struct net_device *netdev)
 }
 
 static void igbvf_get_regs(struct net_device *netdev,
-                           struct ethtool_regs *regs, void *p)
+			   struct ethtool_regs *regs, void *p)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -175,19 +175,19 @@ static int igbvf_get_eeprom_len(struct net_device *netdev)
 }
 
 static int igbvf_get_eeprom(struct net_device *netdev,
-                            struct ethtool_eeprom *eeprom, u8 *bytes)
+			    struct ethtool_eeprom *eeprom, u8 *bytes)
 {
 	return -EOPNOTSUPP;
 }
 
 static int igbvf_set_eeprom(struct net_device *netdev,
-                            struct ethtool_eeprom *eeprom, u8 *bytes)
+			    struct ethtool_eeprom *eeprom, u8 *bytes)
 {
 	return -EOPNOTSUPP;
 }
 
 static void igbvf_get_drvinfo(struct net_device *netdev,
-                              struct ethtool_drvinfo *drvinfo)
+			      struct ethtool_drvinfo *drvinfo)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 
@@ -201,7 +201,7 @@ static void igbvf_get_drvinfo(struct net_device *netdev,
 }
 
 static void igbvf_get_ringparam(struct net_device *netdev,
-                                struct ethtool_ringparam *ring)
+				struct ethtool_ringparam *ring)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct igbvf_ring *tx_ring = adapter->tx_ring;
@@ -214,7 +214,7 @@ static void igbvf_get_ringparam(struct net_device *netdev,
 }
 
 static int igbvf_set_ringparam(struct net_device *netdev,
-                               struct ethtool_ringparam *ring)
+			       struct ethtool_ringparam *ring)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct igbvf_ring *temp_ring;
@@ -224,12 +224,12 @@ static int igbvf_set_ringparam(struct net_device *netdev,
 	if ((ring->rx_mini_pending) || (ring->rx_jumbo_pending))
 		return -EINVAL;
 
-	new_rx_count = max(ring->rx_pending, (u32)IGBVF_MIN_RXD);
-	new_rx_count = min(new_rx_count, (u32)IGBVF_MAX_RXD);
+	new_rx_count = max_t(u32, ring->rx_pending, IGBVF_MIN_RXD);
+	new_rx_count = min_t(u32, new_rx_count, IGBVF_MAX_RXD);
 	new_rx_count = ALIGN(new_rx_count, REQ_RX_DESCRIPTOR_MULTIPLE);
 
-	new_tx_count = max(ring->tx_pending, (u32)IGBVF_MIN_TXD);
-	new_tx_count = min(new_tx_count, (u32)IGBVF_MAX_TXD);
+	new_tx_count = max_t(u32, ring->tx_pending, IGBVF_MIN_TXD);
+	new_tx_count = min_t(u32, new_tx_count, IGBVF_MAX_TXD);
 	new_tx_count = ALIGN(new_tx_count, REQ_TX_DESCRIPTOR_MULTIPLE);
 
 	if ((new_tx_count == adapter->tx_ring->count) &&
@@ -239,7 +239,7 @@ static int igbvf_set_ringparam(struct net_device *netdev,
 	}
 
 	while (test_and_set_bit(__IGBVF_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 
 	if (!netif_running(adapter->netdev)) {
 		adapter->tx_ring->count = new_tx_count;
@@ -255,10 +255,9 @@ static int igbvf_set_ringparam(struct net_device *netdev,
 
 	igbvf_down(adapter);
 
-	/*
-	 * We can't just free everything and then setup again,
+	/* We can't just free everything and then setup again,
 	 * because the ISRs in MSI-X mode get passed pointers
-	 * to the tx and rx ring structs.
+	 * to the Tx and Rx ring structs.
 	 */
 	if (new_tx_count != adapter->tx_ring->count) {
 		memcpy(temp_ring, adapter->tx_ring, sizeof(struct igbvf_ring));
@@ -283,7 +282,7 @@ static int igbvf_set_ringparam(struct net_device *netdev,
 
 		igbvf_free_rx_resources(adapter->rx_ring);
 
-		memcpy(adapter->rx_ring, temp_ring,sizeof(struct igbvf_ring));
+		memcpy(adapter->rx_ring, temp_ring, sizeof(struct igbvf_ring));
 	}
 err_setup:
 	igbvf_up(adapter);
@@ -307,14 +306,13 @@ static int igbvf_link_test(struct igbvf_adapter *adapter, u64 *data)
 }
 
 static void igbvf_diag_test(struct net_device *netdev,
-                            struct ethtool_test *eth_test, u64 *data)
+			    struct ethtool_test *eth_test, u64 *data)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 
 	set_bit(__IGBVF_TESTING, &adapter->state);
 
-	/*
-	 * Link test performed before hardware reset so autoneg doesn't
+	/* Link test performed before hardware reset so autoneg doesn't
 	 * interfere with test result
 	 */
 	if (igbvf_link_test(adapter, &data[0]))
@@ -325,20 +323,20 @@ static void igbvf_diag_test(struct net_device *netdev,
 }
 
 static void igbvf_get_wol(struct net_device *netdev,
-                          struct ethtool_wolinfo *wol)
+			  struct ethtool_wolinfo *wol)
 {
 	wol->supported = 0;
 	wol->wolopts = 0;
 }
 
 static int igbvf_set_wol(struct net_device *netdev,
-                         struct ethtool_wolinfo *wol)
+			 struct ethtool_wolinfo *wol)
 {
 	return -EOPNOTSUPP;
 }
 
 static int igbvf_get_coalesce(struct net_device *netdev,
-                              struct ethtool_coalesce *ec)
+			      struct ethtool_coalesce *ec)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 
@@ -351,13 +349,13 @@ static int igbvf_get_coalesce(struct net_device *netdev,
 }
 
 static int igbvf_set_coalesce(struct net_device *netdev,
-                              struct ethtool_coalesce *ec)
+			      struct ethtool_coalesce *ec)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 
 	if ((ec->rx_coalesce_usecs >= IGBVF_MIN_ITR_USECS) &&
-	     (ec->rx_coalesce_usecs <= IGBVF_MAX_ITR_USECS)) {
+	    (ec->rx_coalesce_usecs <= IGBVF_MAX_ITR_USECS)) {
 		adapter->current_itr = ec->rx_coalesce_usecs << 2;
 		adapter->requested_itr = 1000000000 /
 					(adapter->current_itr * 256);
@@ -366,8 +364,7 @@ static int igbvf_set_coalesce(struct net_device *netdev,
 		adapter->current_itr = IGBVF_START_ITR;
 		adapter->requested_itr = ec->rx_coalesce_usecs;
 	} else if (ec->rx_coalesce_usecs == 0) {
-		/*
-		 * The user's desire is to turn off interrupt throttling
+		/* The user's desire is to turn off interrupt throttling
 		 * altogether, but due to HW limitations, we can't do that.
 		 * Instead we set a very small value in EITR, which would
 		 * allow ~967k interrupts per second, but allow the adapter's
@@ -376,8 +373,9 @@ static int igbvf_set_coalesce(struct net_device *netdev,
 		adapter->current_itr = 4;
 		adapter->requested_itr = 1000000000 /
 					(adapter->current_itr * 256);
-	} else
+	} else {
 		return -EINVAL;
+	}
 
 	writel(adapter->current_itr,
 	       hw->hw_addr + adapter->rx_ring->itr_register);
@@ -388,15 +386,15 @@ static int igbvf_set_coalesce(struct net_device *netdev,
 static int igbvf_nway_reset(struct net_device *netdev)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
+
 	if (netif_running(netdev))
 		igbvf_reinit_locked(adapter);
 	return 0;
 }
 
-
 static void igbvf_get_ethtool_stats(struct net_device *netdev,
-                                    struct ethtool_stats *stats,
-                                    u64 *data)
+				    struct ethtool_stats *stats,
+				    u64 *data)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	int i;
@@ -404,19 +402,18 @@ static void igbvf_get_ethtool_stats(struct net_device *netdev,
 	igbvf_update_stats(adapter);
 	for (i = 0; i < IGBVF_GLOBAL_STATS_LEN; i++) {
 		char *p = (char *)adapter +
-		          igbvf_gstrings_stats[i].stat_offset;
+			  igbvf_gstrings_stats[i].stat_offset;
 		char *b = (char *)adapter +
-		          igbvf_gstrings_stats[i].base_stat_offset;
+			  igbvf_gstrings_stats[i].base_stat_offset;
 		data[i] = ((igbvf_gstrings_stats[i].sizeof_stat ==
-		            sizeof(u64)) ? (*(u64 *)p - *(u64 *)b) :
-		            (*(u32 *)p - *(u32 *)b));
+			    sizeof(u64)) ? (*(u64 *)p - *(u64 *)b) :
+			    (*(u32 *)p - *(u32 *)b));
 	}
-
 }
 
 static int igbvf_get_sset_count(struct net_device *dev, int stringset)
 {
-	switch(stringset) {
+	switch (stringset) {
 	case ETH_SS_TEST:
 		return IGBVF_TEST_LEN;
 	case ETH_SS_STATS:
@@ -427,7 +424,7 @@ static int igbvf_get_sset_count(struct net_device *dev, int stringset)
 }
 
 static void igbvf_get_strings(struct net_device *netdev, u32 stringset,
-                              u8 *data)
+			      u8 *data)
 {
 	u8 *p = data;
 	int i;
diff --git a/drivers/net/ethernet/intel/igbvf/igbvf.h b/drivers/net/ethernet/intel/igbvf/igbvf.h
index 7d6a25c8f889..f166baab8d7e 100644
--- a/drivers/net/ethernet/intel/igbvf/igbvf.h
+++ b/drivers/net/ethernet/intel/igbvf/igbvf.h
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -43,10 +42,10 @@ struct igbvf_info;
 struct igbvf_adapter;
 
 /* Interrupt defines */
-#define IGBVF_START_ITR                    488 /* ~8000 ints/sec */
-#define IGBVF_4K_ITR                       980
-#define IGBVF_20K_ITR                      196
-#define IGBVF_70K_ITR                       56
+#define IGBVF_START_ITR		488 /* ~8000 ints/sec */
+#define IGBVF_4K_ITR		980
+#define IGBVF_20K_ITR		196
+#define IGBVF_70K_ITR		56
 
 enum latency_range {
 	lowest_latency = 0,
@@ -55,56 +54,55 @@ enum latency_range {
 	latency_invalid = 255
 };
 
-
 /* Interrupt modes, as used by the IntMode parameter */
-#define IGBVF_INT_MODE_LEGACY           0
-#define IGBVF_INT_MODE_MSI              1
-#define IGBVF_INT_MODE_MSIX             2
+#define IGBVF_INT_MODE_LEGACY	0
+#define IGBVF_INT_MODE_MSI	1
+#define IGBVF_INT_MODE_MSIX	2
 
 /* Tx/Rx descriptor defines */
-#define IGBVF_DEFAULT_TXD               256
-#define IGBVF_MAX_TXD                   4096
-#define IGBVF_MIN_TXD                   80
+#define IGBVF_DEFAULT_TXD	256
+#define IGBVF_MAX_TXD		4096
+#define IGBVF_MIN_TXD		80
 
-#define IGBVF_DEFAULT_RXD               256
-#define IGBVF_MAX_RXD                   4096
-#define IGBVF_MIN_RXD                   80
+#define IGBVF_DEFAULT_RXD	256
+#define IGBVF_MAX_RXD		4096
+#define IGBVF_MIN_RXD		80
 
-#define IGBVF_MIN_ITR_USECS             10 /* 100000 irq/sec */
-#define IGBVF_MAX_ITR_USECS             10000 /* 100    irq/sec */
+#define IGBVF_MIN_ITR_USECS	10 /* 100000 irq/sec */
+#define IGBVF_MAX_ITR_USECS	10000 /* 100    irq/sec */
 
 /* RX descriptor control thresholds.
  * PTHRESH - MAC will consider prefetch if it has fewer than this number of
- *           descriptors available in its onboard memory.
- *           Setting this to 0 disables RX descriptor prefetch.
+ *	   descriptors available in its onboard memory.
+ *	   Setting this to 0 disables RX descriptor prefetch.
  * HTHRESH - MAC will only prefetch if there are at least this many descriptors
- *           available in host memory.
- *           If PTHRESH is 0, this should also be 0.
+ *	   available in host memory.
+ *	   If PTHRESH is 0, this should also be 0.
  * WTHRESH - RX descriptor writeback threshold - MAC will delay writing back
- *           descriptors until either it has this many to write back, or the
- *           ITR timer expires.
+ *	   descriptors until either it has this many to write back, or the
+ *	   ITR timer expires.
  */
-#define IGBVF_RX_PTHRESH                16
-#define IGBVF_RX_HTHRESH                8
-#define IGBVF_RX_WTHRESH                1
+#define IGBVF_RX_PTHRESH	16
+#define IGBVF_RX_HTHRESH	8
+#define IGBVF_RX_WTHRESH	1
 
 /* this is the size past which hardware will drop packets when setting LPE=0 */
-#define MAXIMUM_ETHERNET_VLAN_SIZE      1522
+#define MAXIMUM_ETHERNET_VLAN_SIZE	1522
 
-#define IGBVF_FC_PAUSE_TIME             0x0680 /* 858 usec */
+#define IGBVF_FC_PAUSE_TIME	0x0680 /* 858 usec */
 
 /* How many Tx Descriptors do we need to call netif_wake_queue ? */
-#define IGBVF_TX_QUEUE_WAKE             32
+#define IGBVF_TX_QUEUE_WAKE	32
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
-#define IGBVF_RX_BUFFER_WRITE           16 /* Must be power of 2 */
+#define IGBVF_RX_BUFFER_WRITE	16 /* Must be power of 2 */
 
-#define AUTO_ALL_MODES                  0
-#define IGBVF_EEPROM_APME               0x0400
+#define AUTO_ALL_MODES		0
+#define IGBVF_EEPROM_APME	0x0400
 
-#define IGBVF_MNG_VLAN_NONE             (-1)
+#define IGBVF_MNG_VLAN_NONE	(-1)
 
 /* Number of packet split data buffers (not including the header buffer) */
-#define PS_PAGE_BUFFERS                 (MAX_PS_BUFFERS - 1)
+#define PS_PAGE_BUFFERS		(MAX_PS_BUFFERS - 1)
 
 enum igbvf_boards {
 	board_vf,
@@ -116,8 +114,7 @@ struct igbvf_queue_stats {
 	u64 bytes;
 };
 
-/*
- * wrappers around a pointer to a socket buffer,
+/* wrappers around a pointer to a socket buffer,
  * so a DMA handle can be stored along with the buffer
  */
 struct igbvf_buffer {
@@ -148,10 +145,10 @@ union igbvf_desc {
 
 struct igbvf_ring {
 	struct igbvf_adapter *adapter;  /* backlink */
-	union igbvf_desc *desc;         /* pointer to ring memory  */
-	dma_addr_t dma;                 /* phys address of ring    */
-	unsigned int size;              /* length of ring in bytes */
-	unsigned int count;             /* number of desc. in ring */
+	union igbvf_desc *desc;	/* pointer to ring memory  */
+	dma_addr_t dma;		/* phys address of ring    */
+	unsigned int size;	/* length of ring in bytes */
+	unsigned int count;	/* number of desc. in ring */
 
 	u16 next_to_use;
 	u16 next_to_clean;
@@ -202,9 +199,7 @@ struct igbvf_adapter {
 	u32 requested_itr; /* ints/sec or adaptive */
 	u32 current_itr; /* Actual ITR register value, not ints/sec */
 
-	/*
-	 * Tx
-	 */
+	/* Tx */
 	struct igbvf_ring *tx_ring /* One per active queue */
 	____cacheline_aligned_in_smp;
 
@@ -226,9 +221,7 @@ struct igbvf_adapter {
 	u32 tx_fifo_size;
 	u32 tx_dma_failed;
 
-	/*
-	 * Rx
-	 */
+	/* Rx */
 	struct igbvf_ring *rx_ring;
 
 	u32 rx_int_delay;
@@ -249,7 +242,7 @@ struct igbvf_adapter {
 	struct net_device *netdev;
 	struct pci_dev *pdev;
 	struct net_device_stats net_stats;
-	spinlock_t stats_lock;      /* prevent concurrent stats updates */
+	spinlock_t stats_lock; /* prevent concurrent stats updates */
 
 	/* structs defined in e1000_hw.h */
 	struct e1000_hw hw;
@@ -286,16 +279,16 @@ struct igbvf_adapter {
 };
 
 struct igbvf_info {
-	enum e1000_mac_type     mac;
-	unsigned int            flags;
-	u32                     pba;
-	void                    (*init_ops)(struct e1000_hw *);
-	s32                     (*get_variants)(struct igbvf_adapter *);
+	enum e1000_mac_type	mac;
+	unsigned int		flags;
+	u32			pba;
+	void			(*init_ops)(struct e1000_hw *);
+	s32			(*get_variants)(struct igbvf_adapter *);
 };
 
 /* hardware capability, feature, and workaround flags */
-#define IGBVF_FLAG_RX_CSUM_DISABLED             (1 << 0)
-#define IGBVF_FLAG_RX_LB_VLAN_BSWAP		(1 << 1)
+#define IGBVF_FLAG_RX_CSUM_DISABLED	(1 << 0)
+#define IGBVF_FLAG_RX_LB_VLAN_BSWAP	(1 << 1)
 #define IGBVF_RX_DESC_ADV(R, i)     \
 	(&((((R).desc))[i].rx_desc))
 #define IGBVF_TX_DESC_ADV(R, i)     \
diff --git a/drivers/net/ethernet/intel/igbvf/mbx.c b/drivers/net/ethernet/intel/igbvf/mbx.c
index b4b65bc9fc5d..7b6cb4c3764c 100644
--- a/drivers/net/ethernet/intel/igbvf/mbx.c
+++ b/drivers/net/ethernet/intel/igbvf/mbx.c
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -54,10 +53,10 @@ out:
 }
 
 /**
- *  e1000_poll_for_ack - Wait for message acknowledgement
+ *  e1000_poll_for_ack - Wait for message acknowledgment
  *  @hw: pointer to the HW structure
  *
- *  returns SUCCESS if it successfully received a message acknowledgement
+ *  returns SUCCESS if it successfully received a message acknowledgment
  **/
 static s32 e1000_poll_for_ack(struct e1000_hw *hw)
 {
@@ -218,7 +217,7 @@ static s32 e1000_check_for_rst_vf(struct e1000_hw *hw)
 	s32 ret_val = -E1000_ERR_MBX;
 
 	if (!e1000_check_for_bit_vf(hw, (E1000_V2PMAILBOX_RSTD |
-	                                 E1000_V2PMAILBOX_RSTI))) {
+					 E1000_V2PMAILBOX_RSTI))) {
 		ret_val = E1000_SUCCESS;
 		hw->mbx.stats.rsts++;
 	}
@@ -239,7 +238,7 @@ static s32 e1000_obtain_mbx_lock_vf(struct e1000_hw *hw)
 	/* Take ownership of the buffer */
 	ew32(V2PMAILBOX(0), E1000_V2PMAILBOX_VFU);
 
-	/* reserve mailbox for vf use */
+	/* reserve mailbox for VF use */
 	if (e1000_read_v2p_mailbox(hw) & E1000_V2PMAILBOX_VFU)
 		ret_val = E1000_SUCCESS;
 
@@ -283,7 +282,7 @@ out_no_write:
 }
 
 /**
- *  e1000_read_mbx_vf - Reads a message from the inbox intended for vf
+ *  e1000_read_mbx_vf - Reads a message from the inbox intended for VF
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -315,17 +314,18 @@ out_no_read:
 }
 
 /**
- *  e1000_init_mbx_params_vf - set initial values for vf mailbox
+ *  e1000_init_mbx_params_vf - set initial values for VF mailbox
  *  @hw: pointer to the HW structure
  *
- *  Initializes the hw->mbx struct to correct values for vf mailbox
+ *  Initializes the hw->mbx struct to correct values for VF mailbox
  */
 s32 e1000_init_mbx_params_vf(struct e1000_hw *hw)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 
 	/* start mailbox as timed out and let the reset_hw call set the timeout
-	 * value to being communications */
+	 * value to being communications
+	 */
 	mbx->timeout = 0;
 	mbx->usec_delay = E1000_VF_MBX_INIT_DELAY;
 
@@ -347,4 +347,3 @@ s32 e1000_init_mbx_params_vf(struct e1000_hw *hw)
 
 	return E1000_SUCCESS;
 }
-
diff --git a/drivers/net/ethernet/intel/igbvf/mbx.h b/drivers/net/ethernet/intel/igbvf/mbx.h
index 24370bcb0e22..f800bf8eedae 100644
--- a/drivers/net/ethernet/intel/igbvf/mbx.h
+++ b/drivers/net/ethernet/intel/igbvf/mbx.h
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -30,44 +29,44 @@
 
 #include "vf.h"
 
-#define E1000_V2PMAILBOX_REQ   0x00000001 /* Request for PF Ready bit */
-#define E1000_V2PMAILBOX_ACK   0x00000002 /* Ack PF message received */
-#define E1000_V2PMAILBOX_VFU   0x00000004 /* VF owns the mailbox buffer */
-#define E1000_V2PMAILBOX_PFU   0x00000008 /* PF owns the mailbox buffer */
-#define E1000_V2PMAILBOX_PFSTS 0x00000010 /* PF wrote a message in the MB */
-#define E1000_V2PMAILBOX_PFACK 0x00000020 /* PF ack the previous VF msg */
-#define E1000_V2PMAILBOX_RSTI  0x00000040 /* PF has reset indication */
-#define E1000_V2PMAILBOX_RSTD  0x00000080 /* PF has indicated reset done */
+#define E1000_V2PMAILBOX_REQ	0x00000001 /* Request for PF Ready bit */
+#define E1000_V2PMAILBOX_ACK	0x00000002 /* Ack PF message received */
+#define E1000_V2PMAILBOX_VFU	0x00000004 /* VF owns the mailbox buffer */
+#define E1000_V2PMAILBOX_PFU	0x00000008 /* PF owns the mailbox buffer */
+#define E1000_V2PMAILBOX_PFSTS	0x00000010 /* PF wrote a message in the MB */
+#define E1000_V2PMAILBOX_PFACK	0x00000020 /* PF ack the previous VF msg */
+#define E1000_V2PMAILBOX_RSTI	0x00000040 /* PF has reset indication */
+#define E1000_V2PMAILBOX_RSTD	0x00000080 /* PF has indicated reset done */
 #define E1000_V2PMAILBOX_R2C_BITS 0x000000B0 /* All read to clear bits */
 
-#define E1000_VFMAILBOX_SIZE   16 /* 16 32 bit words - 64 bytes */
+#define E1000_VFMAILBOX_SIZE	16 /* 16 32 bit words - 64 bytes */
 
 /* If it's a E1000_VF_* msg then it originates in the VF and is sent to the
  * PF.  The reverse is true if it is E1000_PF_*.
  * Message ACK's are the value or'd with 0xF0000000
  */
-#define E1000_VT_MSGTYPE_ACK      0x80000000  /* Messages below or'd with
-                                               * this are the ACK */
-#define E1000_VT_MSGTYPE_NACK     0x40000000  /* Messages below or'd with
-                                               * this are the NACK */
-#define E1000_VT_MSGTYPE_CTS      0x20000000  /* Indicates that VF is still
-                                                 clear to send requests */
+/* Messages below or'd with this are the ACK */
+#define E1000_VT_MSGTYPE_ACK	0x80000000
+/* Messages below or'd with this are the NACK */
+#define E1000_VT_MSGTYPE_NACK	0x40000000
+/* Indicates that VF is still clear to send requests */
+#define E1000_VT_MSGTYPE_CTS	0x20000000
 
 /* We have a total wait time of 1s for vf mailbox posted messages */
-#define E1000_VF_MBX_INIT_TIMEOUT 2000 /* retry count for mailbox timeout */
-#define E1000_VF_MBX_INIT_DELAY   500  /* usec delay between retries */
+#define E1000_VF_MBX_INIT_TIMEOUT	2000 /* retry count for mbx timeout */
+#define E1000_VF_MBX_INIT_DELAY		500  /* usec delay between retries */
 
-#define E1000_VT_MSGINFO_SHIFT    16
+#define E1000_VT_MSGINFO_SHIFT	16
 /* bits 23:16 are used for exra info for certain messages */
-#define E1000_VT_MSGINFO_MASK     (0xFF << E1000_VT_MSGINFO_SHIFT)
+#define E1000_VT_MSGINFO_MASK	(0xFF << E1000_VT_MSGINFO_SHIFT)
 
-#define E1000_VF_RESET            0x01 /* VF requests reset */
-#define E1000_VF_SET_MAC_ADDR     0x02 /* VF requests PF to set MAC addr */
-#define E1000_VF_SET_MULTICAST    0x03 /* VF requests PF to set MC addr */
-#define E1000_VF_SET_VLAN         0x04 /* VF requests PF to set VLAN */
-#define E1000_VF_SET_LPE          0x05 /* VF requests PF to set VMOLR.LPE */
+#define E1000_VF_RESET		0x01 /* VF requests reset */
+#define E1000_VF_SET_MAC_ADDR	0x02 /* VF requests PF to set MAC addr */
+#define E1000_VF_SET_MULTICAST	0x03 /* VF requests PF to set MC addr */
+#define E1000_VF_SET_VLAN	0x04 /* VF requests PF to set VLAN */
+#define E1000_VF_SET_LPE	0x05 /* VF requests PF to set VMOLR.LPE */
 
-#define E1000_PF_CONTROL_MSG      0x0100 /* PF control message */
+#define E1000_PF_CONTROL_MSG	0x0100 /* PF control message */
 
 void e1000_init_mbx_ops_generic(struct e1000_hw *hw);
 s32 e1000_init_mbx_params_vf(struct e1000_hw *);
diff --git a/drivers/net/ethernet/intel/igbvf/netdev.c b/drivers/net/ethernet/intel/igbvf/netdev.c
index ebf9d4a42fdd..95af14e139d7 100644
--- a/drivers/net/ethernet/intel/igbvf/netdev.c
+++ b/drivers/net/ethernet/intel/igbvf/netdev.c
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -66,26 +65,27 @@ static void igbvf_set_interrupt_capability(struct igbvf_adapter *);
 static void igbvf_reset_interrupt_capability(struct igbvf_adapter *);
 
 static struct igbvf_info igbvf_vf_info = {
-	.mac                    = e1000_vfadapt,
-	.flags                  = 0,
-	.pba                    = 10,
-	.init_ops               = e1000_init_function_pointers_vf,
+	.mac		= e1000_vfadapt,
+	.flags		= 0,
+	.pba		= 10,
+	.init_ops	= e1000_init_function_pointers_vf,
 };
 
 static struct igbvf_info igbvf_i350_vf_info = {
-	.mac			= e1000_vfadapt_i350,
-	.flags			= 0,
-	.pba			= 10,
-	.init_ops		= e1000_init_function_pointers_vf,
+	.mac		= e1000_vfadapt_i350,
+	.flags		= 0,
+	.pba		= 10,
+	.init_ops	= e1000_init_function_pointers_vf,
 };
 
 static const struct igbvf_info *igbvf_info_tbl[] = {
-	[board_vf]              = &igbvf_vf_info,
-	[board_i350_vf]		= &igbvf_i350_vf_info,
+	[board_vf]	= &igbvf_vf_info,
+	[board_i350_vf]	= &igbvf_i350_vf_info,
 };
 
 /**
  * igbvf_desc_unused - calculate if we have unused descriptors
+ * @rx_ring: address of receive ring structure
  **/
 static int igbvf_desc_unused(struct igbvf_ring *ring)
 {
@@ -103,9 +103,9 @@ static int igbvf_desc_unused(struct igbvf_ring *ring)
  * @skb: pointer to sk_buff to be indicated to stack
  **/
 static void igbvf_receive_skb(struct igbvf_adapter *adapter,
-                              struct net_device *netdev,
-                              struct sk_buff *skb,
-                              u32 status, u16 vlan)
+			      struct net_device *netdev,
+			      struct sk_buff *skb,
+			      u32 status, u16 vlan)
 {
 	u16 vid;
 
@@ -123,7 +123,7 @@ static void igbvf_receive_skb(struct igbvf_adapter *adapter,
 }
 
 static inline void igbvf_rx_checksum_adv(struct igbvf_adapter *adapter,
-                                         u32 status_err, struct sk_buff *skb)
+					 u32 status_err, struct sk_buff *skb)
 {
 	skb_checksum_none_assert(skb);
 
@@ -153,7 +153,7 @@ static inline void igbvf_rx_checksum_adv(struct igbvf_adapter *adapter,
  * @cleaned_count: number of buffers to repopulate
  **/
 static void igbvf_alloc_rx_buffers(struct igbvf_ring *rx_ring,
-                                   int cleaned_count)
+				   int cleaned_count)
 {
 	struct igbvf_adapter *adapter = rx_ring->adapter;
 	struct net_device *netdev = adapter->netdev;
@@ -188,8 +188,8 @@ static void igbvf_alloc_rx_buffers(struct igbvf_ring *rx_ring,
 			}
 			buffer_info->page_dma =
 				dma_map_page(&pdev->dev, buffer_info->page,
-				             buffer_info->page_offset,
-				             PAGE_SIZE / 2,
+					     buffer_info->page_offset,
+					     PAGE_SIZE / 2,
 					     DMA_FROM_DEVICE);
 			if (dma_mapping_error(&pdev->dev,
 					      buffer_info->page_dma)) {
@@ -209,7 +209,7 @@ static void igbvf_alloc_rx_buffers(struct igbvf_ring *rx_ring,
 
 			buffer_info->skb = skb;
 			buffer_info->dma = dma_map_single(&pdev->dev, skb->data,
-			                                  bufsz,
+							  bufsz,
 							  DMA_FROM_DEVICE);
 			if (dma_mapping_error(&pdev->dev, buffer_info->dma)) {
 				dev_kfree_skb(buffer_info->skb);
@@ -219,14 +219,14 @@ static void igbvf_alloc_rx_buffers(struct igbvf_ring *rx_ring,
 			}
 		}
 		/* Refresh the desc even if buffer_addrs didn't change because
-		 * each write-back erases this info. */
+		 * each write-back erases this info.
+		 */
 		if (adapter->rx_ps_hdr_size) {
 			rx_desc->read.pkt_addr =
 			     cpu_to_le64(buffer_info->page_dma);
 			rx_desc->read.hdr_addr = cpu_to_le64(buffer_info->dma);
 		} else {
-			rx_desc->read.pkt_addr =
-			     cpu_to_le64(buffer_info->dma);
+			rx_desc->read.pkt_addr = cpu_to_le64(buffer_info->dma);
 			rx_desc->read.hdr_addr = 0;
 		}
 
@@ -247,7 +247,8 @@ no_buffers:
 		/* Force memory writes to complete before letting h/w
 		 * know there are new descriptors to fetch.  (Only
 		 * applicable for weak-ordered memory model archs,
-		 * such as IA-64). */
+		 * such as IA-64).
+		*/
 		wmb();
 		writel(i, adapter->hw.hw_addr + rx_ring->tail);
 	}
@@ -261,7 +262,7 @@ no_buffers:
  * is no guarantee that everything was cleaned
  **/
 static bool igbvf_clean_rx_irq(struct igbvf_adapter *adapter,
-                               int *work_done, int work_to_do)
+			       int *work_done, int work_to_do)
 {
 	struct igbvf_ring *rx_ring = adapter->rx_ring;
 	struct net_device *netdev = adapter->netdev;
@@ -292,8 +293,9 @@ static bool igbvf_clean_rx_irq(struct igbvf_adapter *adapter,
 		 * that case, it fills the header buffer and spills the rest
 		 * into the page.
 		 */
-		hlen = (le16_to_cpu(rx_desc->wb.lower.lo_dword.hs_rss.hdr_info) &
-		  E1000_RXDADV_HDRBUFLEN_MASK) >> E1000_RXDADV_HDRBUFLEN_SHIFT;
+		hlen = (le16_to_cpu(rx_desc->wb.lower.lo_dword.hs_rss.hdr_info)
+		       & E1000_RXDADV_HDRBUFLEN_MASK) >>
+		       E1000_RXDADV_HDRBUFLEN_SHIFT;
 		if (hlen > adapter->rx_ps_hdr_size)
 			hlen = adapter->rx_ps_hdr_size;
 
@@ -306,7 +308,7 @@ static bool igbvf_clean_rx_irq(struct igbvf_adapter *adapter,
 		buffer_info->skb = NULL;
 		if (!adapter->rx_ps_hdr_size) {
 			dma_unmap_single(&pdev->dev, buffer_info->dma,
-			                 adapter->rx_buffer_len,
+					 adapter->rx_buffer_len,
 					 DMA_FROM_DEVICE);
 			buffer_info->dma = 0;
 			skb_put(skb, length);
@@ -315,21 +317,21 @@ static bool igbvf_clean_rx_irq(struct igbvf_adapter *adapter,
 
 		if (!skb_shinfo(skb)->nr_frags) {
 			dma_unmap_single(&pdev->dev, buffer_info->dma,
-			                 adapter->rx_ps_hdr_size,
+					 adapter->rx_ps_hdr_size,
 					 DMA_FROM_DEVICE);
 			skb_put(skb, hlen);
 		}
 
 		if (length) {
 			dma_unmap_page(&pdev->dev, buffer_info->page_dma,
-			               PAGE_SIZE / 2,
+				       PAGE_SIZE / 2,
 				       DMA_FROM_DEVICE);
 			buffer_info->page_dma = 0;
 
 			skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
-			                   buffer_info->page,
-			                   buffer_info->page_offset,
-			                   length);
+					   buffer_info->page,
+					   buffer_info->page_offset,
+					   length);
 
 			if ((adapter->rx_buffer_len > (PAGE_SIZE / 2)) ||
 			    (page_count(buffer_info->page) != 1))
@@ -370,7 +372,7 @@ send_up:
 		skb->protocol = eth_type_trans(skb, netdev);
 
 		igbvf_receive_skb(adapter, netdev, skb, staterr,
-		                  rx_desc->wb.upper.vlan);
+				  rx_desc->wb.upper.vlan);
 
 next_desc:
 		rx_desc->wb.upper.status_error = 0;
@@ -402,7 +404,7 @@ next_desc:
 }
 
 static void igbvf_put_txbuf(struct igbvf_adapter *adapter,
-                            struct igbvf_buffer *buffer_info)
+			    struct igbvf_buffer *buffer_info)
 {
 	if (buffer_info->dma) {
 		if (buffer_info->mapped_as_page)
@@ -431,7 +433,7 @@ static void igbvf_put_txbuf(struct igbvf_adapter *adapter,
  * Return 0 on success, negative on failure
  **/
 int igbvf_setup_tx_resources(struct igbvf_adapter *adapter,
-                             struct igbvf_ring *tx_ring)
+			     struct igbvf_ring *tx_ring)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	int size;
@@ -458,7 +460,7 @@ int igbvf_setup_tx_resources(struct igbvf_adapter *adapter,
 err:
 	vfree(tx_ring->buffer_info);
 	dev_err(&adapter->pdev->dev,
-	        "Unable to allocate memory for the transmit descriptor ring\n");
+		"Unable to allocate memory for the transmit descriptor ring\n");
 	return -ENOMEM;
 }
 
@@ -501,7 +503,7 @@ err:
 	vfree(rx_ring->buffer_info);
 	rx_ring->buffer_info = NULL;
 	dev_err(&adapter->pdev->dev,
-	        "Unable to allocate memory for the receive descriptor ring\n");
+		"Unable to allocate memory for the receive descriptor ring\n");
 	return -ENOMEM;
 }
 
@@ -578,13 +580,13 @@ static void igbvf_clean_rx_ring(struct igbvf_ring *rx_ring)
 	for (i = 0; i < rx_ring->count; i++) {
 		buffer_info = &rx_ring->buffer_info[i];
 		if (buffer_info->dma) {
-			if (adapter->rx_ps_hdr_size){
+			if (adapter->rx_ps_hdr_size) {
 				dma_unmap_single(&pdev->dev, buffer_info->dma,
-				                 adapter->rx_ps_hdr_size,
+						 adapter->rx_ps_hdr_size,
 						 DMA_FROM_DEVICE);
 			} else {
 				dma_unmap_single(&pdev->dev, buffer_info->dma,
-				                 adapter->rx_buffer_len,
+						 adapter->rx_buffer_len,
 						 DMA_FROM_DEVICE);
 			}
 			buffer_info->dma = 0;
@@ -599,7 +601,7 @@ static void igbvf_clean_rx_ring(struct igbvf_ring *rx_ring)
 			if (buffer_info->page_dma)
 				dma_unmap_page(&pdev->dev,
 					       buffer_info->page_dma,
-				               PAGE_SIZE / 2,
+					       PAGE_SIZE / 2,
 					       DMA_FROM_DEVICE);
 			put_page(buffer_info->page);
 			buffer_info->page = NULL;
@@ -638,7 +640,7 @@ void igbvf_free_rx_resources(struct igbvf_ring *rx_ring)
 	rx_ring->buffer_info = NULL;
 
 	dma_free_coherent(&pdev->dev, rx_ring->size, rx_ring->desc,
-	                  rx_ring->dma);
+			  rx_ring->dma);
 	rx_ring->desc = NULL;
 }
 
@@ -649,13 +651,12 @@ void igbvf_free_rx_resources(struct igbvf_ring *rx_ring)
  * @packets: the number of packets during this measurement interval
  * @bytes: the number of bytes during this measurement interval
  *
- *      Stores a new ITR value based on packets and byte
- *      counts during the last interrupt.  The advantage of per interrupt
- *      computation is faster updates and more accurate ITR for the current
- *      traffic pattern.  Constants in this function were computed
- *      based on theoretical maximum wire speed and thresholds were set based
- *      on testing data as well as attempting to minimize response time
- *      while increasing bulk throughput.
+ * Stores a new ITR value based on packets and byte counts during the last
+ * interrupt.  The advantage of per interrupt computation is faster updates
+ * and more accurate ITR for the current traffic pattern.  Constants in this
+ * function were computed based on theoretical maximum wire speed and thresholds
+ * were set based on testing data as well as attempting to minimize response
+ * time while increasing bulk throughput.
  **/
 static enum latency_range igbvf_update_itr(struct igbvf_adapter *adapter,
 					   enum latency_range itr_setting,
@@ -744,17 +745,15 @@ static void igbvf_set_itr(struct igbvf_adapter *adapter)
 
 	new_itr = igbvf_range_to_itr(adapter->tx_ring->itr_range);
 
-
 	if (new_itr != adapter->tx_ring->itr_val) {
 		u32 current_itr = adapter->tx_ring->itr_val;
-		/*
-		 * this attempts to bias the interrupt rate towards Bulk
+		/* this attempts to bias the interrupt rate towards Bulk
 		 * by adding intermediate steps when interrupt rate is
 		 * increasing
 		 */
 		new_itr = new_itr > current_itr ?
-			     min(current_itr + (new_itr >> 2), new_itr) :
-			     new_itr;
+			  min(current_itr + (new_itr >> 2), new_itr) :
+			  new_itr;
 		adapter->tx_ring->itr_val = new_itr;
 
 		adapter->tx_ring->set_itr = 1;
@@ -772,9 +771,10 @@ static void igbvf_set_itr(struct igbvf_adapter *adapter)
 
 	if (new_itr != adapter->rx_ring->itr_val) {
 		u32 current_itr = adapter->rx_ring->itr_val;
+
 		new_itr = new_itr > current_itr ?
-			     min(current_itr + (new_itr >> 2), new_itr) :
-			     new_itr;
+			  min(current_itr + (new_itr >> 2), new_itr) :
+			  new_itr;
 		adapter->rx_ring->itr_val = new_itr;
 
 		adapter->rx_ring->set_itr = 1;
@@ -829,7 +829,7 @@ static bool igbvf_clean_tx_irq(struct igbvf_ring *tx_ring)
 				segs = skb_shinfo(skb)->gso_segs ?: 1;
 				/* multiply data chunks by size of headers */
 				bytecount = ((segs - 1) * skb_headlen(skb)) +
-				            skb->len;
+					    skb->len;
 				total_packets += segs;
 				total_bytes += bytecount;
 			}
@@ -849,9 +849,8 @@ static bool igbvf_clean_tx_irq(struct igbvf_ring *tx_ring)
 
 	tx_ring->next_to_clean = i;
 
-	if (unlikely(count &&
-	             netif_carrier_ok(netdev) &&
-	             igbvf_desc_unused(tx_ring) >= IGBVF_TX_QUEUE_WAKE)) {
+	if (unlikely(count && netif_carrier_ok(netdev) &&
+	    igbvf_desc_unused(tx_ring) >= IGBVF_TX_QUEUE_WAKE)) {
 		/* Make sure that anybody stopping the queue after this
 		 * sees the new next_to_clean.
 		 */
@@ -902,8 +901,9 @@ static irqreturn_t igbvf_intr_msix_tx(int irq, void *data)
 	adapter->total_tx_bytes = 0;
 	adapter->total_tx_packets = 0;
 
-	/* auto mask will automatically reenable the interrupt when we write
-	 * EICS */
+	/* auto mask will automatically re-enable the interrupt when we write
+	 * EICS
+	 */
 	if (!igbvf_clean_tx_irq(tx_ring))
 		/* Ring was not completely cleaned, so fire another interrupt */
 		ew32(EICS, tx_ring->eims_value);
@@ -941,15 +941,16 @@ static irqreturn_t igbvf_intr_msix_rx(int irq, void *data)
 #define IGBVF_NO_QUEUE -1
 
 static void igbvf_assign_vector(struct igbvf_adapter *adapter, int rx_queue,
-                                int tx_queue, int msix_vector)
+				int tx_queue, int msix_vector)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 ivar, index;
 
 	/* 82576 uses a table-based method for assigning vectors.
-	   Each queue has a single entry in the table to which we write
-	   a vector number along with a "valid" bit.  Sadly, the layout
-	   of the table is somewhat counterintuitive. */
+	 * Each queue has a single entry in the table to which we write
+	 * a vector number along with a "valid" bit.  Sadly, the layout
+	 * of the table is somewhat counterintuitive.
+	 */
 	if (rx_queue > IGBVF_NO_QUEUE) {
 		index = (rx_queue >> 1);
 		ivar = array_er32(IVAR0, index);
@@ -984,6 +985,7 @@ static void igbvf_assign_vector(struct igbvf_adapter *adapter, int rx_queue,
 
 /**
  * igbvf_configure_msix - Configure MSI-X hardware
+ * @adapter: board private structure
  *
  * igbvf_configure_msix sets up the hardware to properly
  * generate MSI-X interrupts.
@@ -1027,6 +1029,7 @@ static void igbvf_reset_interrupt_capability(struct igbvf_adapter *adapter)
 
 /**
  * igbvf_set_interrupt_capability - set MSI or MSI-X if supported
+ * @adapter: board private structure
  *
  * Attempt to configure interrupts using the best available
  * capabilities of the hardware and kernel.
@@ -1036,27 +1039,28 @@ static void igbvf_set_interrupt_capability(struct igbvf_adapter *adapter)
 	int err = -ENOMEM;
 	int i;
 
-	/* we allocate 3 vectors, 1 for tx, 1 for rx, one for pf messages */
+	/* we allocate 3 vectors, 1 for Tx, 1 for Rx, one for PF messages */
 	adapter->msix_entries = kcalloc(3, sizeof(struct msix_entry),
-	                                GFP_KERNEL);
+					GFP_KERNEL);
 	if (adapter->msix_entries) {
 		for (i = 0; i < 3; i++)
 			adapter->msix_entries[i].entry = i;
 
 		err = pci_enable_msix_range(adapter->pdev,
-		                            adapter->msix_entries, 3, 3);
+					    adapter->msix_entries, 3, 3);
 	}
 
 	if (err < 0) {
 		/* MSI-X failed */
 		dev_err(&adapter->pdev->dev,
-		        "Failed to initialize MSI-X interrupts.\n");
+			"Failed to initialize MSI-X interrupts.\n");
 		igbvf_reset_interrupt_capability(adapter);
 	}
 }
 
 /**
  * igbvf_request_msix - Initialize MSI-X interrupts
+ * @adapter: board private structure
  *
  * igbvf_request_msix allocates MSI-X vectors and requests interrupts from the
  * kernel.
@@ -1075,8 +1079,8 @@ static int igbvf_request_msix(struct igbvf_adapter *adapter)
 	}
 
 	err = request_irq(adapter->msix_entries[vector].vector,
-	                  igbvf_intr_msix_tx, 0, adapter->tx_ring->name,
-	                  netdev);
+			  igbvf_intr_msix_tx, 0, adapter->tx_ring->name,
+			  netdev);
 	if (err)
 		goto out;
 
@@ -1085,8 +1089,8 @@ static int igbvf_request_msix(struct igbvf_adapter *adapter)
 	vector++;
 
 	err = request_irq(adapter->msix_entries[vector].vector,
-	                  igbvf_intr_msix_rx, 0, adapter->rx_ring->name,
-	                  netdev);
+			  igbvf_intr_msix_rx, 0, adapter->rx_ring->name,
+			  netdev);
 	if (err)
 		goto out;
 
@@ -1095,7 +1099,7 @@ static int igbvf_request_msix(struct igbvf_adapter *adapter)
 	vector++;
 
 	err = request_irq(adapter->msix_entries[vector].vector,
-	                  igbvf_msix_other, 0, netdev->name, netdev);
+			  igbvf_msix_other, 0, netdev->name, netdev);
 	if (err)
 		goto out;
 
@@ -1130,6 +1134,7 @@ static int igbvf_alloc_queues(struct igbvf_adapter *adapter)
 
 /**
  * igbvf_request_irq - initialize interrupts
+ * @adapter: board private structure
  *
  * Attempts to configure interrupts using the best available
  * capabilities of the hardware and kernel.
@@ -1146,7 +1151,7 @@ static int igbvf_request_irq(struct igbvf_adapter *adapter)
 		return err;
 
 	dev_err(&adapter->pdev->dev,
-	        "Unable to allocate interrupt, Error: %d\n", err);
+		"Unable to allocate interrupt, Error: %d\n", err);
 
 	return err;
 }
@@ -1164,6 +1169,7 @@ static void igbvf_free_irq(struct igbvf_adapter *adapter)
 
 /**
  * igbvf_irq_disable - Mask off interrupt generation on the NIC
+ * @adapter: board private structure
  **/
 static void igbvf_irq_disable(struct igbvf_adapter *adapter)
 {
@@ -1177,6 +1183,7 @@ static void igbvf_irq_disable(struct igbvf_adapter *adapter)
 
 /**
  * igbvf_irq_enable - Enable default interrupt generation settings
+ * @adapter: board private structure
  **/
 static void igbvf_irq_enable(struct igbvf_adapter *adapter)
 {
@@ -1252,7 +1259,7 @@ static int igbvf_vlan_rx_kill_vid(struct net_device *netdev,
 
 	if (hw->mac.ops.set_vfta(hw, vid, false)) {
 		dev_err(&adapter->pdev->dev,
-		        "Failed to remove vlan id %d\n", vid);
+			"Failed to remove vlan id %d\n", vid);
 		return -EINVAL;
 	}
 	clear_bit(vid, adapter->active_vlans);
@@ -1298,7 +1305,7 @@ static void igbvf_configure_tx(struct igbvf_adapter *adapter)
 
 	/* Turn off Relaxed Ordering on head write-backs.  The writebacks
 	 * MUST be delivered in order or it will completely screw up
-	 * our bookeeping.
+	 * our bookkeeping.
 	 */
 	dca_txctrl = er32(DCA_TXCTRL(0));
 	dca_txctrl &= ~E1000_DCA_TXCTRL_TX_WB_RO_EN;
@@ -1325,15 +1332,15 @@ static void igbvf_setup_srrctl(struct igbvf_adapter *adapter)
 	u32 srrctl = 0;
 
 	srrctl &= ~(E1000_SRRCTL_DESCTYPE_MASK |
-	            E1000_SRRCTL_BSIZEHDR_MASK |
-	            E1000_SRRCTL_BSIZEPKT_MASK);
+		    E1000_SRRCTL_BSIZEHDR_MASK |
+		    E1000_SRRCTL_BSIZEPKT_MASK);
 
 	/* Enable queue drop to avoid head of line blocking */
 	srrctl |= E1000_SRRCTL_DROP_EN;
 
 	/* Setup buffer sizes */
 	srrctl |= ALIGN(adapter->rx_buffer_len, 1024) >>
-	          E1000_SRRCTL_BSIZEPKT_SHIFT;
+		  E1000_SRRCTL_BSIZEPKT_SHIFT;
 
 	if (adapter->rx_buffer_len < 2048) {
 		adapter->rx_ps_hdr_size = 0;
@@ -1341,7 +1348,7 @@ static void igbvf_setup_srrctl(struct igbvf_adapter *adapter)
 	} else {
 		adapter->rx_ps_hdr_size = 128;
 		srrctl |= adapter->rx_ps_hdr_size <<
-		          E1000_SRRCTL_BSIZEHDRSIZE_SHIFT;
+			  E1000_SRRCTL_BSIZEHDRSIZE_SHIFT;
 		srrctl |= E1000_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS;
 	}
 
@@ -1369,8 +1376,7 @@ static void igbvf_configure_rx(struct igbvf_adapter *adapter)
 
 	rdlen = rx_ring->count * sizeof(union e1000_adv_rx_desc);
 
-	/*
-	 * Setup the HW Rx Head and Tail Descriptor Pointers and
+	/* Setup the HW Rx Head and Tail Descriptor Pointers and
 	 * the Base and Length of the Rx Descriptor Ring
 	 */
 	rdba = rx_ring->dma;
@@ -1441,10 +1447,11 @@ static void igbvf_configure(struct igbvf_adapter *adapter)
 	igbvf_setup_srrctl(adapter);
 	igbvf_configure_rx(adapter);
 	igbvf_alloc_rx_buffers(adapter->rx_ring,
-	                       igbvf_desc_unused(adapter->rx_ring));
+			       igbvf_desc_unused(adapter->rx_ring));
 }
 
 /* igbvf_reset - bring the hardware into a known good state
+ * @adapter: private board structure
  *
  * This function boots the hardware and enables some settings that
  * require a configuration cycle of the hardware - those cannot be
@@ -1494,7 +1501,6 @@ int igbvf_up(struct igbvf_adapter *adapter)
 	hw->mac.get_link_status = 1;
 	mod_timer(&adapter->watchdog_timer, jiffies + 1);
 
-
 	return 0;
 }
 
@@ -1504,8 +1510,7 @@ void igbvf_down(struct igbvf_adapter *adapter)
 	struct e1000_hw *hw = &adapter->hw;
 	u32 rxdctl, txdctl;
 
-	/*
-	 * signal that we're down so the interrupt handler does not
+	/* signal that we're down so the interrupt handler does not
 	 * reschedule our watchdog timer
 	 */
 	set_bit(__IGBVF_DOWN, &adapter->state);
@@ -1514,6 +1519,7 @@ void igbvf_down(struct igbvf_adapter *adapter)
 	rxdctl = er32(RXDCTL(0));
 	ew32(RXDCTL(0), rxdctl & ~E1000_RXDCTL_QUEUE_ENABLE);
 
+	netif_carrier_off(netdev);
 	netif_stop_queue(netdev);
 
 	/* disable transmits in the hardware */
@@ -1530,8 +1536,6 @@ void igbvf_down(struct igbvf_adapter *adapter)
 
 	del_timer_sync(&adapter->watchdog_timer);
 
-	netif_carrier_off(netdev);
-
 	/* record the stats before reset*/
 	igbvf_update_stats(adapter);
 
@@ -1547,7 +1551,7 @@ void igbvf_reinit_locked(struct igbvf_adapter *adapter)
 {
 	might_sleep();
 	while (test_and_set_bit(__IGBVF_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 	igbvf_down(adapter);
 	igbvf_up(adapter);
 	clear_bit(__IGBVF_RESETTING, &adapter->state);
@@ -1662,8 +1666,7 @@ static int igbvf_open(struct net_device *netdev)
 	if (err)
 		goto err_setup_rx;
 
-	/*
-	 * before we allocate an interrupt, we must be ready to handle it.
+	/* before we allocate an interrupt, we must be ready to handle it.
 	 * Setting DEBUG_SHIRQ in the kernel makes it fire an interrupt
 	 * as soon as we call pci_request_irq, so we have to setup our
 	 * clean_rx handler before we do so.
@@ -1725,6 +1728,7 @@ static int igbvf_close(struct net_device *netdev)
 
 	return 0;
 }
+
 /**
  * igbvf_set_mac - Change the Ethernet Address of the NIC
  * @netdev: network interface device structure
@@ -1753,15 +1757,15 @@ static int igbvf_set_mac(struct net_device *netdev, void *p)
 	return 0;
 }
 
-#define UPDATE_VF_COUNTER(reg, name)                                    \
-	{                                                               \
-		u32 current_counter = er32(reg);                        \
-		if (current_counter < adapter->stats.last_##name)       \
-			adapter->stats.name += 0x100000000LL;           \
-		adapter->stats.last_##name = current_counter;           \
-		adapter->stats.name &= 0xFFFFFFFF00000000LL;            \
-		adapter->stats.name |= current_counter;                 \
-	}
+#define UPDATE_VF_COUNTER(reg, name) \
+{ \
+	u32 current_counter = er32(reg); \
+	if (current_counter < adapter->stats.last_##name) \
+		adapter->stats.name += 0x100000000LL; \
+	adapter->stats.last_##name = current_counter; \
+	adapter->stats.name &= 0xFFFFFFFF00000000LL; \
+	adapter->stats.name |= current_counter; \
+}
 
 /**
  * igbvf_update_stats - Update the board statistics counters
@@ -1772,8 +1776,7 @@ void igbvf_update_stats(struct igbvf_adapter *adapter)
 	struct e1000_hw *hw = &adapter->hw;
 	struct pci_dev *pdev = adapter->pdev;
 
-	/*
-	 * Prevent stats update while adapter is being reset, link is down
+	/* Prevent stats update while adapter is being reset, link is down
 	 * or if the pci connection is down.
 	 */
 	if (adapter->link_speed == 0)
@@ -1832,7 +1835,7 @@ static bool igbvf_has_link(struct igbvf_adapter *adapter)
  **/
 static void igbvf_watchdog(unsigned long data)
 {
-	struct igbvf_adapter *adapter = (struct igbvf_adapter *) data;
+	struct igbvf_adapter *adapter = (struct igbvf_adapter *)data;
 
 	/* Do the rest outside of interrupt context */
 	schedule_work(&adapter->watchdog_task);
@@ -1841,8 +1844,8 @@ static void igbvf_watchdog(unsigned long data)
 static void igbvf_watchdog_task(struct work_struct *work)
 {
 	struct igbvf_adapter *adapter = container_of(work,
-	                                             struct igbvf_adapter,
-	                                             watchdog_task);
+						     struct igbvf_adapter,
+						     watchdog_task);
 	struct net_device *netdev = adapter->netdev;
 	struct e1000_mac_info *mac = &adapter->hw.mac;
 	struct igbvf_ring *tx_ring = adapter->tx_ring;
@@ -1855,8 +1858,8 @@ static void igbvf_watchdog_task(struct work_struct *work)
 	if (link) {
 		if (!netif_carrier_ok(netdev)) {
 			mac->ops.get_link_up_info(&adapter->hw,
-			                          &adapter->link_speed,
-			                          &adapter->link_duplex);
+						  &adapter->link_speed,
+						  &adapter->link_duplex);
 			igbvf_print_link_info(adapter);
 
 			netif_carrier_on(netdev);
@@ -1876,10 +1879,9 @@ static void igbvf_watchdog_task(struct work_struct *work)
 		igbvf_update_stats(adapter);
 	} else {
 		tx_pending = (igbvf_desc_unused(tx_ring) + 1 <
-		              tx_ring->count);
+			      tx_ring->count);
 		if (tx_pending) {
-			/*
-			 * We've lost link, so the controller stops DMA,
+			/* We've lost link, so the controller stops DMA,
 			 * but we've got queued Tx work that's never going
 			 * to get done, so reset controller to flush Tx.
 			 * (Do the reset outside of interrupt context).
@@ -1898,15 +1900,15 @@ static void igbvf_watchdog_task(struct work_struct *work)
 			  round_jiffies(jiffies + (2 * HZ)));
 }
 
-#define IGBVF_TX_FLAGS_CSUM             0x00000001
-#define IGBVF_TX_FLAGS_VLAN             0x00000002
-#define IGBVF_TX_FLAGS_TSO              0x00000004
-#define IGBVF_TX_FLAGS_IPV4             0x00000008
-#define IGBVF_TX_FLAGS_VLAN_MASK        0xffff0000
-#define IGBVF_TX_FLAGS_VLAN_SHIFT       16
+#define IGBVF_TX_FLAGS_CSUM		0x00000001
+#define IGBVF_TX_FLAGS_VLAN		0x00000002
+#define IGBVF_TX_FLAGS_TSO		0x00000004
+#define IGBVF_TX_FLAGS_IPV4		0x00000008
+#define IGBVF_TX_FLAGS_VLAN_MASK	0xffff0000
+#define IGBVF_TX_FLAGS_VLAN_SHIFT	16
 
 static int igbvf_tso(struct igbvf_adapter *adapter,
-                     struct igbvf_ring *tx_ring,
+		     struct igbvf_ring *tx_ring,
 		     struct sk_buff *skb, u32 tx_flags, u8 *hdr_len,
 		     __be16 protocol)
 {
@@ -1930,17 +1932,18 @@ static int igbvf_tso(struct igbvf_adapter *adapter,
 
 	if (protocol == htons(ETH_P_IP)) {
 		struct iphdr *iph = ip_hdr(skb);
+
 		iph->tot_len = 0;
 		iph->check = 0;
 		tcp_hdr(skb)->check = ~csum_tcpudp_magic(iph->saddr,
-		                                         iph->daddr, 0,
-		                                         IPPROTO_TCP,
-		                                         0);
+							 iph->daddr, 0,
+							 IPPROTO_TCP,
+							 0);
 	} else if (skb_is_gso_v6(skb)) {
 		ipv6_hdr(skb)->payload_len = 0;
 		tcp_hdr(skb)->check = ~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
-		                                       &ipv6_hdr(skb)->daddr,
-		                                       0, IPPROTO_TCP, 0);
+						       &ipv6_hdr(skb)->daddr,
+						       0, IPPROTO_TCP, 0);
 	}
 
 	i = tx_ring->next_to_use;
@@ -1984,7 +1987,7 @@ static int igbvf_tso(struct igbvf_adapter *adapter,
 }
 
 static inline bool igbvf_tx_csum(struct igbvf_adapter *adapter,
-                                 struct igbvf_ring *tx_ring,
+				 struct igbvf_ring *tx_ring,
 				 struct sk_buff *skb, u32 tx_flags,
 				 __be16 protocol)
 {
@@ -2005,8 +2008,7 @@ static inline bool igbvf_tx_csum(struct igbvf_adapter *adapter,
 		info |= (skb_network_offset(skb) << E1000_ADVTXD_MACLEN_SHIFT);
 		if (skb->ip_summed == CHECKSUM_PARTIAL)
 			info |= (skb_transport_header(skb) -
-			         skb_network_header(skb));
-
+				 skb_network_header(skb));
 
 		context_desc->vlan_macip_lens = cpu_to_le32(info);
 
@@ -2055,6 +2057,10 @@ static int igbvf_maybe_stop_tx(struct net_device *netdev, int size)
 
 	netif_stop_queue(netdev);
 
+	/* Herbert's original patch had:
+	 *  smp_mb__after_netif_stop_queue();
+	 * but since that doesn't exist yet, just open code it.
+	 */
 	smp_mb();
 
 	/* We need to check again just in case room has been made available */
@@ -2067,11 +2073,11 @@ static int igbvf_maybe_stop_tx(struct net_device *netdev, int size)
 	return 0;
 }
 
-#define IGBVF_MAX_TXD_PWR       16
-#define IGBVF_MAX_DATA_PER_TXD  (1 << IGBVF_MAX_TXD_PWR)
+#define IGBVF_MAX_TXD_PWR	16
+#define IGBVF_MAX_DATA_PER_TXD	(1 << IGBVF_MAX_TXD_PWR)
 
 static inline int igbvf_tx_map_adv(struct igbvf_adapter *adapter,
-                                   struct igbvf_ring *tx_ring,
+				   struct igbvf_ring *tx_ring,
 				   struct sk_buff *skb)
 {
 	struct igbvf_buffer *buffer_info;
@@ -2093,7 +2099,6 @@ static inline int igbvf_tx_map_adv(struct igbvf_adapter *adapter,
 	if (dma_mapping_error(&pdev->dev, buffer_info->dma))
 		goto dma_error;
 
-
 	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++) {
 		const struct skb_frag_struct *frag;
 
@@ -2111,7 +2116,7 @@ static inline int igbvf_tx_map_adv(struct igbvf_adapter *adapter,
 		buffer_info->time_stamp = jiffies;
 		buffer_info->mapped_as_page = true;
 		buffer_info->dma = skb_frag_dma_map(&pdev->dev, frag, 0, len,
-						DMA_TO_DEVICE);
+						    DMA_TO_DEVICE);
 		if (dma_mapping_error(&pdev->dev, buffer_info->dma))
 			goto dma_error;
 	}
@@ -2133,7 +2138,7 @@ dma_error:
 
 	/* clear timestamp and dma mappings for remaining portion of packet */
 	while (count--) {
-		if (i==0)
+		if (i == 0)
 			i += tx_ring->count;
 		i--;
 		buffer_info = &tx_ring->buffer_info[i];
@@ -2144,10 +2149,10 @@ dma_error:
 }
 
 static inline void igbvf_tx_queue_adv(struct igbvf_adapter *adapter,
-                                      struct igbvf_ring *tx_ring,
+				      struct igbvf_ring *tx_ring,
 				      int tx_flags, int count,
 				      unsigned int first, u32 paylen,
-                                      u8 hdr_len)
+				      u8 hdr_len)
 {
 	union e1000_adv_tx_desc *tx_desc = NULL;
 	struct igbvf_buffer *buffer_info;
@@ -2155,7 +2160,7 @@ static inline void igbvf_tx_queue_adv(struct igbvf_adapter *adapter,
 	unsigned int i;
 
 	cmd_type_len = (E1000_ADVTXD_DTYP_DATA | E1000_ADVTXD_DCMD_IFCS |
-	                E1000_ADVTXD_DCMD_DEXT);
+			E1000_ADVTXD_DCMD_DEXT);
 
 	if (tx_flags & IGBVF_TX_FLAGS_VLAN)
 		cmd_type_len |= E1000_ADVTXD_DCMD_VLE;
@@ -2182,7 +2187,7 @@ static inline void igbvf_tx_queue_adv(struct igbvf_adapter *adapter,
 		tx_desc = IGBVF_TX_DESC_ADV(*tx_ring, i);
 		tx_desc->read.buffer_addr = cpu_to_le64(buffer_info->dma);
 		tx_desc->read.cmd_type_len =
-		         cpu_to_le32(cmd_type_len | buffer_info->length);
+			 cpu_to_le32(cmd_type_len | buffer_info->length);
 		tx_desc->read.olinfo_status = cpu_to_le32(olinfo_status);
 		i++;
 		if (i == tx_ring->count)
@@ -2193,14 +2198,16 @@ static inline void igbvf_tx_queue_adv(struct igbvf_adapter *adapter,
 	/* Force memory writes to complete before letting h/w
 	 * know there are new descriptors to fetch.  (Only
 	 * applicable for weak-ordered memory model archs,
-	 * such as IA-64). */
+	 * such as IA-64).
+	 */
 	wmb();
 
 	tx_ring->buffer_info[first].next_to_watch = tx_desc;
 	tx_ring->next_to_use = i;
 	writel(i, adapter->hw.hw_addr + tx_ring->tail);
 	/* we need this if more than one processor can write to our tail
-	 * at a time, it syncronizes IO on IA64/Altix systems */
+	 * at a time, it synchronizes IO on IA64/Altix systems
+	 */
 	mmiowb();
 }
 
@@ -2225,11 +2232,10 @@ static netdev_tx_t igbvf_xmit_frame_ring_adv(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 	}
 
-	/*
-	 * need: count + 4 desc gap to keep tail from touching
-         *       + 2 desc gap to keep tail from touching head,
-         *       + 1 desc for skb->data,
-         *       + 1 desc for context descriptor,
+	/* need: count + 4 desc gap to keep tail from touching
+	 *       + 2 desc gap to keep tail from touching head,
+	 *       + 1 desc for skb->data,
+	 *       + 1 desc for context descriptor,
 	 * head, otherwise try next time
 	 */
 	if (igbvf_maybe_stop_tx(netdev, skb_shinfo(skb)->nr_frags + 4)) {
@@ -2258,11 +2264,10 @@ static netdev_tx_t igbvf_xmit_frame_ring_adv(struct sk_buff *skb,
 	if (tso)
 		tx_flags |= IGBVF_TX_FLAGS_TSO;
 	else if (igbvf_tx_csum(adapter, tx_ring, skb, tx_flags, protocol) &&
-	         (skb->ip_summed == CHECKSUM_PARTIAL))
+		 (skb->ip_summed == CHECKSUM_PARTIAL))
 		tx_flags |= IGBVF_TX_FLAGS_CSUM;
 
-	/*
-	 * count reflects descriptors mapped, if 0 then mapping error
+	/* count reflects descriptors mapped, if 0 then mapping error
 	 * has occurred and we need to rewind the descriptor queue
 	 */
 	count = igbvf_tx_map_adv(adapter, tx_ring, skb);
@@ -2313,6 +2318,7 @@ static void igbvf_tx_timeout(struct net_device *netdev)
 static void igbvf_reset_task(struct work_struct *work)
 {
 	struct igbvf_adapter *adapter;
+
 	adapter = container_of(work, struct igbvf_adapter, reset_task);
 
 	igbvf_reinit_locked(adapter);
@@ -2356,14 +2362,13 @@ static int igbvf_change_mtu(struct net_device *netdev, int new_mtu)
 	}
 
 	while (test_and_set_bit(__IGBVF_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 	/* igbvf_down has a dependency on max_frame_size */
 	adapter->max_frame_size = max_frame;
 	if (netif_running(netdev))
 		igbvf_down(adapter);
 
-	/*
-	 * NOTE: netdev_alloc_skb reserves 16 bytes, and typically NET_IP_ALIGN
+	/* NOTE: netdev_alloc_skb reserves 16 bytes, and typically NET_IP_ALIGN
 	 * means we reserve 2 more, this pushes us to allocate from the next
 	 * larger slab size.
 	 * i.e. RXBUFFER_2048 --> size-4096 slab
@@ -2382,15 +2387,14 @@ static int igbvf_change_mtu(struct net_device *netdev, int new_mtu)
 		adapter->rx_buffer_len = PAGE_SIZE / 2;
 #endif
 
-
 	/* adjust allocation if LPE protects us, and we aren't using SBP */
 	if ((max_frame == ETH_FRAME_LEN + ETH_FCS_LEN) ||
-	     (max_frame == ETH_FRAME_LEN + VLAN_HLEN + ETH_FCS_LEN))
+	    (max_frame == ETH_FRAME_LEN + VLAN_HLEN + ETH_FCS_LEN))
 		adapter->rx_buffer_len = ETH_FRAME_LEN + VLAN_HLEN +
-		                         ETH_FCS_LEN;
+					 ETH_FCS_LEN;
 
 	dev_info(&adapter->pdev->dev, "changing MTU from %d to %d\n",
-	         netdev->mtu, new_mtu);
+		 netdev->mtu, new_mtu);
 	netdev->mtu = new_mtu;
 
 	if (netif_running(netdev))
@@ -2477,8 +2481,7 @@ static void igbvf_shutdown(struct pci_dev *pdev)
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-/*
- * Polling 'interrupt' - used by things like netconsole to send skbs
+/* Polling 'interrupt' - used by things like netconsole to send skbs
  * without having to re-enable interrupts. It's not called while
  * the interrupt routine is executing.
  */
@@ -2503,7 +2506,7 @@ static void igbvf_netpoll(struct net_device *netdev)
  * this device has been detected.
  */
 static pci_ers_result_t igbvf_io_error_detected(struct pci_dev *pdev,
-                                                pci_channel_state_t state)
+						pci_channel_state_t state)
 {
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
@@ -2583,7 +2586,7 @@ static void igbvf_print_device_info(struct igbvf_adapter *adapter)
 }
 
 static int igbvf_set_features(struct net_device *netdev,
-	netdev_features_t features)
+			      netdev_features_t features)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 
@@ -2596,21 +2599,21 @@ static int igbvf_set_features(struct net_device *netdev,
 }
 
 static const struct net_device_ops igbvf_netdev_ops = {
-	.ndo_open                       = igbvf_open,
-	.ndo_stop                       = igbvf_close,
-	.ndo_start_xmit                 = igbvf_xmit_frame,
-	.ndo_get_stats                  = igbvf_get_stats,
-	.ndo_set_rx_mode		= igbvf_set_multi,
-	.ndo_set_mac_address            = igbvf_set_mac,
-	.ndo_change_mtu                 = igbvf_change_mtu,
-	.ndo_do_ioctl                   = igbvf_ioctl,
-	.ndo_tx_timeout                 = igbvf_tx_timeout,
-	.ndo_vlan_rx_add_vid            = igbvf_vlan_rx_add_vid,
-	.ndo_vlan_rx_kill_vid           = igbvf_vlan_rx_kill_vid,
+	.ndo_open		= igbvf_open,
+	.ndo_stop		= igbvf_close,
+	.ndo_start_xmit		= igbvf_xmit_frame,
+	.ndo_get_stats		= igbvf_get_stats,
+	.ndo_set_rx_mode	= igbvf_set_multi,
+	.ndo_set_mac_address	= igbvf_set_mac,
+	.ndo_change_mtu		= igbvf_change_mtu,
+	.ndo_do_ioctl		= igbvf_ioctl,
+	.ndo_tx_timeout		= igbvf_tx_timeout,
+	.ndo_vlan_rx_add_vid	= igbvf_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= igbvf_vlan_rx_kill_vid,
 #ifdef CONFIG_NET_POLL_CONTROLLER
-	.ndo_poll_controller            = igbvf_netpoll,
+	.ndo_poll_controller	= igbvf_netpoll,
 #endif
-	.ndo_set_features               = igbvf_set_features,
+	.ndo_set_features	= igbvf_set_features,
 };
 
 /**
@@ -2645,8 +2648,8 @@ static int igbvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	} else {
 		err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
 		if (err) {
-			dev_err(&pdev->dev, "No usable DMA "
-			        "configuration, aborting\n");
+			dev_err(&pdev->dev,
+				"No usable DMA configuration, aborting\n");
 			goto err_dma;
 		}
 	}
@@ -2686,7 +2689,7 @@ static int igbvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	err = -EIO;
 	adapter->hw.hw_addr = ioremap(pci_resource_start(pdev, 0),
-	                              pci_resource_len(pdev, 0));
+				      pci_resource_len(pdev, 0));
 
 	if (!adapter->hw.hw_addr)
 		goto err_ioremap;
@@ -2712,16 +2715,16 @@ static int igbvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	adapter->bd_number = cards_found++;
 
 	netdev->hw_features = NETIF_F_SG |
-	                   NETIF_F_IP_CSUM |
+			   NETIF_F_IP_CSUM |
 			   NETIF_F_IPV6_CSUM |
 			   NETIF_F_TSO |
 			   NETIF_F_TSO6 |
 			   NETIF_F_RXCSUM;
 
 	netdev->features = netdev->hw_features |
-	                   NETIF_F_HW_VLAN_CTAG_TX |
-	                   NETIF_F_HW_VLAN_CTAG_RX |
-	                   NETIF_F_HW_VLAN_CTAG_FILTER;
+			   NETIF_F_HW_VLAN_CTAG_TX |
+			   NETIF_F_HW_VLAN_CTAG_RX |
+			   NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	if (pci_using_dac)
 		netdev->features |= NETIF_F_HIGHDMA;
@@ -2742,7 +2745,8 @@ static int igbvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		if (err)
 			dev_info(&pdev->dev, "Error reading MAC address.\n");
 		else if (is_zero_ether_addr(adapter->hw.mac.addr))
-			dev_info(&pdev->dev, "MAC address not assigned by administrator.\n");
+			dev_info(&pdev->dev,
+				 "MAC address not assigned by administrator.\n");
 		memcpy(netdev->dev_addr, adapter->hw.mac.addr,
 		       netdev->addr_len);
 	}
@@ -2751,11 +2755,11 @@ static int igbvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev_info(&pdev->dev, "Assigning random MAC address.\n");
 		eth_hw_addr_random(netdev);
 		memcpy(adapter->hw.mac.addr, netdev->dev_addr,
-			netdev->addr_len);
+		       netdev->addr_len);
 	}
 
 	setup_timer(&adapter->watchdog_timer, &igbvf_watchdog,
-	            (unsigned long) adapter);
+		    (unsigned long)adapter);
 
 	INIT_WORK(&adapter->reset_task, igbvf_reset_task);
 	INIT_WORK(&adapter->watchdog_task, igbvf_watchdog_task);
@@ -2818,8 +2822,7 @@ static void igbvf_remove(struct pci_dev *pdev)
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 
-	/*
-	 * The watchdog timer may be rescheduled, so explicitly
+	/* The watchdog timer may be rescheduled, so explicitly
 	 * disable it from being rescheduled.
 	 */
 	set_bit(__IGBVF_DOWN, &adapter->state);
@@ -2832,9 +2835,8 @@ static void igbvf_remove(struct pci_dev *pdev)
 
 	igbvf_reset_interrupt_capability(adapter);
 
-	/*
-	 * it is important to delete the napi struct prior to freeing the
-	 * rx ring so that you do not end up with null pointer refs
+	/* it is important to delete the NAPI struct prior to freeing the
+	 * Rx ring so that you do not end up with null pointer refs
 	 */
 	netif_napi_del(&adapter->rx_ring->napi);
 	kfree(adapter->tx_ring);
@@ -2866,17 +2868,17 @@ MODULE_DEVICE_TABLE(pci, igbvf_pci_tbl);
 
 /* PCI Device API Driver */
 static struct pci_driver igbvf_driver = {
-	.name     = igbvf_driver_name,
-	.id_table = igbvf_pci_tbl,
-	.probe    = igbvf_probe,
-	.remove   = igbvf_remove,
+	.name		= igbvf_driver_name,
+	.id_table	= igbvf_pci_tbl,
+	.probe		= igbvf_probe,
+	.remove		= igbvf_remove,
 #ifdef CONFIG_PM
 	/* Power Management Hooks */
-	.suspend  = igbvf_suspend,
-	.resume   = igbvf_resume,
+	.suspend	= igbvf_suspend,
+	.resume		= igbvf_resume,
 #endif
-	.shutdown = igbvf_shutdown,
-	.err_handler = &igbvf_err_handler
+	.shutdown	= igbvf_shutdown,
+	.err_handler	= &igbvf_err_handler
 };
 
 /**
@@ -2888,6 +2890,7 @@ static struct pci_driver igbvf_driver = {
 static int __init igbvf_init_module(void)
 {
 	int ret;
+
 	pr_info("%s - version %s\n", igbvf_driver_string, igbvf_driver_version);
 	pr_info("%s\n", igbvf_copyright);
 
@@ -2909,7 +2912,6 @@ static void __exit igbvf_exit_module(void)
 }
 module_exit(igbvf_exit_module);
 
-
 MODULE_AUTHOR("Intel Corporation, <e1000-devel@lists.sourceforge.net>");
 MODULE_DESCRIPTION("Intel(R) Gigabit Virtual Function Network Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/intel/igbvf/regs.h b/drivers/net/ethernet/intel/igbvf/regs.h
index 7dc6341715dc..86a7c120b574 100644
--- a/drivers/net/ethernet/intel/igbvf/regs.h
+++ b/drivers/net/ethernet/intel/igbvf/regs.h
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -28,81 +27,81 @@
 #ifndef _E1000_REGS_H_
 #define _E1000_REGS_H_
 
-#define E1000_CTRL      0x00000 /* Device Control - RW */
-#define E1000_STATUS    0x00008 /* Device Status - RO */
-#define E1000_ITR       0x000C4 /* Interrupt Throttling Rate - RW */
-#define E1000_EICR      0x01580 /* Ext. Interrupt Cause Read - R/clr */
-#define E1000_EITR(_n)  (0x01680 + (0x4 * (_n)))
-#define E1000_EICS      0x01520 /* Ext. Interrupt Cause Set - W0 */
-#define E1000_EIMS      0x01524 /* Ext. Interrupt Mask Set/Read - RW */
-#define E1000_EIMC      0x01528 /* Ext. Interrupt Mask Clear - WO */
-#define E1000_EIAC      0x0152C /* Ext. Interrupt Auto Clear - RW */
-#define E1000_EIAM      0x01530 /* Ext. Interrupt Ack Auto Clear Mask - RW */
-#define E1000_IVAR0     0x01700 /* Interrupt Vector Allocation (array) - RW */
-#define E1000_IVAR_MISC 0x01740 /* IVAR for "other" causes - RW */
-/*
- * Convenience macros
+#define E1000_CTRL	0x00000 /* Device Control - RW */
+#define E1000_STATUS	0x00008 /* Device Status - RO */
+#define E1000_ITR	0x000C4 /* Interrupt Throttling Rate - RW */
+#define E1000_EICR	0x01580 /* Ext. Interrupt Cause Read - R/clr */
+#define E1000_EITR(_n)	(0x01680 + (0x4 * (_n)))
+#define E1000_EICS	0x01520 /* Ext. Interrupt Cause Set - W0 */
+#define E1000_EIMS	0x01524 /* Ext. Interrupt Mask Set/Read - RW */
+#define E1000_EIMC	0x01528 /* Ext. Interrupt Mask Clear - WO */
+#define E1000_EIAC	0x0152C /* Ext. Interrupt Auto Clear - RW */
+#define E1000_EIAM	0x01530 /* Ext. Interrupt Ack Auto Clear Mask - RW */
+#define E1000_IVAR0	0x01700 /* Interrupt Vector Allocation (array) - RW */
+#define E1000_IVAR_MISC	0x01740 /* IVAR for "other" causes - RW */
+
+/* Convenience macros
  *
  * Note: "_n" is the queue number of the register to be written to.
  *
  * Example usage:
  * E1000_RDBAL_REG(current_rx_queue)
  */
-#define E1000_RDBAL(_n)      ((_n) < 4 ? (0x02800 + ((_n) * 0x100)) : \
-                                         (0x0C000 + ((_n) * 0x40)))
-#define E1000_RDBAH(_n)      ((_n) < 4 ? (0x02804 + ((_n) * 0x100)) : \
-                                         (0x0C004 + ((_n) * 0x40)))
-#define E1000_RDLEN(_n)      ((_n) < 4 ? (0x02808 + ((_n) * 0x100)) : \
-                                         (0x0C008 + ((_n) * 0x40)))
-#define E1000_SRRCTL(_n)     ((_n) < 4 ? (0x0280C + ((_n) * 0x100)) : \
-                                         (0x0C00C + ((_n) * 0x40)))
-#define E1000_RDH(_n)        ((_n) < 4 ? (0x02810 + ((_n) * 0x100)) : \
-                                         (0x0C010 + ((_n) * 0x40)))
-#define E1000_RDT(_n)        ((_n) < 4 ? (0x02818 + ((_n) * 0x100)) : \
-                                         (0x0C018 + ((_n) * 0x40)))
-#define E1000_RXDCTL(_n)     ((_n) < 4 ? (0x02828 + ((_n) * 0x100)) : \
-                                         (0x0C028 + ((_n) * 0x40)))
-#define E1000_TDBAL(_n)      ((_n) < 4 ? (0x03800 + ((_n) * 0x100)) : \
-                                         (0x0E000 + ((_n) * 0x40)))
-#define E1000_TDBAH(_n)      ((_n) < 4 ? (0x03804 + ((_n) * 0x100)) : \
-                                         (0x0E004 + ((_n) * 0x40)))
-#define E1000_TDLEN(_n)      ((_n) < 4 ? (0x03808 + ((_n) * 0x100)) : \
-                                         (0x0E008 + ((_n) * 0x40)))
-#define E1000_TDH(_n)        ((_n) < 4 ? (0x03810 + ((_n) * 0x100)) : \
-                                         (0x0E010 + ((_n) * 0x40)))
-#define E1000_TDT(_n)        ((_n) < 4 ? (0x03818 + ((_n) * 0x100)) : \
-                                         (0x0E018 + ((_n) * 0x40)))
-#define E1000_TXDCTL(_n)     ((_n) < 4 ? (0x03828 + ((_n) * 0x100)) : \
-                                         (0x0E028 + ((_n) * 0x40)))
-#define E1000_DCA_TXCTRL(_n) (0x03814 + (_n << 8))
-#define E1000_DCA_RXCTRL(_n) (0x02814 + (_n << 8))
-#define E1000_RAL(_i)  (((_i) <= 15) ? (0x05400 + ((_i) * 8)) : \
-                                       (0x054E0 + ((_i - 16) * 8)))
-#define E1000_RAH(_i)  (((_i) <= 15) ? (0x05404 + ((_i) * 8)) : \
-                                       (0x054E4 + ((_i - 16) * 8)))
+#define E1000_RDBAL(_n)	((_n) < 4 ? (0x02800 + ((_n) * 0x100)) : \
+			 (0x0C000 + ((_n) * 0x40)))
+#define E1000_RDBAH(_n)	((_n) < 4 ? (0x02804 + ((_n) * 0x100)) : \
+			 (0x0C004 + ((_n) * 0x40)))
+#define E1000_RDLEN(_n)	((_n) < 4 ? (0x02808 + ((_n) * 0x100)) : \
+			 (0x0C008 + ((_n) * 0x40)))
+#define E1000_SRRCTL(_n)	((_n) < 4 ? (0x0280C + ((_n) * 0x100)) : \
+				 (0x0C00C + ((_n) * 0x40)))
+#define E1000_RDH(_n)	((_n) < 4 ? (0x02810 + ((_n) * 0x100)) : \
+			 (0x0C010 + ((_n) * 0x40)))
+#define E1000_RDT(_n)	((_n) < 4 ? (0x02818 + ((_n) * 0x100)) : \
+			 (0x0C018 + ((_n) * 0x40)))
+#define E1000_RXDCTL(_n)	((_n) < 4 ? (0x02828 + ((_n) * 0x100)) : \
+				 (0x0C028 + ((_n) * 0x40)))
+#define E1000_TDBAL(_n)	((_n) < 4 ? (0x03800 + ((_n) * 0x100)) : \
+			 (0x0E000 + ((_n) * 0x40)))
+#define E1000_TDBAH(_n)	((_n) < 4 ? (0x03804 + ((_n) * 0x100)) : \
+			 (0x0E004 + ((_n) * 0x40)))
+#define E1000_TDLEN(_n)	((_n) < 4 ? (0x03808 + ((_n) * 0x100)) : \
+			 (0x0E008 + ((_n) * 0x40)))
+#define E1000_TDH(_n)	((_n) < 4 ? (0x03810 + ((_n) * 0x100)) : \
+			 (0x0E010 + ((_n) * 0x40)))
+#define E1000_TDT(_n)	((_n) < 4 ? (0x03818 + ((_n) * 0x100)) : \
+			 (0x0E018 + ((_n) * 0x40)))
+#define E1000_TXDCTL(_n)	((_n) < 4 ? (0x03828 + ((_n) * 0x100)) : \
+				 (0x0E028 + ((_n) * 0x40)))
+#define E1000_DCA_TXCTRL(_n)	(0x03814 + (_n << 8))
+#define E1000_DCA_RXCTRL(_n)	(0x02814 + (_n << 8))
+#define E1000_RAL(_i)	(((_i) <= 15) ? (0x05400 + ((_i) * 8)) : \
+			 (0x054E0 + ((_i - 16) * 8)))
+#define E1000_RAH(_i)	(((_i) <= 15) ? (0x05404 + ((_i) * 8)) : \
+			 (0x054E4 + ((_i - 16) * 8)))
 
 /* Statistics registers */
-#define E1000_VFGPRC    0x00F10
-#define E1000_VFGORC    0x00F18
-#define E1000_VFMPRC    0x00F3C
-#define E1000_VFGPTC    0x00F14
-#define E1000_VFGOTC    0x00F34
-#define E1000_VFGOTLBC  0x00F50
-#define E1000_VFGPTLBC  0x00F44
-#define E1000_VFGORLBC  0x00F48
-#define E1000_VFGPRLBC  0x00F40
+#define E1000_VFGPRC	0x00F10
+#define E1000_VFGORC	0x00F18
+#define E1000_VFMPRC	0x00F3C
+#define E1000_VFGPTC	0x00F14
+#define E1000_VFGOTC	0x00F34
+#define E1000_VFGOTLBC	0x00F50
+#define E1000_VFGPTLBC	0x00F44
+#define E1000_VFGORLBC	0x00F48
+#define E1000_VFGPRLBC	0x00F40
 
 /* These act per VF so an array friendly macro is used */
-#define E1000_V2PMAILBOX(_n)   (0x00C40 + (4 * (_n)))
-#define E1000_VMBMEM(_n)       (0x00800 + (64 * (_n)))
+#define E1000_V2PMAILBOX(_n)	(0x00C40 + (4 * (_n)))
+#define E1000_VMBMEM(_n)	(0x00800 + (64 * (_n)))
 
 /* Define macros for handling registers */
-#define er32(reg) readl(hw->hw_addr + E1000_##reg)
-#define ew32(reg, val) writel((val), hw->hw_addr +  E1000_##reg)
+#define er32(reg)	readl(hw->hw_addr + E1000_##reg)
+#define ew32(reg, val)	writel((val), hw->hw_addr +  E1000_##reg)
 #define array_er32(reg, offset) \
 	readl(hw->hw_addr + E1000_##reg + (offset << 2))
 #define array_ew32(reg, offset, val) \
 	writel((val), hw->hw_addr +  E1000_##reg + (offset << 2))
-#define e1e_flush() er32(STATUS)
+#define e1e_flush()	er32(STATUS)
 
 #endif
diff --git a/drivers/net/ethernet/intel/igbvf/vf.c b/drivers/net/ethernet/intel/igbvf/vf.c
index 955ad8c2c534..a13baa90ae20 100644
--- a/drivers/net/ethernet/intel/igbvf/vf.c
+++ b/drivers/net/ethernet/intel/igbvf/vf.c
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -25,17 +24,16 @@
 
 *******************************************************************************/
 
-
 #include "vf.h"
 
 static s32 e1000_check_for_link_vf(struct e1000_hw *hw);
 static s32 e1000_get_link_up_info_vf(struct e1000_hw *hw, u16 *speed,
-                                     u16 *duplex);
+				     u16 *duplex);
 static s32 e1000_init_hw_vf(struct e1000_hw *hw);
 static s32 e1000_reset_hw_vf(struct e1000_hw *hw);
 
 static void e1000_update_mc_addr_list_vf(struct e1000_hw *hw, u8 *,
-                                         u32, u32, u32);
+					 u32, u32, u32);
 static void e1000_rar_set_vf(struct e1000_hw *, u8 *, u32);
 static s32 e1000_read_mac_addr_vf(struct e1000_hw *);
 static s32 e1000_set_vfta_vf(struct e1000_hw *, u16, bool);
@@ -94,7 +92,7 @@ void e1000_init_function_pointers_vf(struct e1000_hw *hw)
  *  the status register's data which is often stale and inaccurate.
  **/
 static s32 e1000_get_link_up_info_vf(struct e1000_hw *hw, u16 *speed,
-                                     u16 *duplex)
+				     u16 *duplex)
 {
 	s32 status;
 
@@ -130,7 +128,7 @@ static s32 e1000_reset_hw_vf(struct e1000_hw *hw)
 	u8 *addr = (u8 *)(&msgbuf[1]);
 	u32 ctrl;
 
-	/* assert vf queue/interrupt reset */
+	/* assert VF queue/interrupt reset */
 	ctrl = er32(CTRL);
 	ew32(CTRL, ctrl | E1000_CTRL_RST);
 
@@ -144,7 +142,7 @@ static s32 e1000_reset_hw_vf(struct e1000_hw *hw)
 		/* mailbox timeout can now become active */
 		mbx->timeout = E1000_VF_MBX_INIT_TIMEOUT;
 
-		/* notify pf of vf reset completion */
+		/* notify PF of VF reset completion */
 		msgbuf[0] = E1000_VF_RESET;
 		mbx->ops.write_posted(hw, msgbuf, 1);
 
@@ -153,7 +151,8 @@ static s32 e1000_reset_hw_vf(struct e1000_hw *hw)
 		/* set our "perm_addr" based on info provided by PF */
 		ret_val = mbx->ops.read_posted(hw, msgbuf, 3);
 		if (!ret_val) {
-			if (msgbuf[0] == (E1000_VF_RESET | E1000_VT_MSGTYPE_ACK))
+			if (msgbuf[0] == (E1000_VF_RESET |
+					  E1000_VT_MSGTYPE_ACK))
 				memcpy(hw->mac.perm_addr, addr, ETH_ALEN);
 			else
 				ret_val = -E1000_ERR_MAC_INIT;
@@ -194,15 +193,14 @@ static u32 e1000_hash_mc_addr_vf(struct e1000_hw *hw, u8 *mc_addr)
 	/* Register count multiplied by bits per register */
 	hash_mask = (hw->mac.mta_reg_count * 32) - 1;
 
-	/*
-	 * The bit_shift is the number of left-shifts
+	/* The bit_shift is the number of left-shifts
 	 * where 0xFF would still fall within the hash mask.
 	 */
 	while (hash_mask >> bit_shift != 0xFF)
 		bit_shift++;
 
 	hash_value = hash_mask & (((mc_addr[4] >> (8 - bit_shift)) |
-	                          (((u16) mc_addr[5]) << bit_shift)));
+				  (((u16)mc_addr[5]) << bit_shift)));
 
 	return hash_value;
 }
@@ -221,8 +219,8 @@ static u32 e1000_hash_mc_addr_vf(struct e1000_hw *hw, u8 *mc_addr)
  *  unless there are workarounds that change this.
  **/
 static void e1000_update_mc_addr_list_vf(struct e1000_hw *hw,
-                                  u8 *mc_addr_list, u32 mc_addr_count,
-                                  u32 rar_used_count, u32 rar_count)
+					 u8 *mc_addr_list, u32 mc_addr_count,
+					 u32 rar_used_count, u32 rar_count)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	u32 msgbuf[E1000_VFMAILBOX_SIZE];
@@ -305,7 +303,7 @@ void e1000_rlpml_set_vf(struct e1000_hw *hw, u16 max_size)
  *  @addr: pointer to the receive address
  *  @index: receive address array register
  **/
-static void e1000_rar_set_vf(struct e1000_hw *hw, u8 * addr, u32 index)
+static void e1000_rar_set_vf(struct e1000_hw *hw, u8 *addr, u32 index)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	u32 msgbuf[3];
@@ -354,8 +352,7 @@ static s32 e1000_check_for_link_vf(struct e1000_hw *hw)
 	s32 ret_val = E1000_SUCCESS;
 	u32 in_msg = 0;
 
-	/*
-	 * We only want to run this if there has been a rst asserted.
+	/* We only want to run this if there has been a rst asserted.
 	 * in this case that could mean a link change, device reset,
 	 * or a virtual function reset
 	 */
@@ -367,31 +364,33 @@ static s32 e1000_check_for_link_vf(struct e1000_hw *hw)
 	if (!mac->get_link_status)
 		goto out;
 
-	/* if link status is down no point in checking to see if pf is up */
+	/* if link status is down no point in checking to see if PF is up */
 	if (!(er32(STATUS) & E1000_STATUS_LU))
 		goto out;
 
 	/* if the read failed it could just be a mailbox collision, best wait
-	 * until we are called again and don't report an error */
+	 * until we are called again and don't report an error
+	 */
 	if (mbx->ops.read(hw, &in_msg, 1))
 		goto out;
 
 	/* if incoming message isn't clear to send we are waiting on response */
 	if (!(in_msg & E1000_VT_MSGTYPE_CTS)) {
-		/* message is not CTS and is NACK we must have lost CTS status */
+		/* msg is not CTS and is NACK we must have lost CTS status */
 		if (in_msg & E1000_VT_MSGTYPE_NACK)
 			ret_val = -E1000_ERR_MAC_INIT;
 		goto out;
 	}
 
-	/* the pf is talking, if we timed out in the past we reinit */
+	/* the PF is talking, if we timed out in the past we reinit */
 	if (!mbx->timeout) {
 		ret_val = -E1000_ERR_MAC_INIT;
 		goto out;
 	}
 
 	/* if we passed all the tests above then the link is up and we no
-	 * longer need to check for link */
+	 * longer need to check for link
+	 */
 	mac->get_link_status = false;
 
 out:
diff --git a/drivers/net/ethernet/intel/igbvf/vf.h b/drivers/net/ethernet/intel/igbvf/vf.h
index 57db3c68dfcd..0f1eca639f68 100644
--- a/drivers/net/ethernet/intel/igbvf/vf.h
+++ b/drivers/net/ethernet/intel/igbvf/vf.h
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -38,30 +37,29 @@
 
 struct e1000_hw;
 
-#define E1000_DEV_ID_82576_VF                 0x10CA
-#define E1000_DEV_ID_I350_VF                  0x1520
-#define E1000_REVISION_0 0
-#define E1000_REVISION_1 1
-#define E1000_REVISION_2 2
-#define E1000_REVISION_3 3
-#define E1000_REVISION_4 4
+#define E1000_DEV_ID_82576_VF		0x10CA
+#define E1000_DEV_ID_I350_VF		0x1520
+#define E1000_REVISION_0	0
+#define E1000_REVISION_1	1
+#define E1000_REVISION_2	2
+#define E1000_REVISION_3	3
+#define E1000_REVISION_4	4
 
-#define E1000_FUNC_0     0
-#define E1000_FUNC_1     1
+#define E1000_FUNC_0	0
+#define E1000_FUNC_1	1
 
-/*
- * Receive Address Register Count
+/* Receive Address Register Count
  * Number of high/low register pairs in the RAR.  The RAR (Receive Address
  * Registers) holds the directed and multicast addresses that we monitor.
  * These entries are also used for MAC-based filtering.
  */
-#define E1000_RAR_ENTRIES_VF      1
+#define E1000_RAR_ENTRIES_VF	1
 
 /* Receive Descriptor - Advanced */
 union e1000_adv_rx_desc {
 	struct {
-		u64 pkt_addr;             /* Packet buffer address */
-		u64 hdr_addr;             /* Header buffer address */
+		u64 pkt_addr; /* Packet buffer address */
+		u64 hdr_addr; /* Header buffer address */
 	} read;
 	struct {
 		struct {
@@ -69,53 +67,53 @@ union e1000_adv_rx_desc {
 				u32 data;
 				struct {
 					u16 pkt_info; /* RSS/Packet type */
-					u16 hdr_info; /* Split Header,
-					               * hdr buffer length */
+					/* Split Header, hdr buffer length */
+					u16 hdr_info;
 				} hs_rss;
 			} lo_dword;
 			union {
-				u32 rss;          /* RSS Hash */
+				u32 rss; /* RSS Hash */
 				struct {
-					u16 ip_id;    /* IP id */
-					u16 csum;     /* Packet Checksum */
+					u16 ip_id; /* IP id */
+					u16 csum;  /* Packet Checksum */
 				} csum_ip;
 			} hi_dword;
 		} lower;
 		struct {
-			u32 status_error;     /* ext status/error */
-			u16 length;           /* Packet length */
-			u16 vlan;             /* VLAN tag */
+			u32 status_error; /* ext status/error */
+			u16 length; /* Packet length */
+			u16 vlan;   /* VLAN tag */
 		} upper;
 	} wb;  /* writeback */
 };
 
-#define E1000_RXDADV_HDRBUFLEN_MASK      0x7FE0
-#define E1000_RXDADV_HDRBUFLEN_SHIFT     5
+#define E1000_RXDADV_HDRBUFLEN_MASK	0x7FE0
+#define E1000_RXDADV_HDRBUFLEN_SHIFT	5
 
 /* Transmit Descriptor - Advanced */
 union e1000_adv_tx_desc {
 	struct {
-		u64 buffer_addr;    /* Address of descriptor's data buf */
+		u64 buffer_addr; /* Address of descriptor's data buf */
 		u32 cmd_type_len;
 		u32 olinfo_status;
 	} read;
 	struct {
-		u64 rsvd;       /* Reserved */
+		u64 rsvd; /* Reserved */
 		u32 nxtseq_seed;
 		u32 status;
 	} wb;
 };
 
 /* Adv Transmit Descriptor Config Masks */
-#define E1000_ADVTXD_DTYP_CTXT    0x00200000 /* Advanced Context Descriptor */
-#define E1000_ADVTXD_DTYP_DATA    0x00300000 /* Advanced Data Descriptor */
-#define E1000_ADVTXD_DCMD_EOP     0x01000000 /* End of Packet */
-#define E1000_ADVTXD_DCMD_IFCS    0x02000000 /* Insert FCS (Ethernet CRC) */
-#define E1000_ADVTXD_DCMD_RS      0x08000000 /* Report Status */
-#define E1000_ADVTXD_DCMD_DEXT    0x20000000 /* Descriptor extension (1=Adv) */
-#define E1000_ADVTXD_DCMD_VLE     0x40000000 /* VLAN pkt enable */
-#define E1000_ADVTXD_DCMD_TSE     0x80000000 /* TCP Seg enable */
-#define E1000_ADVTXD_PAYLEN_SHIFT    14 /* Adv desc PAYLEN shift */
+#define E1000_ADVTXD_DTYP_CTXT	0x00200000 /* Advanced Context Descriptor */
+#define E1000_ADVTXD_DTYP_DATA	0x00300000 /* Advanced Data Descriptor */
+#define E1000_ADVTXD_DCMD_EOP	0x01000000 /* End of Packet */
+#define E1000_ADVTXD_DCMD_IFCS	0x02000000 /* Insert FCS (Ethernet CRC) */
+#define E1000_ADVTXD_DCMD_RS	0x08000000 /* Report Status */
+#define E1000_ADVTXD_DCMD_DEXT	0x20000000 /* Descriptor extension (1=Adv) */
+#define E1000_ADVTXD_DCMD_VLE	0x40000000 /* VLAN pkt enable */
+#define E1000_ADVTXD_DCMD_TSE	0x80000000 /* TCP Seg enable */
+#define E1000_ADVTXD_PAYLEN_SHIFT	14 /* Adv desc PAYLEN shift */
 
 /* Context descriptors */
 struct e1000_adv_tx_context_desc {
@@ -125,11 +123,11 @@ struct e1000_adv_tx_context_desc {
 	u32 mss_l4len_idx;
 };
 
-#define E1000_ADVTXD_MACLEN_SHIFT    9  /* Adv ctxt desc mac len shift */
-#define E1000_ADVTXD_TUCMD_IPV4    0x00000400  /* IP Packet Type: 1=IPv4 */
-#define E1000_ADVTXD_TUCMD_L4T_TCP 0x00000800  /* L4 Packet TYPE of TCP */
-#define E1000_ADVTXD_L4LEN_SHIFT     8  /* Adv ctxt L4LEN shift */
-#define E1000_ADVTXD_MSS_SHIFT      16  /* Adv ctxt MSS shift */
+#define E1000_ADVTXD_MACLEN_SHIFT	9  /* Adv ctxt desc mac len shift */
+#define E1000_ADVTXD_TUCMD_IPV4		0x00000400 /* IP Packet Type: 1=IPv4 */
+#define E1000_ADVTXD_TUCMD_L4T_TCP	0x00000800 /* L4 Packet TYPE of TCP */
+#define E1000_ADVTXD_L4LEN_SHIFT	8  /* Adv ctxt L4LEN shift */
+#define E1000_ADVTXD_MSS_SHIFT		16 /* Adv ctxt MSS shift */
 
 enum e1000_mac_type {
 	e1000_undefined = 0,
@@ -262,5 +260,4 @@ struct e1000_hw {
 void e1000_rlpml_set_vf(struct e1000_hw *, u16);
 void e1000_init_function_pointers_vf(struct e1000_hw *hw);
 
-
 #endif /* _E1000_VF_H_ */
diff --git a/drivers/net/ethernet/intel/ixgb/ixgb_main.c b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
index 11a1bdbe3fd9..31f91459312f 100644
--- a/drivers/net/ethernet/intel/ixgb/ixgb_main.c
+++ b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
@@ -285,6 +285,8 @@ ixgb_down(struct ixgb_adapter *adapter, bool kill_watchdog)
 	/* prevent the interrupt handler from restarting watchdog */
 	set_bit(__IXGB_DOWN, &adapter->flags);
 
+	netif_carrier_off(netdev);
+
 	napi_disable(&adapter->napi);
 	/* waiting for NAPI to complete can re-enable interrupts */
 	ixgb_irq_disable(adapter);
@@ -298,7 +300,6 @@ ixgb_down(struct ixgb_adapter *adapter, bool kill_watchdog)
 
 	adapter->link_speed = 0;
 	adapter->link_duplex = 0;
-	netif_carrier_off(netdev);
 	netif_stop_queue(netdev);
 
 	ixgb_reset(adapter);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 7dcbbec09a70..636f9e350162 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -151,6 +151,7 @@ struct vf_data_storage {
 	u16 tx_rate;
 	u16 vlan_count;
 	u8 spoofchk_enabled;
+	bool rss_query_enabled;
 	unsigned int vf_api;
 };
 
@@ -613,7 +614,6 @@ struct ixgbe_adapter {
 #define IXGBE_FLAG_RX_1BUF_CAPABLE              (u32)(1 << 4)
 #define IXGBE_FLAG_RX_PS_CAPABLE                (u32)(1 << 5)
 #define IXGBE_FLAG_RX_PS_ENABLED                (u32)(1 << 6)
-#define IXGBE_FLAG_IN_NETPOLL                   (u32)(1 << 7)
 #define IXGBE_FLAG_DCA_ENABLED                  (u32)(1 << 8)
 #define IXGBE_FLAG_DCA_CAPABLE                  (u32)(1 << 9)
 #define IXGBE_FLAG_IMIR_ENABLED                 (u32)(1 << 10)
@@ -643,7 +643,6 @@ struct ixgbe_adapter {
 #define IXGBE_FLAG2_RSS_FIELD_IPV4_UDP		(u32)(1 << 8)
 #define IXGBE_FLAG2_RSS_FIELD_IPV6_UDP		(u32)(1 << 9)
 #define IXGBE_FLAG2_PTP_PPS_ENABLED		(u32)(1 << 10)
-#define IXGBE_FLAG2_BRIDGE_MODE_VEB		(u32)(1 << 11)
 
 	/* Tx fast path data */
 	int num_tx_queues;
@@ -723,6 +722,8 @@ struct ixgbe_adapter {
 	u8 __iomem *io_addr; /* Mainly for iounmap use */
 	u32 wol;
 
+	u16 bridge_mode;
+
 	u16 eeprom_verh;
 	u16 eeprom_verl;
 	u16 eeprom_cap;
@@ -766,6 +767,15 @@ struct ixgbe_adapter {
 
 	u8 default_up;
 	unsigned long fwd_bitmask; /* Bitmask indicating in use pools */
+
+/* maximum number of RETA entries among all devices supported by ixgbe
+ * driver: currently it's x550 device in non-SRIOV mode
+ */
+#define IXGBE_MAX_RETA_ENTRIES 512
+	u8 rss_indir_tbl[IXGBE_MAX_RETA_ENTRIES];
+
+#define IXGBE_RSS_KEY_SIZE     40  /* size of RSS Hash Key in bytes */
+	u32 rss_key[IXGBE_RSS_KEY_SIZE / sizeof(u32)];
 };
 
 static inline u8 ixgbe_max_rss_indices(struct ixgbe_adapter *adapter)
@@ -955,4 +965,5 @@ void ixgbe_sriov_reinit(struct ixgbe_adapter *adapter);
 netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 				  struct ixgbe_adapter *adapter,
 				  struct ixgbe_ring *tx_ring);
+u32 ixgbe_rss_indir_tbl_entries(struct ixgbe_adapter *adapter);
 #endif /* _IXGBE_H_ */
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
index c5c97b483d7c..824a7ab79ab6 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
@@ -171,17 +171,21 @@ static s32 ixgbe_init_phy_ops_82598(struct ixgbe_hw *hw)
  *  @hw: pointer to hardware structure
  *
  *  Starts the hardware using the generic start_hw function.
- *  Disables relaxed ordering Then set pcie completion timeout
+ *  Disables relaxed ordering for archs other than SPARC
+ *  Then set pcie completion timeout
  *
  **/
 static s32 ixgbe_start_hw_82598(struct ixgbe_hw *hw)
 {
+#ifndef CONFIG_SPARC
 	u32 regval;
 	u32 i;
+#endif
 	s32 ret_val;
 
 	ret_val = ixgbe_start_hw_generic(hw);
 
+#ifndef CONFIG_SPARC
 	/* Disable relaxed ordering */
 	for (i = 0; ((i < hw->mac.max_tx_queues) &&
 	     (i < IXGBE_DCA_MAX_QUEUES_82598)); i++) {
@@ -197,7 +201,7 @@ static s32 ixgbe_start_hw_82598(struct ixgbe_hw *hw)
 			    IXGBE_DCA_RXCTRL_HEAD_WRO_EN);
 		IXGBE_WRITE_REG(hw, IXGBE_DCA_RXCTRL(i), regval);
 	}
-
+#endif
 	if (ret_val)
 		return ret_val;
 
@@ -1193,6 +1197,8 @@ static struct ixgbe_mac_operations mac_ops_82598 = {
 	.init_thermal_sensor_thresh = NULL,
 	.prot_autoc_read	= &prot_autoc_read_generic,
 	.prot_autoc_write	= &prot_autoc_write_generic,
+	.enable_rx		= &ixgbe_enable_rx_generic,
+	.disable_rx		= &ixgbe_disable_rx_generic,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_82598 = {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
index cf55a0df877b..e0c363948bf4 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
@@ -1977,7 +1977,10 @@ static s32 ixgbe_enable_rx_dma_82599(struct ixgbe_hw *hw, u32 regval)
 	 */
 	hw->mac.ops.disable_rx_buff(hw);
 
-	IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, regval);
+	if (regval & IXGBE_RXCTRL_RXEN)
+		hw->mac.ops.enable_rx(hw);
+	else
+		hw->mac.ops.disable_rx(hw);
 
 	hw->mac.ops.enable_rx_buff(hw);
 
@@ -2336,6 +2339,8 @@ static struct ixgbe_mac_operations mac_ops_82599 = {
 	.init_thermal_sensor_thresh = &ixgbe_init_thermal_sensor_thresh_generic,
 	.prot_autoc_read	= &prot_autoc_read_82599,
 	.prot_autoc_write	= &prot_autoc_write_82599,
+	.enable_rx		= &ixgbe_enable_rx_generic,
+	.disable_rx		= &ixgbe_disable_rx_generic,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_82599 = {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index 9c66babd4edd..06d8f3cfa099 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -312,7 +312,6 @@ s32 ixgbe_start_hw_generic(struct ixgbe_hw *hw)
 s32 ixgbe_start_hw_gen2(struct ixgbe_hw *hw)
 {
 	u32 i;
-	u32 regval;
 
 	/* Clear the rate limiters */
 	for (i = 0; i < hw->mac.max_tx_queues; i++) {
@@ -321,20 +320,25 @@ s32 ixgbe_start_hw_gen2(struct ixgbe_hw *hw)
 	}
 	IXGBE_WRITE_FLUSH(hw);
 
+#ifndef CONFIG_SPARC
 	/* Disable relaxed ordering */
 	for (i = 0; i < hw->mac.max_tx_queues; i++) {
+		u32 regval;
+
 		regval = IXGBE_READ_REG(hw, IXGBE_DCA_TXCTRL_82599(i));
 		regval &= ~IXGBE_DCA_TXCTRL_DESC_WRO_EN;
 		IXGBE_WRITE_REG(hw, IXGBE_DCA_TXCTRL_82599(i), regval);
 	}
 
 	for (i = 0; i < hw->mac.max_rx_queues; i++) {
+		u32 regval;
+
 		regval = IXGBE_READ_REG(hw, IXGBE_DCA_RXCTRL(i));
 		regval &= ~(IXGBE_DCA_RXCTRL_DATA_WRO_EN |
 			    IXGBE_DCA_RXCTRL_HEAD_WRO_EN);
 		IXGBE_WRITE_REG(hw, IXGBE_DCA_RXCTRL(i), regval);
 	}
-
+#endif
 	return 0;
 }
 
@@ -703,7 +707,7 @@ s32 ixgbe_stop_adapter_generic(struct ixgbe_hw *hw)
 	hw->adapter_stopped = true;
 
 	/* Disable the receive unit */
-	IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, 0);
+	hw->mac.ops.disable_rx(hw);
 
 	/* Clear interrupt mask to stop interrupts from being generated */
 	IXGBE_WRITE_REG(hw, IXGBE_EIMC, IXGBE_IRQ_CLEAR_MASK);
@@ -2639,7 +2643,10 @@ s32 ixgbe_enable_rx_buff_generic(struct ixgbe_hw *hw)
  **/
 s32 ixgbe_enable_rx_dma_generic(struct ixgbe_hw *hw, u32 regval)
 {
-	IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, regval);
+	if (regval & IXGBE_RXCTRL_RXEN)
+		hw->mac.ops.enable_rx(hw);
+	else
+		hw->mac.ops.disable_rx(hw);
 
 	return 0;
 }
@@ -3850,3 +3857,44 @@ s32 ixgbe_init_thermal_sensor_thresh_generic(struct ixgbe_hw *hw)
 	return 0;
 }
 
+void ixgbe_disable_rx_generic(struct ixgbe_hw *hw)
+{
+	u32 rxctrl;
+
+	rxctrl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
+	if (rxctrl & IXGBE_RXCTRL_RXEN) {
+		if (hw->mac.type != ixgbe_mac_82598EB) {
+			u32 pfdtxgswc;
+
+			pfdtxgswc = IXGBE_READ_REG(hw, IXGBE_PFDTXGSWC);
+			if (pfdtxgswc & IXGBE_PFDTXGSWC_VT_LBEN) {
+				pfdtxgswc &= ~IXGBE_PFDTXGSWC_VT_LBEN;
+				IXGBE_WRITE_REG(hw, IXGBE_PFDTXGSWC, pfdtxgswc);
+				hw->mac.set_lben = true;
+			} else {
+				hw->mac.set_lben = false;
+			}
+		}
+		rxctrl &= ~IXGBE_RXCTRL_RXEN;
+		IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, rxctrl);
+	}
+}
+
+void ixgbe_enable_rx_generic(struct ixgbe_hw *hw)
+{
+	u32 rxctrl;
+
+	rxctrl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
+	IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, (rxctrl | IXGBE_RXCTRL_RXEN));
+
+	if (hw->mac.type != ixgbe_mac_82598EB) {
+		if (hw->mac.set_lben) {
+			u32 pfdtxgswc;
+
+			pfdtxgswc = IXGBE_READ_REG(hw, IXGBE_PFDTXGSWC);
+			pfdtxgswc |= IXGBE_PFDTXGSWC_VT_LBEN;
+			IXGBE_WRITE_REG(hw, IXGBE_PFDTXGSWC, pfdtxgswc);
+			hw->mac.set_lben = false;
+		}
+	}
+}
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
index 8cfadcb2676e..f21f8a165ec4 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
@@ -130,6 +130,8 @@ void ixgbe_set_rxpba_generic(struct ixgbe_hw *hw, int num_pb,
 
 s32 ixgbe_get_thermal_sensor_data_generic(struct ixgbe_hw *hw);
 s32 ixgbe_init_thermal_sensor_thresh_generic(struct ixgbe_hw *hw);
+void ixgbe_disable_rx_generic(struct ixgbe_hw *hw);
+void ixgbe_enable_rx_generic(struct ixgbe_hw *hw);
 
 #define IXGBE_FAILED_READ_REG 0xffffffffU
 #define IXGBE_FAILED_READ_CFG_DWORD 0xffffffffU
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index e5be0dd508de..eafa9ec802ba 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -1351,7 +1351,7 @@ static bool reg_pattern_test(struct ixgbe_adapter *adapter, u64 *data, int reg,
 
 	if (ixgbe_removed(adapter->hw.hw_addr)) {
 		*data = 1;
-		return 1;
+		return true;
 	}
 	for (pat = 0; pat < ARRAY_SIZE(test_pattern); pat++) {
 		before = ixgbe_read_reg(&adapter->hw, reg);
@@ -1376,7 +1376,7 @@ static bool reg_set_and_check(struct ixgbe_adapter *adapter, u64 *data, int reg,
 
 	if (ixgbe_removed(adapter->hw.hw_addr)) {
 		*data = 1;
-		return 1;
+		return true;
 	}
 	before = ixgbe_read_reg(&adapter->hw, reg);
 	ixgbe_write_reg(&adapter->hw, reg, write & mask);
@@ -1637,9 +1637,7 @@ static void ixgbe_free_desc_rings(struct ixgbe_adapter *adapter)
 	/* shut down the DMA engines now so they can be reinitialized later */
 
 	/* first Rx */
-	reg_ctl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
-	reg_ctl &= ~IXGBE_RXCTRL_RXEN;
-	IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, reg_ctl);
+	hw->mac.ops.disable_rx(hw);
 	ixgbe_disable_rx_queue(adapter, rx_ring);
 
 	/* now Tx */
@@ -1670,6 +1668,7 @@ static int ixgbe_setup_desc_rings(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_ring *tx_ring = &adapter->test_tx_ring;
 	struct ixgbe_ring *rx_ring = &adapter->test_rx_ring;
+	struct ixgbe_hw *hw = &adapter->hw;
 	u32 rctl, reg_data;
 	int ret_val;
 	int err;
@@ -1713,14 +1712,16 @@ static int ixgbe_setup_desc_rings(struct ixgbe_adapter *adapter)
 		goto err_nomem;
 	}
 
-	rctl = IXGBE_READ_REG(&adapter->hw, IXGBE_RXCTRL);
-	IXGBE_WRITE_REG(&adapter->hw, IXGBE_RXCTRL, rctl & ~IXGBE_RXCTRL_RXEN);
+	hw->mac.ops.disable_rx(hw);
 
 	ixgbe_configure_rx_ring(adapter, rx_ring);
 
-	rctl |= IXGBE_RXCTRL_RXEN | IXGBE_RXCTRL_DMBYPS;
+	rctl = IXGBE_READ_REG(&adapter->hw, IXGBE_RXCTRL);
+	rctl |= IXGBE_RXCTRL_DMBYPS;
 	IXGBE_WRITE_REG(&adapter->hw, IXGBE_RXCTRL, rctl);
 
+	hw->mac.ops.enable_rx(hw);
+
 	return 0;
 
 err_nomem:
@@ -2852,6 +2853,45 @@ static int ixgbe_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
 	return ret;
 }
 
+static u32 ixgbe_get_rxfh_key_size(struct net_device *netdev)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+
+	return sizeof(adapter->rss_key);
+}
+
+static u32 ixgbe_rss_indir_size(struct net_device *netdev)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+
+	return ixgbe_rss_indir_tbl_entries(adapter);
+}
+
+static void ixgbe_get_reta(struct ixgbe_adapter *adapter, u32 *indir)
+{
+	int i, reta_size = ixgbe_rss_indir_tbl_entries(adapter);
+
+	for (i = 0; i < reta_size; i++)
+		indir[i] = adapter->rss_indir_tbl[i];
+}
+
+static int ixgbe_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
+			  u8 *hfunc)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+
+	if (indir)
+		ixgbe_get_reta(adapter, indir);
+
+	if (key)
+		memcpy(key, adapter->rss_key, ixgbe_get_rxfh_key_size(netdev));
+
+	return 0;
+}
+
 static int ixgbe_get_ts_info(struct net_device *dev,
 			     struct ethtool_ts_info *info)
 {
@@ -3109,6 +3149,9 @@ static const struct ethtool_ops ixgbe_ethtool_ops = {
 	.set_coalesce           = ixgbe_set_coalesce,
 	.get_rxnfc		= ixgbe_get_rxnfc,
 	.set_rxnfc		= ixgbe_set_rxnfc,
+	.get_rxfh_indir_size	= ixgbe_rss_indir_size,
+	.get_rxfh_key_size	= ixgbe_get_rxfh_key_size,
+	.get_rxfh		= ixgbe_get_rxfh,
 	.get_channels		= ixgbe_get_channels,
 	.set_channels		= ixgbe_set_channels,
 	.get_ts_info		= ixgbe_get_ts_info,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
index 2ad91cb04dab..631c603fc966 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
@@ -71,6 +71,7 @@ int ixgbe_fcoe_ddp_put(struct net_device *netdev, u16 xid)
 	struct ixgbe_fcoe *fcoe;
 	struct ixgbe_adapter *adapter;
 	struct ixgbe_fcoe_ddp *ddp;
+	struct ixgbe_hw *hw;
 	u32 fcbuff;
 
 	if (!netdev)
@@ -85,25 +86,51 @@ int ixgbe_fcoe_ddp_put(struct net_device *netdev, u16 xid)
 	if (!ddp->udl)
 		return 0;
 
+	hw = &adapter->hw;
 	len = ddp->len;
-	/* if there an error, force to invalidate ddp context */
-	if (ddp->err) {
+	/* if no error then skip ddp context invalidation */
+	if (!ddp->err)
+		goto skip_ddpinv;
+
+	if (hw->mac.type == ixgbe_mac_X550) {
+		/* X550 does not require DDP FCoE lock */
+
+		IXGBE_WRITE_REG(hw, IXGBE_FCDFC(0, xid), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_FCDFC(3, xid),
+				(xid | IXGBE_FCFLTRW_WE));
+
+		/* program FCBUFF */
+		IXGBE_WRITE_REG(hw, IXGBE_FCDDC(2, xid), 0);
+
+		/* program FCDMARW */
+		IXGBE_WRITE_REG(hw, IXGBE_FCDDC(3, xid),
+				(xid | IXGBE_FCDMARW_WE));
+
+		/* read FCBUFF to check context invalidated */
+		IXGBE_WRITE_REG(hw, IXGBE_FCDDC(3, xid),
+				(xid | IXGBE_FCDMARW_RE));
+		fcbuff = IXGBE_READ_REG(hw, IXGBE_FCDDC(2, xid));
+	} else {
+		/* other hardware requires DDP FCoE lock */
 		spin_lock_bh(&fcoe->lock);
-		IXGBE_WRITE_REG(&adapter->hw, IXGBE_FCFLT, 0);
-		IXGBE_WRITE_REG(&adapter->hw, IXGBE_FCFLTRW,
+		IXGBE_WRITE_REG(hw, IXGBE_FCFLT, 0);
+		IXGBE_WRITE_REG(hw, IXGBE_FCFLTRW,
 				(xid | IXGBE_FCFLTRW_WE));
-		IXGBE_WRITE_REG(&adapter->hw, IXGBE_FCBUFF, 0);
-		IXGBE_WRITE_REG(&adapter->hw, IXGBE_FCDMARW,
+		IXGBE_WRITE_REG(hw, IXGBE_FCBUFF, 0);
+		IXGBE_WRITE_REG(hw, IXGBE_FCDMARW,
 				(xid | IXGBE_FCDMARW_WE));
 
 		/* guaranteed to be invalidated after 100us */
-		IXGBE_WRITE_REG(&adapter->hw, IXGBE_FCDMARW,
+		IXGBE_WRITE_REG(hw, IXGBE_FCDMARW,
 				(xid | IXGBE_FCDMARW_RE));
-		fcbuff = IXGBE_READ_REG(&adapter->hw, IXGBE_FCBUFF);
+		fcbuff = IXGBE_READ_REG(hw, IXGBE_FCBUFF);
 		spin_unlock_bh(&fcoe->lock);
-		if (fcbuff & IXGBE_FCBUFF_VALID)
-			udelay(100);
-	}
+		}
+
+	if (fcbuff & IXGBE_FCBUFF_VALID)
+		usleep_range(100, 150);
+
+skip_ddpinv:
 	if (ddp->sgl)
 		dma_unmap_sg(&adapter->pdev->dev, ddp->sgl, ddp->sgc,
 			     DMA_FROM_DEVICE);
@@ -272,7 +299,6 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 
 	/* program DMA context */
 	hw = &adapter->hw;
-	spin_lock_bh(&fcoe->lock);
 
 	/* turn on last frame indication for target mode as FCP_RSPtarget is
 	 * supposed to send FCP_RSP when it is done. */
@@ -283,16 +309,33 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 		IXGBE_WRITE_REG(hw, IXGBE_FCRXCTRL, fcrxctl);
 	}
 
-	IXGBE_WRITE_REG(hw, IXGBE_FCPTRL, ddp->udp & DMA_BIT_MASK(32));
-	IXGBE_WRITE_REG(hw, IXGBE_FCPTRH, (u64)ddp->udp >> 32);
-	IXGBE_WRITE_REG(hw, IXGBE_FCBUFF, fcbuff);
-	IXGBE_WRITE_REG(hw, IXGBE_FCDMARW, fcdmarw);
-	/* program filter context */
-	IXGBE_WRITE_REG(hw, IXGBE_FCPARAM, 0);
-	IXGBE_WRITE_REG(hw, IXGBE_FCFLT, IXGBE_FCFLT_VALID);
-	IXGBE_WRITE_REG(hw, IXGBE_FCFLTRW, fcfltrw);
+	if (hw->mac.type == ixgbe_mac_X550) {
+		/* X550 does not require DDP lock */
+
+		IXGBE_WRITE_REG(hw, IXGBE_FCDDC(0, xid),
+				ddp->udp & DMA_BIT_MASK(32));
+		IXGBE_WRITE_REG(hw, IXGBE_FCDDC(1, xid), (u64)ddp->udp >> 32);
+		IXGBE_WRITE_REG(hw, IXGBE_FCDDC(2, xid), fcbuff);
+		IXGBE_WRITE_REG(hw, IXGBE_FCDDC(3, xid), fcdmarw);
+		/* program filter context */
+		IXGBE_WRITE_REG(hw, IXGBE_FCDFC(0, xid), IXGBE_FCFLT_VALID);
+		IXGBE_WRITE_REG(hw, IXGBE_FCDFC(1, xid), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_FCDFC(3, xid), fcfltrw);
+	} else {
+		/* DDP lock for indirect DDP context access */
+		spin_lock_bh(&fcoe->lock);
+
+		IXGBE_WRITE_REG(hw, IXGBE_FCPTRL, ddp->udp & DMA_BIT_MASK(32));
+		IXGBE_WRITE_REG(hw, IXGBE_FCPTRH, (u64)ddp->udp >> 32);
+		IXGBE_WRITE_REG(hw, IXGBE_FCBUFF, fcbuff);
+		IXGBE_WRITE_REG(hw, IXGBE_FCDMARW, fcdmarw);
+		/* program filter context */
+		IXGBE_WRITE_REG(hw, IXGBE_FCPARAM, 0);
+		IXGBE_WRITE_REG(hw, IXGBE_FCFLT, IXGBE_FCFLT_VALID);
+		IXGBE_WRITE_REG(hw, IXGBE_FCFLTRW, fcfltrw);
 
-	spin_unlock_bh(&fcoe->lock);
+		spin_unlock_bh(&fcoe->lock);
+	}
 
 	return 1;
 
@@ -371,6 +414,7 @@ int ixgbe_fcoe_ddp(struct ixgbe_adapter *adapter,
 	struct fcoe_crc_eof *crc;
 	__le32 fcerr = ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_ERR_FCERR);
 	__le32 ddp_err;
+	int ddp_max;
 	u32 fctl;
 	u16 xid;
 
@@ -392,7 +436,11 @@ int ixgbe_fcoe_ddp(struct ixgbe_adapter *adapter,
 	else
 		xid =  be16_to_cpu(fh->fh_rx_id);
 
-	if (xid >= IXGBE_FCOE_DDP_MAX)
+	ddp_max = IXGBE_FCOE_DDP_MAX;
+	/* X550 has different DDP Max limit */
+	if (adapter->hw.mac.type == ixgbe_mac_X550)
+		ddp_max = IXGBE_FCOE_DDP_MAX_X550;
+	if (xid >= ddp_max)
 		return -EINVAL;
 
 	fcoe = &adapter->fcoe;
@@ -612,7 +660,8 @@ void ixgbe_configure_fcoe(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_ring_feature *fcoe = &adapter->ring_feature[RING_F_FCOE];
 	struct ixgbe_hw *hw = &adapter->hw;
-	int i, fcoe_q, fcoe_i;
+	int i, fcoe_q, fcoe_i, fcoe_q_h = 0;
+	int fcreta_size;
 	u32 etqf;
 
 	/* Minimal functionality for FCoE requires at least CRC offloads */
@@ -633,10 +682,23 @@ void ixgbe_configure_fcoe(struct ixgbe_adapter *adapter)
 		return;
 
 	/* Use one or more Rx queues for FCoE by redirection table */
-	for (i = 0; i < IXGBE_FCRETA_SIZE; i++) {
+	fcreta_size = IXGBE_FCRETA_SIZE;
+	if (adapter->hw.mac.type == ixgbe_mac_X550)
+		fcreta_size = IXGBE_FCRETA_SIZE_X550;
+
+	for (i = 0; i < fcreta_size; i++) {
+		if (adapter->hw.mac.type == ixgbe_mac_X550) {
+			int fcoe_i_h = fcoe->offset + ((i + fcreta_size) %
+							fcoe->indices);
+			fcoe_q_h = adapter->rx_ring[fcoe_i_h]->reg_idx;
+			fcoe_q_h = (fcoe_q_h << IXGBE_FCRETA_ENTRY_HIGH_SHIFT) &
+				   IXGBE_FCRETA_ENTRY_HIGH_MASK;
+		}
+
 		fcoe_i = fcoe->offset + (i % fcoe->indices);
 		fcoe_i &= IXGBE_FCRETA_ENTRY_MASK;
 		fcoe_q = adapter->rx_ring[fcoe_i]->reg_idx;
+		fcoe_q |= fcoe_q_h;
 		IXGBE_WRITE_REG(hw, IXGBE_FCRETA(i), fcoe_q);
 	}
 	IXGBE_WRITE_REG(hw, IXGBE_FCRECTL, IXGBE_FCRECTL_ENA);
@@ -672,13 +734,18 @@ void ixgbe_configure_fcoe(struct ixgbe_adapter *adapter)
 void ixgbe_free_fcoe_ddp_resources(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
-	int cpu, i;
+	int cpu, i, ddp_max;
 
 	/* do nothing if no DDP pools were allocated */
 	if (!fcoe->ddp_pool)
 		return;
 
-	for (i = 0; i < IXGBE_FCOE_DDP_MAX; i++)
+	ddp_max = IXGBE_FCOE_DDP_MAX;
+	/* X550 has different DDP Max limit */
+	if (adapter->hw.mac.type == ixgbe_mac_X550)
+		ddp_max = IXGBE_FCOE_DDP_MAX_X550;
+
+	for (i = 0; i < ddp_max; i++)
 		ixgbe_fcoe_ddp_put(adapter->netdev, i);
 
 	for_each_possible_cpu(cpu)
@@ -758,6 +825,9 @@ static int ixgbe_fcoe_ddp_enable(struct ixgbe_adapter *adapter)
 	}
 
 	adapter->netdev->fcoe_ddp_xid = IXGBE_FCOE_DDP_MAX - 1;
+	/* X550 has different DDP Max limit */
+	if (adapter->hw.mac.type == ixgbe_mac_X550)
+		adapter->netdev->fcoe_ddp_xid = IXGBE_FCOE_DDP_MAX_X550 - 1;
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h
index 0772b7730fce..38385876effb 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h
@@ -46,6 +46,7 @@
 #define IXGBE_FCBUFF_MAX	65536	/* 64KB max */
 #define IXGBE_FCBUFF_MIN	4096	/* 4KB min */
 #define IXGBE_FCOE_DDP_MAX	512	/* 9 bits xid */
+#define IXGBE_FCOE_DDP_MAX_X550	2048	/* 11 bits xid */
 
 /* Default traffic class to use for FCoE */
 #define IXGBE_FCOE_DEFTC	3
@@ -77,7 +78,7 @@ struct ixgbe_fcoe {
 	struct ixgbe_fcoe_ddp_pool __percpu *ddp_pool;
 	atomic_t refcnt;
 	spinlock_t lock;
-	struct ixgbe_fcoe_ddp ddp[IXGBE_FCOE_DDP_MAX];
+	struct ixgbe_fcoe_ddp ddp[IXGBE_FCOE_DDP_MAX_X550];
 	void *extra_ddp_buffer;
 	dma_addr_t extra_ddp_buffer_dma;
 	unsigned long mode;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 70cc4c5c0a01..d3f4b0ceb3f7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -1619,14 +1619,10 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 static void ixgbe_rx_skb(struct ixgbe_q_vector *q_vector,
 			 struct sk_buff *skb)
 {
-	struct ixgbe_adapter *adapter = q_vector->adapter;
-
 	if (ixgbe_qv_busy_polling(q_vector))
 		netif_receive_skb(skb);
-	else if (!(adapter->flags & IXGBE_FLAG_IN_NETPOLL))
-		napi_gro_receive(&q_vector->napi, skb);
 	else
-		netif_rx(skb);
+		napi_gro_receive(&q_vector->napi, skb);
 }
 
 /**
@@ -2609,7 +2605,7 @@ static irqreturn_t ixgbe_msix_other(int irq, void *data)
 	eicr = IXGBE_READ_REG(hw, IXGBE_EICS);
 
 	/* The lower 16bits of the EICR register are for the queue interrupts
-	 * which should be masked here in order to not accidently clear them if
+	 * which should be masked here in order to not accidentally clear them if
 	 * the bits are high when ixgbe_msix_other is called. There is a race
 	 * condition otherwise which results in possible performance loss
 	 * especially if the ixgbe_msix_other interrupt is triggering
@@ -3232,89 +3228,148 @@ static void ixgbe_configure_srrctl(struct ixgbe_adapter *adapter,
 	IXGBE_WRITE_REG(hw, IXGBE_SRRCTL(reg_idx), srrctl);
 }
 
-static void ixgbe_setup_reta(struct ixgbe_adapter *adapter, const u32 *seed)
+/**
+ * Return a number of entries in the RSS indirection table
+ *
+ * @adapter: device handle
+ *
+ *  - 82598/82599/X540:     128
+ *  - X550(non-SRIOV mode): 512
+ *  - X550(SRIOV mode):     64
+ */
+u32 ixgbe_rss_indir_tbl_entries(struct ixgbe_adapter *adapter)
+{
+	if (adapter->hw.mac.type < ixgbe_mac_X550)
+		return 128;
+	else if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
+		return 64;
+	else
+		return 512;
+}
+
+/**
+ * Write the RETA table to HW
+ *
+ * @adapter: device handle
+ *
+ * Write the RSS redirection table stored in adapter.rss_indir_tbl[] to HW.
+ */
+static void ixgbe_store_reta(struct ixgbe_adapter *adapter)
 {
+	u32 i, reta_entries = ixgbe_rss_indir_tbl_entries(adapter);
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 reta = 0;
-	int i, j;
-	int reta_entries = 128;
-	u16 rss_i = adapter->ring_feature[RING_F_RSS].indices;
-	int indices_multi;
-
-	/*
-	 * Program table for at least 2 queues w/ SR-IOV so that VFs can
-	 * make full use of any rings they may have.  We will use the
-	 * PSRTYPE register to control how many rings we use within the PF.
-	 */
-	if ((adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) && (rss_i < 2))
-		rss_i = 2;
-
-	/* Fill out hash function seeds */
-	for (i = 0; i < 10; i++)
-		IXGBE_WRITE_REG(hw, IXGBE_RSSRK(i), seed[i]);
+	u32 indices_multi;
+	u8 *indir_tbl = adapter->rss_indir_tbl;
 
 	/* Fill out the redirection table as follows:
-	 * 82598: 128 (8 bit wide) entries containing pair of 4 bit RSS indices
-	 * 82599/X540: 128 (8 bit wide) entries containing 4 bit RSS index
-	 * X550: 512 (8 bit wide) entries containing 6 bit RSS index
+	 *  - 82598:      8 bit wide entries containing pair of 4 bit RSS
+	 *    indices.
+	 *  - 82599/X540: 8 bit wide entries containing 4 bit RSS index
+	 *  - X550:       8 bit wide entries containing 6 bit RSS index
 	 */
 	if (adapter->hw.mac.type == ixgbe_mac_82598EB)
 		indices_multi = 0x11;
 	else
 		indices_multi = 0x1;
 
-	switch (adapter->hw.mac.type) {
-	case ixgbe_mac_X550:
-	case ixgbe_mac_X550EM_x:
-		if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
-			reta_entries = 512;
-	default:
-		break;
-	}
-
-	/* Fill out redirection table */
-	for (i = 0, j = 0; i < reta_entries; i++, j++) {
-		if (j == rss_i)
-			j = 0;
-		reta = (reta << 8) | (j * indices_multi);
+	/* Write redirection table to HW */
+	for (i = 0; i < reta_entries; i++) {
+		reta |= indices_multi * indir_tbl[i] << (i & 0x3) * 8;
 		if ((i & 3) == 3) {
 			if (i < 128)
 				IXGBE_WRITE_REG(hw, IXGBE_RETA(i >> 2), reta);
 			else
 				IXGBE_WRITE_REG(hw, IXGBE_ERETA((i >> 2) - 32),
 						reta);
+			reta = 0;
 		}
 	}
 }
 
-static void ixgbe_setup_vfreta(struct ixgbe_adapter *adapter, const u32 *seed)
+/**
+ * Write the RETA table to HW (for x550 devices in SRIOV mode)
+ *
+ * @adapter: device handle
+ *
+ * Write the RSS redirection table stored in adapter.rss_indir_tbl[] to HW.
+ */
+static void ixgbe_store_vfreta(struct ixgbe_adapter *adapter)
 {
+	u32 i, reta_entries = ixgbe_rss_indir_tbl_entries(adapter);
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 vfreta = 0;
+	unsigned int pf_pool = adapter->num_vfs;
+
+	/* Write redirection table to HW */
+	for (i = 0; i < reta_entries; i++) {
+		vfreta |= (u32)adapter->rss_indir_tbl[i] << (i & 0x3) * 8;
+		if ((i & 3) == 3) {
+			IXGBE_WRITE_REG(hw, IXGBE_PFVFRETA(i >> 2, pf_pool),
+					vfreta);
+			vfreta = 0;
+		}
+	}
+}
+
+static void ixgbe_setup_reta(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 i, j;
+	u32 reta_entries = ixgbe_rss_indir_tbl_entries(adapter);
+	u16 rss_i = adapter->ring_feature[RING_F_RSS].indices;
+
+	/* Program table for at least 2 queues w/ SR-IOV so that VFs can
+	 * make full use of any rings they may have.  We will use the
+	 * PSRTYPE register to control how many rings we use within the PF.
+	 */
+	if ((adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) && (rss_i < 2))
+		rss_i = 2;
+
+	/* Fill out hash function seeds */
+	for (i = 0; i < 10; i++)
+		IXGBE_WRITE_REG(hw, IXGBE_RSSRK(i), adapter->rss_key[i]);
+
+	/* Fill out redirection table */
+	memset(adapter->rss_indir_tbl, 0, sizeof(adapter->rss_indir_tbl));
+
+	for (i = 0, j = 0; i < reta_entries; i++, j++) {
+		if (j == rss_i)
+			j = 0;
+
+		adapter->rss_indir_tbl[i] = j;
+	}
+
+	ixgbe_store_reta(adapter);
+}
+
+static void ixgbe_setup_vfreta(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
 	u16 rss_i = adapter->ring_feature[RING_F_RSS].indices;
 	unsigned int pf_pool = adapter->num_vfs;
 	int i, j;
 
 	/* Fill out hash function seeds */
 	for (i = 0; i < 10; i++)
-		IXGBE_WRITE_REG(hw, IXGBE_PFVFRSSRK(i, pf_pool), seed[i]);
+		IXGBE_WRITE_REG(hw, IXGBE_PFVFRSSRK(i, pf_pool),
+				adapter->rss_key[i]);
 
 	/* Fill out the redirection table */
 	for (i = 0, j = 0; i < 64; i++, j++) {
 		if (j == rss_i)
 			j = 0;
-		vfreta = (vfreta << 8) | j;
-		if ((i & 3) == 3)
-			IXGBE_WRITE_REG(hw, IXGBE_PFVFRETA(i >> 2, pf_pool),
-					vfreta);
+
+		adapter->rss_indir_tbl[i] = j;
 	}
+
+	ixgbe_store_vfreta(adapter);
 }
 
 static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 mrqc = 0, rss_field = 0, vfmrqc = 0;
-	u32 rss_key[10];
 	u32 rxcsum;
 
 	/* Disable indicating checksum in descriptor, enables RSS hash */
@@ -3358,7 +3413,7 @@ static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 	if (adapter->flags2 & IXGBE_FLAG2_RSS_FIELD_IPV6_UDP)
 		rss_field |= IXGBE_MRQC_RSS_FIELD_IPV6_UDP;
 
-	netdev_rss_key_fill(rss_key, sizeof(rss_key));
+	netdev_rss_key_fill(adapter->rss_key, sizeof(adapter->rss_key));
 	if ((hw->mac.type >= ixgbe_mac_X550) &&
 	    (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)) {
 		unsigned int pf_pool = adapter->num_vfs;
@@ -3368,12 +3423,12 @@ static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 		IXGBE_WRITE_REG(hw, IXGBE_MRQC, mrqc);
 
 		/* Setup RSS through the VF registers */
-		ixgbe_setup_vfreta(adapter, rss_key);
+		ixgbe_setup_vfreta(adapter);
 		vfmrqc = IXGBE_MRQC_RSSEN;
 		vfmrqc |= rss_field;
 		IXGBE_WRITE_REG(hw, IXGBE_PFVFMRQC(pf_pool), vfmrqc);
 	} else {
-		ixgbe_setup_reta(adapter, rss_key);
+		ixgbe_setup_reta(adapter);
 		mrqc |= rss_field;
 		IXGBE_WRITE_REG(hw, IXGBE_MRQC, mrqc);
 	}
@@ -3557,7 +3612,7 @@ static void ixgbe_configure_virtualization(struct ixgbe_adapter *adapter)
 	IXGBE_WRITE_REG(hw, IXGBE_VFRE(reg_offset ^ 1), reg_offset - 1);
 	IXGBE_WRITE_REG(hw, IXGBE_VFTE(reg_offset), (~0) << vf_shift);
 	IXGBE_WRITE_REG(hw, IXGBE_VFTE(reg_offset ^ 1), reg_offset - 1);
-	if (adapter->flags2 & IXGBE_FLAG2_BRIDGE_MODE_VEB)
+	if (adapter->bridge_mode == BRIDGE_MODE_VEB)
 		IXGBE_WRITE_REG(hw, IXGBE_PFDTXGSWC, IXGBE_PFDTXGSWC_VT_LBEN);
 
 	/* Map PF MAC address in RAR Entry 0 to first pool following VFs */
@@ -3603,6 +3658,10 @@ static void ixgbe_configure_virtualization(struct ixgbe_adapter *adapter)
 		/* enable ethertype anti spoofing if hw supports it */
 		if (hw->mac.ops.set_ethertype_anti_spoofing)
 			hw->mac.ops.set_ethertype_anti_spoofing(hw, true, i);
+
+		/* Enable/Disable RSS query feature  */
+		ixgbe_ndo_set_vf_rss_query_en(adapter->netdev, i,
+					  adapter->vfinfo[i].rss_query_enabled);
 	}
 }
 
@@ -3705,8 +3764,7 @@ static void ixgbe_configure_rx(struct ixgbe_adapter *adapter)
 	u32 rxctrl, rfctl;
 
 	/* disable receives while setting up the descriptors */
-	rxctrl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
-	IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, rxctrl & ~IXGBE_RXCTRL_RXEN);
+	hw->mac.ops.disable_rx(hw);
 
 	ixgbe_setup_psrtype(adapter);
 	ixgbe_setup_rdrxctl(adapter);
@@ -3731,6 +3789,7 @@ static void ixgbe_configure_rx(struct ixgbe_adapter *adapter)
 	for (i = 0; i < adapter->num_rx_queues; i++)
 		ixgbe_configure_rx_ring(adapter, adapter->rx_ring[i]);
 
+	rxctrl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
 	/* disable drop enable for 82598 parts */
 	if (hw->mac.type == ixgbe_mac_82598EB)
 		rxctrl |= IXGBE_RXCTRL_DMBYPS;
@@ -3924,7 +3983,7 @@ static void ixgbe_flush_sw_mac_table(struct ixgbe_adapter *adapter)
 	for (i = 0; i < hw->mac.num_rar_entries; i++) {
 		adapter->mac_table[i].state |= IXGBE_MAC_STATE_MODIFIED;
 		adapter->mac_table[i].state &= ~IXGBE_MAC_STATE_IN_USE;
-		memset(adapter->mac_table[i].addr, 0, ETH_ALEN);
+		eth_zero_addr(adapter->mac_table[i].addr);
 		adapter->mac_table[i].queue = 0;
 	}
 	ixgbe_sync_mac_table(adapter);
@@ -3992,7 +4051,7 @@ int ixgbe_del_mac_filter(struct ixgbe_adapter *adapter, u8 *addr, u16 queue)
 		    adapter->mac_table[i].queue == queue) {
 			adapter->mac_table[i].state |= IXGBE_MAC_STATE_MODIFIED;
 			adapter->mac_table[i].state &= ~IXGBE_MAC_STATE_IN_USE;
-			memset(adapter->mac_table[i].addr, 0, ETH_ALEN);
+			eth_zero_addr(adapter->mac_table[i].addr);
 			adapter->mac_table[i].queue = 0;
 			ixgbe_sync_mac_table(adapter);
 			return 0;
@@ -5014,7 +5073,6 @@ void ixgbe_down(struct ixgbe_adapter *adapter)
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct net_device *upper;
 	struct list_head *iter;
-	u32 rxctrl;
 	int i;
 
 	/* signal that we are down to the interrupt handler */
@@ -5022,8 +5080,7 @@ void ixgbe_down(struct ixgbe_adapter *adapter)
 		return; /* do nothing if already down */
 
 	/* disable receives */
-	rxctrl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
-	IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, rxctrl & ~IXGBE_RXCTRL_RXEN);
+	hw->mac.ops.disable_rx(hw);
 
 	/* disable all enabled rx queues */
 	for (i = 0; i < adapter->num_rx_queues; i++)
@@ -6174,7 +6231,6 @@ static void ixgbe_check_hang_subtask(struct ixgbe_adapter *adapter)
 
 	/* Cause software interrupt to ensure rings are cleaned */
 	ixgbe_irq_rearm_queues(adapter, eics);
-
 }
 
 /**
@@ -7507,14 +7563,9 @@ static void ixgbe_netpoll(struct net_device *netdev)
 	if (test_bit(__IXGBE_DOWN, &adapter->state))
 		return;
 
-	adapter->flags |= IXGBE_FLAG_IN_NETPOLL;
-	if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED) {
-		for (i = 0; i < adapter->num_q_vectors; i++)
-			ixgbe_msix_clean_rings(0, adapter->q_vector[i]);
-	} else {
-		ixgbe_intr(adapter->pdev->irq, netdev);
-	}
-	adapter->flags &= ~IXGBE_FLAG_IN_NETPOLL;
+	/* loop through and schedule all active queues */
+	for (i = 0; i < adapter->num_q_vectors; i++)
+		ixgbe_msix_clean_rings(0, adapter->q_vector[i]);
 }
 
 #endif
@@ -7882,6 +7933,80 @@ static int ixgbe_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	return ndo_dflt_fdb_add(ndm, tb, dev, addr, vid, flags);
 }
 
+/**
+ * ixgbe_configure_bridge_mode - set various bridge modes
+ * @adapter - the private structure
+ * @mode - requested bridge mode
+ *
+ * Configure some settings require for various bridge modes.
+ **/
+static int ixgbe_configure_bridge_mode(struct ixgbe_adapter *adapter,
+				       __u16 mode)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	unsigned int p, num_pools;
+	u32 vmdctl;
+
+	switch (mode) {
+	case BRIDGE_MODE_VEPA:
+		/* disable Tx loopback, rely on switch hairpin mode */
+		IXGBE_WRITE_REG(&adapter->hw, IXGBE_PFDTXGSWC, 0);
+
+		/* must enable Rx switching replication to allow multicast
+		 * packet reception on all VFs, and to enable source address
+		 * pruning.
+		 */
+		vmdctl = IXGBE_READ_REG(hw, IXGBE_VMD_CTL);
+		vmdctl |= IXGBE_VT_CTL_REPLEN;
+		IXGBE_WRITE_REG(hw, IXGBE_VMD_CTL, vmdctl);
+
+		/* enable Rx source address pruning. Note, this requires
+		 * replication to be enabled or else it does nothing.
+		 */
+		num_pools = adapter->num_vfs + adapter->num_rx_pools;
+		for (p = 0; p < num_pools; p++) {
+			if (hw->mac.ops.set_source_address_pruning)
+				hw->mac.ops.set_source_address_pruning(hw,
+								       true,
+								       p);
+		}
+		break;
+	case BRIDGE_MODE_VEB:
+		/* enable Tx loopback for internal VF/PF communication */
+		IXGBE_WRITE_REG(&adapter->hw, IXGBE_PFDTXGSWC,
+				IXGBE_PFDTXGSWC_VT_LBEN);
+
+		/* disable Rx switching replication unless we have SR-IOV
+		 * virtual functions
+		 */
+		vmdctl = IXGBE_READ_REG(hw, IXGBE_VMD_CTL);
+		if (!adapter->num_vfs)
+			vmdctl &= ~IXGBE_VT_CTL_REPLEN;
+		IXGBE_WRITE_REG(hw, IXGBE_VMD_CTL, vmdctl);
+
+		/* disable Rx source address pruning, since we don't expect to
+		 * be receiving external loopback of our transmitted frames.
+		 */
+		num_pools = adapter->num_vfs + adapter->num_rx_pools;
+		for (p = 0; p < num_pools; p++) {
+			if (hw->mac.ops.set_source_address_pruning)
+				hw->mac.ops.set_source_address_pruning(hw,
+								       false,
+								       p);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	adapter->bridge_mode = mode;
+
+	e_info(drv, "enabling bridge mode: %s\n",
+	       mode == BRIDGE_MODE_VEPA ? "VEPA" : "VEB");
+
+	return 0;
+}
+
 static int ixgbe_ndo_bridge_setlink(struct net_device *dev,
 				    struct nlmsghdr *nlh, u16 flags)
 {
@@ -7897,8 +8022,8 @@ static int ixgbe_ndo_bridge_setlink(struct net_device *dev,
 		return -EINVAL;
 
 	nla_for_each_nested(attr, br_spec, rem) {
+		u32 status;
 		__u16 mode;
-		u32 reg = 0;
 
 		if (nla_type(attr) != IFLA_BRIDGE_MODE)
 			continue;
@@ -7907,19 +8032,11 @@ static int ixgbe_ndo_bridge_setlink(struct net_device *dev,
 			return -EINVAL;
 
 		mode = nla_get_u16(attr);
-		if (mode == BRIDGE_MODE_VEPA) {
-			reg = 0;
-			adapter->flags2 &= ~IXGBE_FLAG2_BRIDGE_MODE_VEB;
-		} else if (mode == BRIDGE_MODE_VEB) {
-			reg = IXGBE_PFDTXGSWC_VT_LBEN;
-			adapter->flags2 |= IXGBE_FLAG2_BRIDGE_MODE_VEB;
-		} else
-			return -EINVAL;
+		status = ixgbe_configure_bridge_mode(adapter, mode);
+		if (status)
+			return status;
 
-		IXGBE_WRITE_REG(&adapter->hw, IXGBE_PFDTXGSWC, reg);
-
-		e_info(drv, "enabling bridge mode: %s\n",
-			mode == BRIDGE_MODE_VEPA ? "VEPA" : "VEB");
+		break;
 	}
 
 	return 0;
@@ -7930,17 +8047,12 @@ static int ixgbe_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 				    u32 filter_mask)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
-	u16 mode;
 
 	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
 		return 0;
 
-	if (adapter->flags2 & IXGBE_FLAG2_BRIDGE_MODE_VEB)
-		mode = BRIDGE_MODE_VEB;
-	else
-		mode = BRIDGE_MODE_VEPA;
-
-	return ndo_dflt_bridge_getlink(skb, pid, seq, dev, mode, 0, 0);
+	return ndo_dflt_bridge_getlink(skb, pid, seq, dev,
+				       adapter->bridge_mode, 0, 0);
 }
 
 static void *ixgbe_fwd_add(struct net_device *pdev, struct net_device *vdev)
@@ -8052,6 +8164,7 @@ static const struct net_device_ops ixgbe_netdev_ops = {
 	.ndo_set_vf_vlan	= ixgbe_ndo_set_vf_vlan,
 	.ndo_set_vf_rate	= ixgbe_ndo_set_vf_bw,
 	.ndo_set_vf_spoofchk	= ixgbe_ndo_set_vf_spoofchk,
+	.ndo_set_vf_rss_query_en = ixgbe_ndo_set_vf_rss_query_en,
 	.ndo_get_vf_config	= ixgbe_ndo_get_vf_config,
 	.ndo_get_stats64	= ixgbe_get_stats64,
 #ifdef CONFIG_IXGBE_DCB
@@ -8406,7 +8519,6 @@ skip_sriov:
 			   NETIF_F_IPV6_CSUM |
 			   NETIF_F_HW_VLAN_CTAG_TX |
 			   NETIF_F_HW_VLAN_CTAG_RX |
-			   NETIF_F_HW_VLAN_CTAG_FILTER |
 			   NETIF_F_TSO |
 			   NETIF_F_TSO6 |
 			   NETIF_F_RXHASH |
@@ -8428,6 +8540,7 @@ skip_sriov:
 	}
 
 	netdev->hw_features |= NETIF_F_RXALL;
+	netdev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	netdev->vlan_features |= NETIF_F_TSO;
 	netdev->vlan_features |= NETIF_F_TSO6;
@@ -8989,8 +9102,6 @@ static void __exit ixgbe_exit_module(void)
 	pci_unregister_driver(&ixgbe_driver);
 
 	ixgbe_dbg_exit();
-
-	rcu_barrier(); /* Wait for completion of call_rcu()'s */
 }
 
 #ifdef CONFIG_IXGBE_DCA
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_mbx.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_mbx.h
index a5cb755de3a9..b1e4703ff2a5 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_mbx.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_mbx.h
@@ -73,6 +73,7 @@ enum ixgbe_pfvf_api_rev {
 	ixgbe_mbox_api_10,	/* API version 1.0, linux/freebsd VF driver */
 	ixgbe_mbox_api_20,	/* API version 2.0, solaris Phase1 VF driver */
 	ixgbe_mbox_api_11,	/* API version 1.1, linux/freebsd VF driver */
+	ixgbe_mbox_api_12,	/* API version 1.2, linux/freebsd VF driver */
 	/* This value should always be last */
 	ixgbe_mbox_api_unknown,	/* indicates that API version is not known */
 };
@@ -97,6 +98,10 @@ enum ixgbe_pfvf_api_rev {
 #define IXGBE_VF_TRANS_VLAN	3	/* Indication of port vlan */
 #define IXGBE_VF_DEF_QUEUE	4	/* Default queue offset */
 
+/* mailbox API, version 1.2 VF requests */
+#define IXGBE_VF_GET_RETA	0x0a	/* VF request for RETA */
+#define IXGBE_VF_GET_RSS_KEY	0x0b	/* get RSS key */
+
 /* length of permanent address message returned from PF */
 #define IXGBE_VF_PERMADDR_MSG_LEN 4
 /* word in permanent address message with the current multicast type */
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
index 79c00f57d3e7..e5ba04025e2b 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -279,20 +279,18 @@ static int ixgbe_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
  * read the timecounter and return the correct value on ns,
  * after converting it into a struct timespec.
  */
-static int ixgbe_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int ixgbe_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct ixgbe_adapter *adapter =
 		container_of(ptp, struct ixgbe_adapter, ptp_caps);
 	u64 ns;
-	u32 remainder;
 	unsigned long flags;
 
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
 	ns = timecounter_read(&adapter->tc);
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
@@ -306,15 +304,14 @@ static int ixgbe_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
  * wall timer value.
  */
 static int ixgbe_ptp_settime(struct ptp_clock_info *ptp,
-			     const struct timespec *ts)
+			     const struct timespec64 *ts)
 {
 	struct ixgbe_adapter *adapter =
 		container_of(ptp, struct ixgbe_adapter, ptp_caps);
 	u64 ns;
 	unsigned long flags;
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 
 	/* reset the timecounter */
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
@@ -407,7 +404,7 @@ void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter)
 {
 	bool timeout = time_is_before_jiffies(adapter->last_overflow_check +
 					     IXGBE_OVERFLOW_PERIOD);
-	struct timespec ts;
+	struct timespec64 ts;
 
 	if (timeout) {
 		ixgbe_ptp_gettime(&adapter->ptp_caps, &ts);
@@ -488,7 +485,7 @@ static void ixgbe_ptp_tx_hwtstamp(struct ixgbe_adapter *adapter)
  * @work: pointer to the work struct
  *
  * This work item polls TSYNCTXCTL valid bit to determine when a Tx hardware
- * timestamp has been taken for the current skb. It is necesary, because the
+ * timestamp has been taken for the current skb. It is necessary, because the
  * descriptor's "done" bit does not correlate with the timestamp event.
  */
 static void ixgbe_ptp_tx_hwtstamp_work(struct work_struct *work)
@@ -874,8 +871,8 @@ static int ixgbe_ptp_create_clock(struct ixgbe_adapter *adapter)
 		adapter->ptp_caps.pps = 1;
 		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq;
 		adapter->ptp_caps.adjtime = ixgbe_ptp_adjtime;
-		adapter->ptp_caps.gettime = ixgbe_ptp_gettime;
-		adapter->ptp_caps.settime = ixgbe_ptp_settime;
+		adapter->ptp_caps.gettime64 = ixgbe_ptp_gettime;
+		adapter->ptp_caps.settime64 = ixgbe_ptp_settime;
 		adapter->ptp_caps.enable = ixgbe_ptp_feature_enable;
 		break;
 	case ixgbe_mac_82599EB:
@@ -890,8 +887,8 @@ static int ixgbe_ptp_create_clock(struct ixgbe_adapter *adapter)
 		adapter->ptp_caps.pps = 0;
 		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq;
 		adapter->ptp_caps.adjtime = ixgbe_ptp_adjtime;
-		adapter->ptp_caps.gettime = ixgbe_ptp_gettime;
-		adapter->ptp_caps.settime = ixgbe_ptp_settime;
+		adapter->ptp_caps.gettime64 = ixgbe_ptp_gettime;
+		adapter->ptp_caps.settime64 = ixgbe_ptp_settime;
 		adapter->ptp_caps.enable = ixgbe_ptp_feature_enable;
 		break;
 	default:
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index 7f37fe7269a7..1d17b5872dd1 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -36,6 +36,7 @@
 #include <linux/ip.h>
 #include <linux/tcp.h>
 #include <linux/ipv6.h>
+#include <linux/if_bridge.h>
 #ifdef NETIF_F_HW_VLAN_CTAG_TX
 #include <linux/if_vlan.h>
 #endif
@@ -79,7 +80,7 @@ static int __ixgbe_enable_sriov(struct ixgbe_adapter *adapter)
 
 	/* Initialize default switching mode VEB */
 	IXGBE_WRITE_REG(hw, IXGBE_PFDTXGSWC, IXGBE_PFDTXGSWC_VT_LBEN);
-	adapter->flags2 |= IXGBE_FLAG2_BRIDGE_MODE_VEB;
+	adapter->bridge_mode = BRIDGE_MODE_VEB;
 
 	/* If call to enable VFs succeeded then allocate memory
 	 * for per VF control structures.
@@ -105,9 +106,18 @@ static int __ixgbe_enable_sriov(struct ixgbe_adapter *adapter)
 		adapter->flags2 &= ~(IXGBE_FLAG2_RSC_CAPABLE |
 				     IXGBE_FLAG2_RSC_ENABLED);
 
-		/* enable spoof checking for all VFs */
-		for (i = 0; i < adapter->num_vfs; i++)
+		for (i = 0; i < adapter->num_vfs; i++) {
+			/* enable spoof checking for all VFs */
 			adapter->vfinfo[i].spoofchk_enabled = true;
+
+			/* We support VF RSS querying only for 82599 and x540
+			 * devices at the moment. These devices share RSS
+			 * indirection table and RSS hash key with PF therefore
+			 * we want to disable the querying by default.
+			 */
+			adapter->vfinfo[i].rss_query_enabled = 0;
+		}
+
 		return 0;
 	}
 
@@ -141,7 +151,7 @@ void ixgbe_enable_sriov(struct ixgbe_adapter *adapter)
 		 * The 82599 supports up to 64 VFs per physical function
 		 * but this implementation limits allocation to 63 so that
 		 * basic networking resources are still available to the
-		 * physical function.  If the user requests greater thn
+		 * physical function.  If the user requests greater than
 		 * 63 VFs then it is an error - reset to default of zero.
 		 */
 		adapter->num_vfs = min_t(unsigned int, adapter->num_vfs, IXGBE_MAX_VFS_DRV_LIMIT);
@@ -424,6 +434,7 @@ static s32 ixgbe_set_vf_lpe(struct ixgbe_adapter *adapter, u32 *msgbuf, u32 vf)
 #endif /* CONFIG_FCOE */
 		switch (adapter->vfinfo[vf].vf_api) {
 		case ixgbe_mbox_api_11:
+		case ixgbe_mbox_api_12:
 			/*
 			 * Version 1.1 supports jumbo frames on VFs if PF has
 			 * jumbo frames enabled which means legacy VFs are
@@ -891,6 +902,7 @@ static int ixgbe_negotiate_vf_api(struct ixgbe_adapter *adapter,
 	switch (api) {
 	case ixgbe_mbox_api_10:
 	case ixgbe_mbox_api_11:
+	case ixgbe_mbox_api_12:
 		adapter->vfinfo[vf].vf_api = api;
 		return 0;
 	default:
@@ -914,6 +926,7 @@ static int ixgbe_get_vf_queues(struct ixgbe_adapter *adapter,
 	switch (adapter->vfinfo[vf].vf_api) {
 	case ixgbe_mbox_api_20:
 	case ixgbe_mbox_api_11:
+	case ixgbe_mbox_api_12:
 		break;
 	default:
 		return -1;
@@ -941,6 +954,53 @@ static int ixgbe_get_vf_queues(struct ixgbe_adapter *adapter,
 	return 0;
 }
 
+static int ixgbe_get_vf_reta(struct ixgbe_adapter *adapter, u32 *msgbuf, u32 vf)
+{
+	u32 i, j;
+	u32 *out_buf = &msgbuf[1];
+	const u8 *reta = adapter->rss_indir_tbl;
+	u32 reta_size = ixgbe_rss_indir_tbl_entries(adapter);
+
+	/* Check if operation is permitted */
+	if (!adapter->vfinfo[vf].rss_query_enabled)
+		return -EPERM;
+
+	/* verify the PF is supporting the correct API */
+	if (adapter->vfinfo[vf].vf_api != ixgbe_mbox_api_12)
+		return -EOPNOTSUPP;
+
+	/* This mailbox command is supported (required) only for 82599 and x540
+	 * VFs which support up to 4 RSS queues. Therefore we will compress the
+	 * RETA by saving only 2 bits from each entry. This way we will be able
+	 * to transfer the whole RETA in a single mailbox operation.
+	 */
+	for (i = 0; i < reta_size / 16; i++) {
+		out_buf[i] = 0;
+		for (j = 0; j < 16; j++)
+			out_buf[i] |= (u32)(reta[16 * i + j] & 0x3) << (2 * j);
+	}
+
+	return 0;
+}
+
+static int ixgbe_get_vf_rss_key(struct ixgbe_adapter *adapter,
+				u32 *msgbuf, u32 vf)
+{
+	u32 *rss_key = &msgbuf[1];
+
+	/* Check if the operation is permitted */
+	if (!adapter->vfinfo[vf].rss_query_enabled)
+		return -EPERM;
+
+	/* verify the PF is supporting the correct API */
+	if (adapter->vfinfo[vf].vf_api != ixgbe_mbox_api_12)
+		return -EOPNOTSUPP;
+
+	memcpy(rss_key, adapter->rss_key, sizeof(adapter->rss_key));
+
+	return 0;
+}
+
 static int ixgbe_rcv_msg_from_vf(struct ixgbe_adapter *adapter, u32 vf)
 {
 	u32 mbx_size = IXGBE_VFMAILBOX_SIZE;
@@ -997,6 +1057,12 @@ static int ixgbe_rcv_msg_from_vf(struct ixgbe_adapter *adapter, u32 vf)
 	case IXGBE_VF_GET_QUEUES:
 		retval = ixgbe_get_vf_queues(adapter, msgbuf, vf);
 		break;
+	case IXGBE_VF_GET_RETA:
+		retval = ixgbe_get_vf_reta(adapter, msgbuf, vf);
+		break;
+	case IXGBE_VF_GET_RSS_KEY:
+		retval = ixgbe_get_vf_rss_key(adapter, msgbuf, vf);
+		break;
 	default:
 		e_err(drv, "Unhandled Msg %8.8x\n", msgbuf[0]);
 		retval = IXGBE_ERR_MBX;
@@ -1330,6 +1396,26 @@ int ixgbe_ndo_set_vf_spoofchk(struct net_device *netdev, int vf, bool setting)
 	return 0;
 }
 
+int ixgbe_ndo_set_vf_rss_query_en(struct net_device *netdev, int vf,
+				  bool setting)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+
+	/* This operation is currently supported only for 82599 and x540
+	 * devices.
+	 */
+	if (adapter->hw.mac.type < ixgbe_mac_82599EB ||
+	    adapter->hw.mac.type >= ixgbe_mac_X550)
+		return -EOPNOTSUPP;
+
+	if (vf >= adapter->num_vfs)
+		return -EINVAL;
+
+	adapter->vfinfo[vf].rss_query_enabled = setting;
+
+	return 0;
+}
+
 int ixgbe_ndo_get_vf_config(struct net_device *netdev,
 			    int vf, struct ifla_vf_info *ivi)
 {
@@ -1343,5 +1429,6 @@ int ixgbe_ndo_get_vf_config(struct net_device *netdev,
 	ivi->vlan = adapter->vfinfo[vf].pf_vlan;
 	ivi->qos = adapter->vfinfo[vf].pf_qos;
 	ivi->spoofchk = adapter->vfinfo[vf].spoofchk_enabled;
+	ivi->rss_query_en = adapter->vfinfo[vf].rss_query_enabled;
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
index 32c26d586c01..2c197e6d1fe7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
@@ -47,6 +47,8 @@ int ixgbe_ndo_set_vf_vlan(struct net_device *netdev, int queue, u16 vlan,
 int ixgbe_ndo_set_vf_bw(struct net_device *netdev, int vf, int min_tx_rate,
 			int max_tx_rate);
 int ixgbe_ndo_set_vf_spoofchk(struct net_device *netdev, int vf, bool setting);
+int ixgbe_ndo_set_vf_rss_query_en(struct net_device *netdev, int vf,
+				  bool setting);
 int ixgbe_ndo_get_vf_config(struct net_device *netdev,
 			    int vf, struct ifla_vf_info *ivi);
 void ixgbe_check_vf_rate_limit(struct ixgbe_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index fc5ecee56ca8..dd6ba5916dfe 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -285,6 +285,8 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_VLVF(_i)  (0x0F100 + ((_i) * 4))  /* 64 of these (0-63) */
 #define IXGBE_VLVFB(_i) (0x0F200 + ((_i) * 4))  /* 128 of these (0-127) */
 #define IXGBE_VMVIR(_i) (0x08000 + ((_i) * 4))  /* 64 of these (0-63) */
+#define IXGBE_PFFLPL	0x050B0
+#define IXGBE_PFFLPH	0x050B4
 #define IXGBE_VT_CTL         0x051B0
 #define IXGBE_PFMAILBOX(_i)  (0x04B00 + (4 * (_i))) /* 64 total */
 #define IXGBE_PFMBMEM(_i)    (0x13000 + (64 * (_i))) /* 64 Mailboxes, 16 DW each */
@@ -608,6 +610,8 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_RTTBCNRM    0x04980
 #define IXGBE_RTTQCNRM    0x04980
 
+/* FCoE Direct DMA Context */
+#define IXGBE_FCDDC(_i, _j)	(0x20000 + ((_i) * 0x4) + ((_j) * 0x10))
 /* FCoE DMA Context Registers */
 #define IXGBE_FCPTRL    0x02410 /* FC User Desc. PTR Low */
 #define IXGBE_FCPTRH    0x02414 /* FC USer Desc. PTR High */
@@ -634,6 +638,9 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_TSOFF     0x04A98 /* Tx FC SOF */
 #define IXGBE_REOFF     0x05158 /* Rx FC EOF */
 #define IXGBE_RSOFF     0x051F8 /* Rx FC SOF */
+/* FCoE Direct Filter Context */
+#define IXGBE_FCDFC(_i, _j)	(0x28000 + ((_i) * 0x4) + ((_j) * 0x10))
+#define IXGBE_FCDFCD(_i)	(0x30000 + ((_i) * 0x4))
 /* FCoE Filter Context Registers */
 #define IXGBE_FCFLT     0x05108 /* FC FLT Context */
 #define IXGBE_FCFLTRW   0x05110 /* FC Filter RW Control */
@@ -664,6 +671,10 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_FCRECTL_ENA       0x1        /* FCoE Redir Table Enable */
 #define IXGBE_FCRETA_SIZE       8          /* Max entries in FCRETA */
 #define IXGBE_FCRETA_ENTRY_MASK 0x0000007f /* 7 bits for the queue index */
+#define IXGBE_FCRETA_SIZE_X550	32 /* Max entries in FCRETA */
+/* Higher 7 bits for the queue index */
+#define IXGBE_FCRETA_ENTRY_HIGH_MASK	0x007F0000
+#define IXGBE_FCRETA_ENTRY_HIGH_SHIFT	16
 
 /* Stats registers */
 #define IXGBE_CRCERRS   0x04000
@@ -1690,7 +1701,7 @@ enum {
 #define IXGBE_MACC_FS        0x00040000
 #define IXGBE_MAC_RX2TX_LPBK 0x00000002
 
-/* Veto Bit definiton */
+/* Veto Bit definition */
 #define IXGBE_MMNGC_MNG_VETO  0x00000001
 
 /* LINKS Bit Masks */
@@ -2462,8 +2473,8 @@ struct ixgbe_hic_read_shadow_ram {
 
 struct ixgbe_hic_write_shadow_ram {
 	union ixgbe_hic_hdr2 hdr;
-	u32 address;
-	u16 length;
+	__be32 address;
+	__be16 length;
 	u16 pad2;
 	u16 data;
 	u16 pad3;
@@ -3067,6 +3078,10 @@ struct ixgbe_mac_operations {
 	s32 (*set_fw_drv_ver)(struct ixgbe_hw *, u8, u8, u8, u8);
 	s32 (*get_thermal_sensor_data)(struct ixgbe_hw *);
 	s32 (*init_thermal_sensor_thresh)(struct ixgbe_hw *hw);
+	void (*disable_rx)(struct ixgbe_hw *hw);
+	void (*enable_rx)(struct ixgbe_hw *hw);
+	void (*set_source_address_pruning)(struct ixgbe_hw *, bool,
+					   unsigned int);
 	void (*set_ethertype_anti_spoofing)(struct ixgbe_hw *, bool, int);
 
 	/* DMA Coalescing */
@@ -3137,6 +3152,7 @@ struct ixgbe_mac_info {
 	u8                              flags;
 	u8				san_mac_rar_index;
 	struct ixgbe_thermal_sensor_data  thermal_sensor_data;
+	bool				set_lben;
 };
 
 struct ixgbe_phy_info {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
index 49395420c9b3..f5f948d08b43 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
@@ -820,6 +820,8 @@ static struct ixgbe_mac_operations mac_ops_X540 = {
 	.init_thermal_sensor_thresh = NULL,
 	.prot_autoc_read	= &prot_autoc_read_generic,
 	.prot_autoc_write	= &prot_autoc_write_generic,
+	.enable_rx		= &ixgbe_enable_rx_generic,
+	.disable_rx		= &ixgbe_disable_rx_generic,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_X540 = {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
index 50bf81908dd6..cf5cf819a6b8 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
@@ -557,6 +557,47 @@ static s32 ixgbe_update_flash_X550(struct ixgbe_hw *hw)
 	return status;
 }
 
+/** ixgbe_disable_rx_x550 - Disable RX unit
+ *
+ *  Enables the Rx DMA unit for x550
+ **/
+static void ixgbe_disable_rx_x550(struct ixgbe_hw *hw)
+{
+	u32 rxctrl, pfdtxgswc;
+	s32 status;
+	struct ixgbe_hic_disable_rxen fw_cmd;
+
+	rxctrl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
+	if (rxctrl & IXGBE_RXCTRL_RXEN) {
+		pfdtxgswc = IXGBE_READ_REG(hw, IXGBE_PFDTXGSWC);
+		if (pfdtxgswc & IXGBE_PFDTXGSWC_VT_LBEN) {
+			pfdtxgswc &= ~IXGBE_PFDTXGSWC_VT_LBEN;
+			IXGBE_WRITE_REG(hw, IXGBE_PFDTXGSWC, pfdtxgswc);
+			hw->mac.set_lben = true;
+		} else {
+			hw->mac.set_lben = false;
+		}
+
+		fw_cmd.hdr.cmd = FW_DISABLE_RXEN_CMD;
+		fw_cmd.hdr.buf_len = FW_DISABLE_RXEN_LEN;
+		fw_cmd.hdr.checksum = FW_DEFAULT_CHECKSUM;
+		fw_cmd.port_number = (u8)hw->bus.lan_id;
+
+		status = ixgbe_host_interface_command(hw, (u32 *)&fw_cmd,
+					sizeof(struct ixgbe_hic_disable_rxen),
+					IXGBE_HI_COMMAND_TIMEOUT, true);
+
+		/* If we fail - disable RX using register write */
+		if (status) {
+			rxctrl = IXGBE_READ_REG(hw, IXGBE_RXCTRL);
+			if (rxctrl & IXGBE_RXCTRL_RXEN) {
+				rxctrl &= ~IXGBE_RXCTRL_RXEN;
+				IXGBE_WRITE_REG(hw, IXGBE_RXCTRL, rxctrl);
+			}
+		}
+	}
+}
+
 /** ixgbe_update_eeprom_checksum_X550 - Updates the EEPROM checksum and flash
  *  @hw: pointer to hardware structure
  *
@@ -1306,8 +1347,8 @@ mac_reset_top:
  *  @enable: enable or disable switch for Ethertype anti-spoofing
  *  @vf: Virtual Function pool - VF Pool to set for Ethertype anti-spoofing
  **/
-void ixgbe_set_ethertype_anti_spoofing_X550(struct ixgbe_hw *hw, bool enable,
-					    int vf)
+static void ixgbe_set_ethertype_anti_spoofing_X550(struct ixgbe_hw *hw,
+						   bool enable, int vf)
 {
 	int vf_target_reg = vf >> 3;
 	int vf_target_shift = vf % 8 + IXGBE_SPOOF_ETHERTYPEAS_SHIFT;
@@ -1322,6 +1363,33 @@ void ixgbe_set_ethertype_anti_spoofing_X550(struct ixgbe_hw *hw, bool enable,
 	IXGBE_WRITE_REG(hw, IXGBE_PFVFSPOOF(vf_target_reg), pfvfspoof);
 }
 
+/** ixgbe_set_source_address_pruning_X550 - Enable/Disbale src address pruning
+ *  @hw: pointer to hardware structure
+ *  @enable: enable or disable source address pruning
+ *  @pool: Rx pool to set source address pruning for
+ **/
+static void ixgbe_set_source_address_pruning_X550(struct ixgbe_hw *hw,
+						  bool enable,
+						  unsigned int pool)
+{
+	u64 pfflp;
+
+	/* max rx pool is 63 */
+	if (pool > 63)
+		return;
+
+	pfflp = (u64)IXGBE_READ_REG(hw, IXGBE_PFFLPL);
+	pfflp |= (u64)IXGBE_READ_REG(hw, IXGBE_PFFLPH) << 32;
+
+	if (enable)
+		pfflp |= (1ULL << pool);
+	else
+		pfflp &= ~(1ULL << pool);
+
+	IXGBE_WRITE_REG(hw, IXGBE_PFFLPL, (u32)pfflp);
+	IXGBE_WRITE_REG(hw, IXGBE_PFFLPH, (u32)(pfflp >> 32));
+}
+
 #define X550_COMMON_MAC \
 	.init_hw			= &ixgbe_init_hw_generic, \
 	.start_hw			= &ixgbe_start_hw_X540, \
@@ -1356,6 +1424,8 @@ void ixgbe_set_ethertype_anti_spoofing_X550(struct ixgbe_hw *hw, bool enable,
 	.init_uta_tables		= &ixgbe_init_uta_tables_generic, \
 	.set_mac_anti_spoofing		= &ixgbe_set_mac_anti_spoofing, \
 	.set_vlan_anti_spoofing		= &ixgbe_set_vlan_anti_spoofing, \
+	.set_source_address_pruning	= \
+				&ixgbe_set_source_address_pruning_X550, \
 	.set_ethertype_anti_spoofing	= \
 				&ixgbe_set_ethertype_anti_spoofing_X550, \
 	.acquire_swfw_sync		= &ixgbe_acquire_swfw_sync_X540, \
@@ -1366,6 +1436,8 @@ void ixgbe_set_ethertype_anti_spoofing_X550(struct ixgbe_hw *hw, bool enable,
 	.init_thermal_sensor_thresh	= NULL, \
 	.prot_autoc_read		= &prot_autoc_read_generic, \
 	.prot_autoc_write		= &prot_autoc_write_generic, \
+	.enable_rx			= &ixgbe_enable_rx_generic, \
+	.disable_rx			= &ixgbe_disable_rx_x550, \
 
 static struct ixgbe_mac_operations mac_ops_X550 = {
 	X550_COMMON_MAC
diff --git a/drivers/net/ethernet/intel/ixgbevf/defines.h b/drivers/net/ethernet/intel/ixgbevf/defines.h
index 7412d378b77b..770e21a64388 100644
--- a/drivers/net/ethernet/intel/ixgbevf/defines.h
+++ b/drivers/net/ethernet/intel/ixgbevf/defines.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2012 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -29,138 +28,138 @@
 #define _IXGBEVF_DEFINES_H_
 
 /* Device IDs */
-#define IXGBE_DEV_ID_82599_VF           0x10ED
-#define IXGBE_DEV_ID_X540_VF            0x1515
+#define IXGBE_DEV_ID_82599_VF		0x10ED
+#define IXGBE_DEV_ID_X540_VF		0x1515
 #define IXGBE_DEV_ID_X550_VF		0x1565
 #define IXGBE_DEV_ID_X550EM_X_VF	0x15A8
 
-#define IXGBE_VF_IRQ_CLEAR_MASK         7
-#define IXGBE_VF_MAX_TX_QUEUES          8
-#define IXGBE_VF_MAX_RX_QUEUES          8
+#define IXGBE_VF_IRQ_CLEAR_MASK		7
+#define IXGBE_VF_MAX_TX_QUEUES		8
+#define IXGBE_VF_MAX_RX_QUEUES		8
 
 /* DCB define */
 #define IXGBE_VF_MAX_TRAFFIC_CLASS	8
 
 /* Link speed */
 typedef u32 ixgbe_link_speed;
-#define IXGBE_LINK_SPEED_1GB_FULL       0x0020
-#define IXGBE_LINK_SPEED_10GB_FULL      0x0080
+#define IXGBE_LINK_SPEED_1GB_FULL	0x0020
+#define IXGBE_LINK_SPEED_10GB_FULL	0x0080
 #define IXGBE_LINK_SPEED_100_FULL	0x0008
 
-#define IXGBE_CTRL_RST              0x04000000 /* Reset (SW) */
-#define IXGBE_RXDCTL_ENABLE         0x02000000 /* Enable specific Rx Queue */
-#define IXGBE_TXDCTL_ENABLE         0x02000000 /* Enable specific Tx Queue */
-#define IXGBE_LINKS_UP              0x40000000
-#define IXGBE_LINKS_SPEED_82599     0x30000000
-#define IXGBE_LINKS_SPEED_10G_82599 0x30000000
-#define IXGBE_LINKS_SPEED_1G_82599  0x20000000
-#define IXGBE_LINKS_SPEED_100_82599 0x10000000
+#define IXGBE_CTRL_RST		0x04000000 /* Reset (SW) */
+#define IXGBE_RXDCTL_ENABLE	0x02000000 /* Enable specific Rx Queue */
+#define IXGBE_TXDCTL_ENABLE	0x02000000 /* Enable specific Tx Queue */
+#define IXGBE_LINKS_UP		0x40000000
+#define IXGBE_LINKS_SPEED_82599		0x30000000
+#define IXGBE_LINKS_SPEED_10G_82599	0x30000000
+#define IXGBE_LINKS_SPEED_1G_82599	0x20000000
+#define IXGBE_LINKS_SPEED_100_82599	0x10000000
 
 /* Number of Transmit and Receive Descriptors must be a multiple of 8 */
-#define IXGBE_REQ_TX_DESCRIPTOR_MULTIPLE  8
-#define IXGBE_REQ_RX_DESCRIPTOR_MULTIPLE  8
-#define IXGBE_REQ_TX_BUFFER_GRANULARITY   1024
+#define IXGBE_REQ_TX_DESCRIPTOR_MULTIPLE	8
+#define IXGBE_REQ_RX_DESCRIPTOR_MULTIPLE	8
+#define IXGBE_REQ_TX_BUFFER_GRANULARITY		1024
 
 /* Interrupt Vector Allocation Registers */
-#define IXGBE_IVAR_ALLOC_VAL    0x80 /* Interrupt Allocation valid */
+#define IXGBE_IVAR_ALLOC_VAL	0x80 /* Interrupt Allocation valid */
 
-#define IXGBE_VF_INIT_TIMEOUT   200 /* Number of retries to clear RSTI */
+#define IXGBE_VF_INIT_TIMEOUT	200 /* Number of retries to clear RSTI */
 
 /* Receive Config masks */
-#define IXGBE_RXCTRL_RXEN       0x00000001  /* Enable Receiver */
-#define IXGBE_RXCTRL_DMBYPS     0x00000002  /* Descriptor Monitor Bypass */
-#define IXGBE_RXDCTL_ENABLE     0x02000000  /* Enable specific Rx Queue */
-#define IXGBE_RXDCTL_VME        0x40000000  /* VLAN mode enable */
-#define IXGBE_RXDCTL_RLPMLMASK  0x00003FFF  /* Only supported on the X540 */
-#define IXGBE_RXDCTL_RLPML_EN   0x00008000
+#define IXGBE_RXCTRL_RXEN	0x00000001  /* Enable Receiver */
+#define IXGBE_RXCTRL_DMBYPS	0x00000002  /* Descriptor Monitor Bypass */
+#define IXGBE_RXDCTL_ENABLE	0x02000000  /* Enable specific Rx Queue */
+#define IXGBE_RXDCTL_VME	0x40000000  /* VLAN mode enable */
+#define IXGBE_RXDCTL_RLPMLMASK	0x00003FFF  /* Only supported on the X540 */
+#define IXGBE_RXDCTL_RLPML_EN	0x00008000
 
 /* DCA Control */
 #define IXGBE_DCA_TXCTRL_TX_WB_RO_EN (1 << 11) /* Tx Desc writeback RO bit */
 
 /* PSRTYPE bit definitions */
-#define IXGBE_PSRTYPE_TCPHDR    0x00000010
-#define IXGBE_PSRTYPE_UDPHDR    0x00000020
-#define IXGBE_PSRTYPE_IPV4HDR   0x00000100
-#define IXGBE_PSRTYPE_IPV6HDR   0x00000200
-#define IXGBE_PSRTYPE_L2HDR     0x00001000
+#define IXGBE_PSRTYPE_TCPHDR	0x00000010
+#define IXGBE_PSRTYPE_UDPHDR	0x00000020
+#define IXGBE_PSRTYPE_IPV4HDR	0x00000100
+#define IXGBE_PSRTYPE_IPV6HDR	0x00000200
+#define IXGBE_PSRTYPE_L2HDR	0x00001000
 
 /* SRRCTL bit definitions */
-#define IXGBE_SRRCTL_BSIZEPKT_SHIFT     10     /* so many KBs */
-#define IXGBE_SRRCTL_RDMTS_SHIFT        22
-#define IXGBE_SRRCTL_RDMTS_MASK         0x01C00000
-#define IXGBE_SRRCTL_DROP_EN            0x10000000
-#define IXGBE_SRRCTL_BSIZEPKT_MASK      0x0000007F
-#define IXGBE_SRRCTL_BSIZEHDR_MASK      0x00003F00
-#define IXGBE_SRRCTL_DESCTYPE_LEGACY    0x00000000
+#define IXGBE_SRRCTL_BSIZEPKT_SHIFT	10     /* so many KBs */
+#define IXGBE_SRRCTL_RDMTS_SHIFT	22
+#define IXGBE_SRRCTL_RDMTS_MASK		0x01C00000
+#define IXGBE_SRRCTL_DROP_EN		0x10000000
+#define IXGBE_SRRCTL_BSIZEPKT_MASK	0x0000007F
+#define IXGBE_SRRCTL_BSIZEHDR_MASK	0x00003F00
+#define IXGBE_SRRCTL_DESCTYPE_LEGACY	0x00000000
 #define IXGBE_SRRCTL_DESCTYPE_ADV_ONEBUF 0x02000000
-#define IXGBE_SRRCTL_DESCTYPE_HDR_SPLIT  0x04000000
+#define IXGBE_SRRCTL_DESCTYPE_HDR_SPLIT	0x04000000
 #define IXGBE_SRRCTL_DESCTYPE_HDR_REPLICATION_LARGE_PKT 0x08000000
 #define IXGBE_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS 0x0A000000
-#define IXGBE_SRRCTL_DESCTYPE_MASK      0x0E000000
+#define IXGBE_SRRCTL_DESCTYPE_MASK	0x0E000000
 
 /* Receive Descriptor bit definitions */
-#define IXGBE_RXD_STAT_DD         0x01    /* Descriptor Done */
-#define IXGBE_RXD_STAT_EOP        0x02    /* End of Packet */
-#define IXGBE_RXD_STAT_FLM        0x04    /* FDir Match */
-#define IXGBE_RXD_STAT_VP         0x08    /* IEEE VLAN Packet */
-#define IXGBE_RXDADV_NEXTP_MASK   0x000FFFF0 /* Next Descriptor Index */
-#define IXGBE_RXDADV_NEXTP_SHIFT  0x00000004
-#define IXGBE_RXD_STAT_UDPCS      0x10    /* UDP xsum calculated */
-#define IXGBE_RXD_STAT_L4CS       0x20    /* L4 xsum calculated */
-#define IXGBE_RXD_STAT_IPCS       0x40    /* IP xsum calculated */
-#define IXGBE_RXD_STAT_PIF        0x80    /* passed in-exact filter */
-#define IXGBE_RXD_STAT_CRCV       0x100   /* Speculative CRC Valid */
-#define IXGBE_RXD_STAT_VEXT       0x200   /* 1st VLAN found */
-#define IXGBE_RXD_STAT_UDPV       0x400   /* Valid UDP checksum */
-#define IXGBE_RXD_STAT_DYNINT     0x800   /* Pkt caused INT via DYNINT */
-#define IXGBE_RXD_STAT_TS         0x10000 /* Time Stamp */
-#define IXGBE_RXD_STAT_SECP       0x20000 /* Security Processing */
-#define IXGBE_RXD_STAT_LB         0x40000 /* Loopback Status */
-#define IXGBE_RXD_STAT_ACK        0x8000  /* ACK Packet indication */
-#define IXGBE_RXD_ERR_CE          0x01    /* CRC Error */
-#define IXGBE_RXD_ERR_LE          0x02    /* Length Error */
-#define IXGBE_RXD_ERR_PE          0x08    /* Packet Error */
-#define IXGBE_RXD_ERR_OSE         0x10    /* Oversize Error */
-#define IXGBE_RXD_ERR_USE         0x20    /* Undersize Error */
-#define IXGBE_RXD_ERR_TCPE        0x40    /* TCP/UDP Checksum Error */
-#define IXGBE_RXD_ERR_IPE         0x80    /* IP Checksum Error */
-#define IXGBE_RXDADV_ERR_MASK     0xFFF00000 /* RDESC.ERRORS mask */
-#define IXGBE_RXDADV_ERR_SHIFT    20         /* RDESC.ERRORS shift */
-#define IXGBE_RXDADV_ERR_HBO      0x00800000 /*Header Buffer Overflow */
-#define IXGBE_RXDADV_ERR_CE       0x01000000 /* CRC Error */
-#define IXGBE_RXDADV_ERR_LE       0x02000000 /* Length Error */
-#define IXGBE_RXDADV_ERR_PE       0x08000000 /* Packet Error */
-#define IXGBE_RXDADV_ERR_OSE      0x10000000 /* Oversize Error */
-#define IXGBE_RXDADV_ERR_USE      0x20000000 /* Undersize Error */
-#define IXGBE_RXDADV_ERR_TCPE     0x40000000 /* TCP/UDP Checksum Error */
-#define IXGBE_RXDADV_ERR_IPE      0x80000000 /* IP Checksum Error */
-#define IXGBE_RXD_VLAN_ID_MASK    0x0FFF  /* VLAN ID is in lower 12 bits */
-#define IXGBE_RXD_PRI_MASK        0xE000  /* Priority is in upper 3 bits */
-#define IXGBE_RXD_PRI_SHIFT       13
-#define IXGBE_RXD_CFI_MASK        0x1000  /* CFI is bit 12 */
-#define IXGBE_RXD_CFI_SHIFT       12
-
-#define IXGBE_RXDADV_STAT_DD            IXGBE_RXD_STAT_DD  /* Done */
-#define IXGBE_RXDADV_STAT_EOP           IXGBE_RXD_STAT_EOP /* End of Packet */
-#define IXGBE_RXDADV_STAT_FLM           IXGBE_RXD_STAT_FLM /* FDir Match */
-#define IXGBE_RXDADV_STAT_VP            IXGBE_RXD_STAT_VP  /* IEEE VLAN Pkt */
-#define IXGBE_RXDADV_STAT_MASK          0x000FFFFF /* Stat/NEXTP: bit 0-19 */
-#define IXGBE_RXDADV_STAT_FCEOFS        0x00000040 /* FCoE EOF/SOF Stat */
-#define IXGBE_RXDADV_STAT_FCSTAT        0x00000030 /* FCoE Pkt Stat */
-#define IXGBE_RXDADV_STAT_FCSTAT_NOMTCH 0x00000000 /* 00: No Ctxt Match */
-#define IXGBE_RXDADV_STAT_FCSTAT_NODDP  0x00000010 /* 01: Ctxt w/o DDP */
-#define IXGBE_RXDADV_STAT_FCSTAT_FCPRSP 0x00000020 /* 10: Recv. FCP_RSP */
-#define IXGBE_RXDADV_STAT_FCSTAT_DDP    0x00000030 /* 11: Ctxt w/ DDP */
-
-#define IXGBE_RXDADV_RSSTYPE_MASK       0x0000000F
-#define IXGBE_RXDADV_PKTTYPE_MASK       0x0000FFF0
-#define IXGBE_RXDADV_PKTTYPE_MASK_EX    0x0001FFF0
-#define IXGBE_RXDADV_HDRBUFLEN_MASK     0x00007FE0
-#define IXGBE_RXDADV_RSCCNT_MASK        0x001E0000
-#define IXGBE_RXDADV_RSCCNT_SHIFT       17
-#define IXGBE_RXDADV_HDRBUFLEN_SHIFT    5
-#define IXGBE_RXDADV_SPLITHEADER_EN     0x00001000
-#define IXGBE_RXDADV_SPH                0x8000
+#define IXGBE_RXD_STAT_DD	0x01    /* Descriptor Done */
+#define IXGBE_RXD_STAT_EOP	0x02    /* End of Packet */
+#define IXGBE_RXD_STAT_FLM	0x04    /* FDir Match */
+#define IXGBE_RXD_STAT_VP	0x08    /* IEEE VLAN Packet */
+#define IXGBE_RXDADV_NEXTP_MASK	0x000FFFF0 /* Next Descriptor Index */
+#define IXGBE_RXDADV_NEXTP_SHIFT	0x00000004
+#define IXGBE_RXD_STAT_UDPCS	0x10    /* UDP xsum calculated */
+#define IXGBE_RXD_STAT_L4CS	0x20    /* L4 xsum calculated */
+#define IXGBE_RXD_STAT_IPCS	0x40    /* IP xsum calculated */
+#define IXGBE_RXD_STAT_PIF	0x80    /* passed in-exact filter */
+#define IXGBE_RXD_STAT_CRCV	0x100   /* Speculative CRC Valid */
+#define IXGBE_RXD_STAT_VEXT	0x200   /* 1st VLAN found */
+#define IXGBE_RXD_STAT_UDPV	0x400   /* Valid UDP checksum */
+#define IXGBE_RXD_STAT_DYNINT	0x800   /* Pkt caused INT via DYNINT */
+#define IXGBE_RXD_STAT_TS	0x10000 /* Time Stamp */
+#define IXGBE_RXD_STAT_SECP	0x20000 /* Security Processing */
+#define IXGBE_RXD_STAT_LB	0x40000 /* Loopback Status */
+#define IXGBE_RXD_STAT_ACK	0x8000  /* ACK Packet indication */
+#define IXGBE_RXD_ERR_CE	0x01    /* CRC Error */
+#define IXGBE_RXD_ERR_LE	0x02    /* Length Error */
+#define IXGBE_RXD_ERR_PE	0x08    /* Packet Error */
+#define IXGBE_RXD_ERR_OSE	0x10    /* Oversize Error */
+#define IXGBE_RXD_ERR_USE	0x20    /* Undersize Error */
+#define IXGBE_RXD_ERR_TCPE	0x40    /* TCP/UDP Checksum Error */
+#define IXGBE_RXD_ERR_IPE	0x80    /* IP Checksum Error */
+#define IXGBE_RXDADV_ERR_MASK	0xFFF00000 /* RDESC.ERRORS mask */
+#define IXGBE_RXDADV_ERR_SHIFT	20         /* RDESC.ERRORS shift */
+#define IXGBE_RXDADV_ERR_HBO	0x00800000 /*Header Buffer Overflow */
+#define IXGBE_RXDADV_ERR_CE	0x01000000 /* CRC Error */
+#define IXGBE_RXDADV_ERR_LE	0x02000000 /* Length Error */
+#define IXGBE_RXDADV_ERR_PE	0x08000000 /* Packet Error */
+#define IXGBE_RXDADV_ERR_OSE	0x10000000 /* Oversize Error */
+#define IXGBE_RXDADV_ERR_USE	0x20000000 /* Undersize Error */
+#define IXGBE_RXDADV_ERR_TCPE	0x40000000 /* TCP/UDP Checksum Error */
+#define IXGBE_RXDADV_ERR_IPE	0x80000000 /* IP Checksum Error */
+#define IXGBE_RXD_VLAN_ID_MASK	0x0FFF  /* VLAN ID is in lower 12 bits */
+#define IXGBE_RXD_PRI_MASK	0xE000  /* Priority is in upper 3 bits */
+#define IXGBE_RXD_PRI_SHIFT	13
+#define IXGBE_RXD_CFI_MASK	0x1000  /* CFI is bit 12 */
+#define IXGBE_RXD_CFI_SHIFT	12
+
+#define IXGBE_RXDADV_STAT_DD		IXGBE_RXD_STAT_DD  /* Done */
+#define IXGBE_RXDADV_STAT_EOP		IXGBE_RXD_STAT_EOP /* End of Packet */
+#define IXGBE_RXDADV_STAT_FLM		IXGBE_RXD_STAT_FLM /* FDir Match */
+#define IXGBE_RXDADV_STAT_VP		IXGBE_RXD_STAT_VP  /* IEEE VLAN Pkt */
+#define IXGBE_RXDADV_STAT_MASK		0x000FFFFF /* Stat/NEXTP: bit 0-19 */
+#define IXGBE_RXDADV_STAT_FCEOFS	0x00000040 /* FCoE EOF/SOF Stat */
+#define IXGBE_RXDADV_STAT_FCSTAT	0x00000030 /* FCoE Pkt Stat */
+#define IXGBE_RXDADV_STAT_FCSTAT_NOMTCH	0x00000000 /* 00: No Ctxt Match */
+#define IXGBE_RXDADV_STAT_FCSTAT_NODDP	0x00000010 /* 01: Ctxt w/o DDP */
+#define IXGBE_RXDADV_STAT_FCSTAT_FCPRSP	0x00000020 /* 10: Recv. FCP_RSP */
+#define IXGBE_RXDADV_STAT_FCSTAT_DDP	0x00000030 /* 11: Ctxt w/ DDP */
+
+#define IXGBE_RXDADV_RSSTYPE_MASK	0x0000000F
+#define IXGBE_RXDADV_PKTTYPE_MASK	0x0000FFF0
+#define IXGBE_RXDADV_PKTTYPE_MASK_EX	0x0001FFF0
+#define IXGBE_RXDADV_HDRBUFLEN_MASK	0x00007FE0
+#define IXGBE_RXDADV_RSCCNT_MASK	0x001E0000
+#define IXGBE_RXDADV_RSCCNT_SHIFT	17
+#define IXGBE_RXDADV_HDRBUFLEN_SHIFT	5
+#define IXGBE_RXDADV_SPLITHEADER_EN	0x00001000
+#define IXGBE_RXDADV_SPH		0x8000
 
 #define IXGBE_RXD_ERR_FRAME_ERR_MASK ( \
 				      IXGBE_RXD_ERR_CE |  \
@@ -176,16 +175,16 @@ typedef u32 ixgbe_link_speed;
 					 IXGBE_RXDADV_ERR_OSE | \
 					 IXGBE_RXDADV_ERR_USE)
 
-#define IXGBE_TXD_POPTS_IXSM 0x01       /* Insert IP checksum */
-#define IXGBE_TXD_POPTS_TXSM 0x02       /* Insert TCP/UDP checksum */
-#define IXGBE_TXD_CMD_EOP    0x01000000 /* End of Packet */
-#define IXGBE_TXD_CMD_IFCS   0x02000000 /* Insert FCS (Ethernet CRC) */
-#define IXGBE_TXD_CMD_IC     0x04000000 /* Insert Checksum */
-#define IXGBE_TXD_CMD_RS     0x08000000 /* Report Status */
-#define IXGBE_TXD_CMD_DEXT   0x20000000 /* Descriptor extension (0 = legacy) */
-#define IXGBE_TXD_CMD_VLE    0x40000000 /* Add VLAN tag */
-#define IXGBE_TXD_STAT_DD    0x00000001 /* Descriptor Done */
-#define IXGBE_TXD_CMD	     (IXGBE_TXD_CMD_EOP | IXGBE_TXD_CMD_RS)
+#define IXGBE_TXD_POPTS_IXSM	0x01       /* Insert IP checksum */
+#define IXGBE_TXD_POPTS_TXSM	0x02       /* Insert TCP/UDP checksum */
+#define IXGBE_TXD_CMD_EOP	0x01000000 /* End of Packet */
+#define IXGBE_TXD_CMD_IFCS	0x02000000 /* Insert FCS (Ethernet CRC) */
+#define IXGBE_TXD_CMD_IC	0x04000000 /* Insert Checksum */
+#define IXGBE_TXD_CMD_RS	0x08000000 /* Report Status */
+#define IXGBE_TXD_CMD_DEXT	0x20000000 /* Descriptor ext (0 = legacy) */
+#define IXGBE_TXD_CMD_VLE	0x40000000 /* Add VLAN tag */
+#define IXGBE_TXD_STAT_DD	0x00000001 /* Descriptor Done */
+#define IXGBE_TXD_CMD		(IXGBE_TXD_CMD_EOP | IXGBE_TXD_CMD_RS)
 
 /* Transmit Descriptor - Advanced */
 union ixgbe_adv_tx_desc {
@@ -241,44 +240,44 @@ struct ixgbe_adv_tx_context_desc {
 };
 
 /* Adv Transmit Descriptor Config Masks */
-#define IXGBE_ADVTXD_DTYP_MASK  0x00F00000 /* DTYP mask */
-#define IXGBE_ADVTXD_DTYP_CTXT  0x00200000 /* Advanced Context Desc */
-#define IXGBE_ADVTXD_DTYP_DATA  0x00300000 /* Advanced Data Descriptor */
-#define IXGBE_ADVTXD_DCMD_EOP   IXGBE_TXD_CMD_EOP  /* End of Packet */
-#define IXGBE_ADVTXD_DCMD_IFCS  IXGBE_TXD_CMD_IFCS /* Insert FCS */
-#define IXGBE_ADVTXD_DCMD_RS    IXGBE_TXD_CMD_RS   /* Report Status */
-#define IXGBE_ADVTXD_DCMD_DEXT  IXGBE_TXD_CMD_DEXT /* Desc ext (1=Adv) */
-#define IXGBE_ADVTXD_DCMD_VLE   IXGBE_TXD_CMD_VLE  /* VLAN pkt enable */
-#define IXGBE_ADVTXD_DCMD_TSE   0x80000000 /* TCP Seg enable */
-#define IXGBE_ADVTXD_STAT_DD    IXGBE_TXD_STAT_DD  /* Descriptor Done */
-#define IXGBE_ADVTXD_TUCMD_IPV4      0x00000400  /* IP Packet Type: 1=IPv4 */
-#define IXGBE_ADVTXD_TUCMD_IPV6      0x00000000  /* IP Packet Type: 0=IPv6 */
-#define IXGBE_ADVTXD_TUCMD_L4T_UDP   0x00000000  /* L4 Packet TYPE of UDP */
-#define IXGBE_ADVTXD_TUCMD_L4T_TCP   0x00000800  /* L4 Packet TYPE of TCP */
-#define IXGBE_ADVTXD_TUCMD_L4T_SCTP  0x00001000  /* L4 Packet TYPE of SCTP */
-#define IXGBE_ADVTXD_IDX_SHIFT  4 /* Adv desc Index shift */
+#define IXGBE_ADVTXD_DTYP_MASK	0x00F00000 /* DTYP mask */
+#define IXGBE_ADVTXD_DTYP_CTXT	0x00200000 /* Advanced Context Desc */
+#define IXGBE_ADVTXD_DTYP_DATA	0x00300000 /* Advanced Data Descriptor */
+#define IXGBE_ADVTXD_DCMD_EOP	IXGBE_TXD_CMD_EOP  /* End of Packet */
+#define IXGBE_ADVTXD_DCMD_IFCS	IXGBE_TXD_CMD_IFCS /* Insert FCS */
+#define IXGBE_ADVTXD_DCMD_RS	IXGBE_TXD_CMD_RS   /* Report Status */
+#define IXGBE_ADVTXD_DCMD_DEXT	IXGBE_TXD_CMD_DEXT /* Desc ext (1=Adv) */
+#define IXGBE_ADVTXD_DCMD_VLE	IXGBE_TXD_CMD_VLE  /* VLAN pkt enable */
+#define IXGBE_ADVTXD_DCMD_TSE	0x80000000 /* TCP Seg enable */
+#define IXGBE_ADVTXD_STAT_DD	IXGBE_TXD_STAT_DD  /* Descriptor Done */
+#define IXGBE_ADVTXD_TUCMD_IPV4	0x00000400  /* IP Packet Type: 1=IPv4 */
+#define IXGBE_ADVTXD_TUCMD_IPV6	0x00000000  /* IP Packet Type: 0=IPv6 */
+#define IXGBE_ADVTXD_TUCMD_L4T_UDP	0x00000000  /* L4 Packet TYPE of UDP */
+#define IXGBE_ADVTXD_TUCMD_L4T_TCP	0x00000800  /* L4 Packet TYPE of TCP */
+#define IXGBE_ADVTXD_TUCMD_L4T_SCTP	0x00001000  /* L4 Packet TYPE of SCTP */
+#define IXGBE_ADVTXD_IDX_SHIFT	4 /* Adv desc Index shift */
 #define IXGBE_ADVTXD_CC		0x00000080 /* Check Context */
-#define IXGBE_ADVTXD_POPTS_SHIFT      8  /* Adv desc POPTS shift */
-#define IXGBE_ADVTXD_POPTS_IXSM (IXGBE_TXD_POPTS_IXSM << \
+#define IXGBE_ADVTXD_POPTS_SHIFT	8  /* Adv desc POPTS shift */
+#define IXGBE_ADVTXD_POPTS_IXSM	(IXGBE_TXD_POPTS_IXSM << \
 				 IXGBE_ADVTXD_POPTS_SHIFT)
-#define IXGBE_ADVTXD_POPTS_TXSM (IXGBE_TXD_POPTS_TXSM << \
+#define IXGBE_ADVTXD_POPTS_TXSM	(IXGBE_TXD_POPTS_TXSM << \
 				 IXGBE_ADVTXD_POPTS_SHIFT)
-#define IXGBE_ADVTXD_PAYLEN_SHIFT    14 /* Adv desc PAYLEN shift */
-#define IXGBE_ADVTXD_MACLEN_SHIFT    9  /* Adv ctxt desc mac len shift */
-#define IXGBE_ADVTXD_VLAN_SHIFT      16  /* Adv ctxt vlan tag shift */
-#define IXGBE_ADVTXD_L4LEN_SHIFT     8  /* Adv ctxt L4LEN shift */
-#define IXGBE_ADVTXD_MSS_SHIFT       16  /* Adv ctxt MSS shift */
+#define IXGBE_ADVTXD_PAYLEN_SHIFT	14 /* Adv desc PAYLEN shift */
+#define IXGBE_ADVTXD_MACLEN_SHIFT	9  /* Adv ctxt desc mac len shift */
+#define IXGBE_ADVTXD_VLAN_SHIFT		16 /* Adv ctxt vlan tag shift */
+#define IXGBE_ADVTXD_L4LEN_SHIFT	8  /* Adv ctxt L4LEN shift */
+#define IXGBE_ADVTXD_MSS_SHIFT		16 /* Adv ctxt MSS shift */
 
 /* Interrupt register bitmasks */
 
-#define IXGBE_EITR_CNT_WDIS     0x80000000
+#define IXGBE_EITR_CNT_WDIS	0x80000000
 #define IXGBE_MAX_EITR		0x00000FF8
 #define IXGBE_MIN_EITR		8
 
 /* Error Codes */
-#define IXGBE_ERR_INVALID_MAC_ADDR              -1
-#define IXGBE_ERR_RESET_FAILED                  -2
-#define IXGBE_ERR_INVALID_ARGUMENT              -3
+#define IXGBE_ERR_INVALID_MAC_ADDR	-1
+#define IXGBE_ERR_RESET_FAILED		-2
+#define IXGBE_ERR_INVALID_ARGUMENT	-3
 
 /* Transmit Config masks */
 #define IXGBE_TXDCTL_ENABLE		0x02000000 /* Ena specific Tx Queue */
diff --git a/drivers/net/ethernet/intel/ixgbevf/ethtool.c b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
index cc0e5b7ff041..b2f5b161d792 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -100,6 +99,7 @@ static const char ixgbe_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Register test  (offline)",
 	"Link test   (on/offline)"
 };
+
 #define IXGBE_TEST_LEN (sizeof(ixgbe_gstrings_test) / ETH_GSTRING_LEN)
 
 static int ixgbevf_get_settings(struct net_device *netdev,
@@ -120,6 +120,7 @@ static int ixgbevf_get_settings(struct net_device *netdev,
 
 	if (link_up) {
 		__u32 speed = SPEED_10000;
+
 		switch (link_speed) {
 		case IXGBE_LINK_SPEED_10GB_FULL:
 			speed = SPEED_10000;
@@ -145,12 +146,14 @@ static int ixgbevf_get_settings(struct net_device *netdev,
 static u32 ixgbevf_get_msglevel(struct net_device *netdev)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+
 	return adapter->msg_enable;
 }
 
 static void ixgbevf_set_msglevel(struct net_device *netdev, u32 data)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+
 	adapter->msg_enable = data;
 }
 
@@ -185,7 +188,8 @@ static void ixgbevf_get_regs(struct net_device *netdev,
 
 	/* Interrupt */
 	/* don't read EICR because it can clear interrupt causes, instead
-	 * read EICS which is a shadow but doesn't clear EICR */
+	 * read EICS which is a shadow but doesn't clear EICR
+	 */
 	regs_buff[5] = IXGBE_READ_REG(hw, IXGBE_VTEICS);
 	regs_buff[6] = IXGBE_READ_REG(hw, IXGBE_VTEICS);
 	regs_buff[7] = IXGBE_READ_REG(hw, IXGBE_VTEIMS);
@@ -390,21 +394,21 @@ clear_reset:
 
 static int ixgbevf_get_sset_count(struct net_device *dev, int stringset)
 {
-       switch (stringset) {
-       case ETH_SS_TEST:
-	       return IXGBE_TEST_LEN;
-       case ETH_SS_STATS:
-	       return IXGBE_GLOBAL_STATS_LEN;
-       default:
-	       return -EINVAL;
-       }
+	switch (stringset) {
+	case ETH_SS_TEST:
+		return IXGBE_TEST_LEN;
+	case ETH_SS_STATS:
+		return IXGBE_GLOBAL_STATS_LEN;
+	default:
+		return -EINVAL;
+	}
 }
 
 static void ixgbevf_get_ethtool_stats(struct net_device *netdev,
 				      struct ethtool_stats *stats, u64 *data)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
-	char *base = (char *) adapter;
+	char *base = (char *)adapter;
 	int i;
 #ifdef BP_EXTENDED_STATS
 	u64 rx_yields = 0, rx_cleaned = 0, rx_missed = 0,
@@ -594,8 +598,7 @@ static int ixgbevf_reg_test(struct ixgbevf_adapter *adapter, u64 *data)
 	}
 	test = reg_test_vf;
 
-	/*
-	 * Perform the register test, looping through the test table
+	/* Perform the register test, looping through the test table
 	 * until we either fail or reach the null entry.
 	 */
 	while (test->reg) {
@@ -617,8 +620,8 @@ static int ixgbevf_reg_test(struct ixgbevf_adapter *adapter, u64 *data)
 				break;
 			case WRITE_NO_TEST:
 				ixgbe_write_reg(&adapter->hw,
-						  test->reg + (i * 0x40),
-						  test->write);
+						test->reg + (i * 0x40),
+						test->write);
 				break;
 			case TABLE32_TEST:
 				b = reg_pattern_test(adapter, data,
@@ -670,7 +673,8 @@ static void ixgbevf_diag_test(struct net_device *netdev,
 		hw_dbg(&adapter->hw, "offline testing starting\n");
 
 		/* Link test performed before hardware reset so autoneg doesn't
-		 * interfere with test result */
+		 * interfere with test result
+		 */
 		if (ixgbevf_link_test(adapter, &data[1]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
@@ -724,7 +728,7 @@ static int ixgbevf_get_coalesce(struct net_device *netdev,
 	else
 		ec->rx_coalesce_usecs = adapter->rx_itr_setting >> 2;
 
-	/* if in mixed tx/rx queues per vector mode, report only rx settings */
+	/* if in mixed Tx/Rx queues per vector mode, report only Rx settings */
 	if (adapter->q_vector[0]->tx.count && adapter->q_vector[0]->rx.count)
 		return 0;
 
@@ -745,12 +749,11 @@ static int ixgbevf_set_coalesce(struct net_device *netdev,
 	int num_vectors, i;
 	u16 tx_itr_param, rx_itr_param;
 
-	/* don't accept tx specific changes if we've got mixed RxTx vectors */
-	if (adapter->q_vector[0]->tx.count && adapter->q_vector[0]->rx.count
-	    && ec->tx_coalesce_usecs)
+	/* don't accept Tx specific changes if we've got mixed RxTx vectors */
+	if (adapter->q_vector[0]->tx.count &&
+	    adapter->q_vector[0]->rx.count && ec->tx_coalesce_usecs)
 		return -EINVAL;
 
-
 	if ((ec->rx_coalesce_usecs > (IXGBE_MAX_EITR >> 2)) ||
 	    (ec->tx_coalesce_usecs > (IXGBE_MAX_EITR >> 2)))
 		return -EINVAL;
@@ -765,7 +768,6 @@ static int ixgbevf_set_coalesce(struct net_device *netdev,
 	else
 		rx_itr_param = adapter->rx_itr_setting;
 
-
 	if (ec->tx_coalesce_usecs > 1)
 		adapter->tx_itr_setting = ec->tx_coalesce_usecs << 2;
 	else
@@ -781,10 +783,10 @@ static int ixgbevf_set_coalesce(struct net_device *netdev,
 	for (i = 0; i < num_vectors; i++) {
 		q_vector = adapter->q_vector[i];
 		if (q_vector->tx.count && !q_vector->rx.count)
-			/* tx only */
+			/* Tx only */
 			q_vector->itr = tx_itr_param;
 		else
-			/* rx only or mixed */
+			/* Rx only or mixed */
 			q_vector->itr = rx_itr_param;
 		ixgbevf_write_eitr(q_vector);
 	}
@@ -792,23 +794,92 @@ static int ixgbevf_set_coalesce(struct net_device *netdev,
 	return 0;
 }
 
+static int ixgbevf_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
+			     u32 *rules __always_unused)
+{
+	struct ixgbevf_adapter *adapter = netdev_priv(dev);
+
+	switch (info->cmd) {
+	case ETHTOOL_GRXRINGS:
+		info->data = adapter->num_rx_queues;
+		return 0;
+	default:
+		hw_dbg(&adapter->hw, "Command parameters not supported\n");
+		return -EOPNOTSUPP;
+	}
+}
+
+static u32 ixgbevf_get_rxfh_indir_size(struct net_device *netdev)
+{
+	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+
+	/* We support this operation only for 82599 and x540 at the moment */
+	if (adapter->hw.mac.type < ixgbe_mac_X550_vf)
+		return IXGBEVF_82599_RETA_SIZE;
+
+	return 0;
+}
+
+static u32 ixgbevf_get_rxfh_key_size(struct net_device *netdev)
+{
+	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+
+	/* We support this operation only for 82599 and x540 at the moment */
+	if (adapter->hw.mac.type < ixgbe_mac_X550_vf)
+		return IXGBEVF_RSS_HASH_KEY_SIZE;
+
+	return 0;
+}
+
+static int ixgbevf_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
+			    u8 *hfunc)
+{
+	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+	int err = 0;
+
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+
+	/* If neither indirection table nor hash key was requested - just
+	 * return a success avoiding taking any locks.
+	 */
+	if (!indir && !key)
+		return 0;
+
+	spin_lock_bh(&adapter->mbx_lock);
+	if (indir)
+		err = ixgbevf_get_reta_locked(&adapter->hw, indir,
+					      adapter->num_rx_queues);
+
+	if (!err && key)
+		err = ixgbevf_get_rss_key_locked(&adapter->hw, key);
+
+	spin_unlock_bh(&adapter->mbx_lock);
+
+	return err;
+}
+
 static const struct ethtool_ops ixgbevf_ethtool_ops = {
-	.get_settings           = ixgbevf_get_settings,
-	.get_drvinfo            = ixgbevf_get_drvinfo,
-	.get_regs_len           = ixgbevf_get_regs_len,
-	.get_regs               = ixgbevf_get_regs,
-	.nway_reset             = ixgbevf_nway_reset,
-	.get_link               = ethtool_op_get_link,
-	.get_ringparam          = ixgbevf_get_ringparam,
-	.set_ringparam          = ixgbevf_set_ringparam,
-	.get_msglevel           = ixgbevf_get_msglevel,
-	.set_msglevel           = ixgbevf_set_msglevel,
-	.self_test              = ixgbevf_diag_test,
-	.get_sset_count         = ixgbevf_get_sset_count,
-	.get_strings            = ixgbevf_get_strings,
-	.get_ethtool_stats      = ixgbevf_get_ethtool_stats,
-	.get_coalesce           = ixgbevf_get_coalesce,
-	.set_coalesce           = ixgbevf_set_coalesce,
+	.get_settings		= ixgbevf_get_settings,
+	.get_drvinfo		= ixgbevf_get_drvinfo,
+	.get_regs_len		= ixgbevf_get_regs_len,
+	.get_regs		= ixgbevf_get_regs,
+	.nway_reset		= ixgbevf_nway_reset,
+	.get_link		= ethtool_op_get_link,
+	.get_ringparam		= ixgbevf_get_ringparam,
+	.set_ringparam		= ixgbevf_set_ringparam,
+	.get_msglevel		= ixgbevf_get_msglevel,
+	.set_msglevel		= ixgbevf_set_msglevel,
+	.self_test		= ixgbevf_diag_test,
+	.get_sset_count		= ixgbevf_get_sset_count,
+	.get_strings		= ixgbevf_get_strings,
+	.get_ethtool_stats	= ixgbevf_get_ethtool_stats,
+	.get_coalesce		= ixgbevf_get_coalesce,
+	.set_coalesce		= ixgbevf_set_coalesce,
+	.get_rxnfc		= ixgbevf_get_rxnfc,
+	.get_rxfh_indir_size	= ixgbevf_get_rxfh_indir_size,
+	.get_rxfh_key_size	= ixgbevf_get_rxfh_key_size,
+	.get_rxfh		= ixgbevf_get_rxfh,
 };
 
 void ixgbevf_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index 3a9b356dff01..775d08900949 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -51,7 +50,8 @@
 #define DESC_NEEDED (MAX_SKB_FRAGS + 4)
 
 /* wrapper around a pointer to a socket buffer,
- * so a DMA handle can be stored along with the buffer */
+ * so a DMA handle can be stored along with the buffer
+ */
 struct ixgbevf_tx_buffer {
 	union ixgbe_adv_tx_desc *next_to_watch;
 	unsigned long time_stamp;
@@ -132,9 +132,10 @@ struct ixgbevf_ring {
 	u8 __iomem *tail;
 	struct sk_buff *skb;
 
-	u16 reg_idx; /* holds the special value that gets the hardware register
-		      * offset associated with this ring, which is different
-		      * for DCB and RSS modes */
+	/* holds the special value that gets the hardware register offset
+	 * associated with this ring, which is different for DCB and RSS modes
+	 */
+	u16 reg_idx;
 	int queue_index; /* needed for multiqueue queue management */
 };
 
@@ -143,21 +144,23 @@ struct ixgbevf_ring {
 
 #define MAX_RX_QUEUES IXGBE_VF_MAX_RX_QUEUES
 #define MAX_TX_QUEUES IXGBE_VF_MAX_TX_QUEUES
-#define IXGBEVF_MAX_RSS_QUEUES 2
+#define IXGBEVF_MAX_RSS_QUEUES	2
+#define IXGBEVF_82599_RETA_SIZE	128
+#define IXGBEVF_RSS_HASH_KEY_SIZE	40
 
-#define IXGBEVF_DEFAULT_TXD   1024
-#define IXGBEVF_DEFAULT_RXD   512
-#define IXGBEVF_MAX_TXD       4096
-#define IXGBEVF_MIN_TXD       64
-#define IXGBEVF_MAX_RXD       4096
-#define IXGBEVF_MIN_RXD       64
+#define IXGBEVF_DEFAULT_TXD	1024
+#define IXGBEVF_DEFAULT_RXD	512
+#define IXGBEVF_MAX_TXD		4096
+#define IXGBEVF_MIN_TXD		64
+#define IXGBEVF_MAX_RXD		4096
+#define IXGBEVF_MIN_RXD		64
 
 /* Supported Rx Buffer Sizes */
-#define IXGBEVF_RXBUFFER_256   256    /* Used for packet split */
-#define IXGBEVF_RXBUFFER_2048  2048
+#define IXGBEVF_RXBUFFER_256	256    /* Used for packet split */
+#define IXGBEVF_RXBUFFER_2048	2048
 
-#define IXGBEVF_RX_HDR_SIZE IXGBEVF_RXBUFFER_256
-#define IXGBEVF_RX_BUFSZ    IXGBEVF_RXBUFFER_2048
+#define IXGBEVF_RX_HDR_SIZE	IXGBEVF_RXBUFFER_256
+#define IXGBEVF_RX_BUFSZ	IXGBEVF_RXBUFFER_2048
 
 #define MAXIMUM_ETHERNET_VLAN_SIZE (VLAN_ETH_FRAME_LEN + ETH_FCS_LEN)
 
@@ -186,10 +189,11 @@ struct ixgbevf_ring_container {
  */
 struct ixgbevf_q_vector {
 	struct ixgbevf_adapter *adapter;
-	u16 v_idx;		/* index of q_vector within array, also used for
-				 * finding the bit in EICR and friends that
-				 * represents the vector for this ring */
-	u16 itr;		/* Interrupt throttle rate written to EITR */
+	/* index of q_vector within array, also used for finding the bit in
+	 * EICR and friends that represents the vector for this ring
+	 */
+	u16 v_idx;
+	u16 itr; /* Interrupt throttle rate written to EITR */
 	struct napi_struct napi;
 	struct ixgbevf_ring_container rx, tx;
 	char name[IFNAMSIZ + 9];
@@ -199,19 +203,21 @@ struct ixgbevf_q_vector {
 #define IXGBEVF_QV_STATE_NAPI		1    /* NAPI owns this QV */
 #define IXGBEVF_QV_STATE_POLL		2    /* poll owns this QV */
 #define IXGBEVF_QV_STATE_DISABLED	4    /* QV is disabled */
-#define IXGBEVF_QV_OWNED (IXGBEVF_QV_STATE_NAPI | IXGBEVF_QV_STATE_POLL)
-#define IXGBEVF_QV_LOCKED (IXGBEVF_QV_OWNED | IXGBEVF_QV_STATE_DISABLED)
+#define IXGBEVF_QV_OWNED	(IXGBEVF_QV_STATE_NAPI | IXGBEVF_QV_STATE_POLL)
+#define IXGBEVF_QV_LOCKED	(IXGBEVF_QV_OWNED | IXGBEVF_QV_STATE_DISABLED)
 #define IXGBEVF_QV_STATE_NAPI_YIELD	8    /* NAPI yielded this QV */
 #define IXGBEVF_QV_STATE_POLL_YIELD	16   /* poll yielded this QV */
-#define IXGBEVF_QV_YIELD (IXGBEVF_QV_STATE_NAPI_YIELD | IXGBEVF_QV_STATE_POLL_YIELD)
-#define IXGBEVF_QV_USER_PEND (IXGBEVF_QV_STATE_POLL | IXGBEVF_QV_STATE_POLL_YIELD)
+#define IXGBEVF_QV_YIELD	(IXGBEVF_QV_STATE_NAPI_YIELD | \
+				 IXGBEVF_QV_STATE_POLL_YIELD)
+#define IXGBEVF_QV_USER_PEND	(IXGBEVF_QV_STATE_POLL | \
+				 IXGBEVF_QV_STATE_POLL_YIELD)
 	spinlock_t lock;
 #endif /* CONFIG_NET_RX_BUSY_POLL */
 };
+
 #ifdef CONFIG_NET_RX_BUSY_POLL
 static inline void ixgbevf_qv_init_lock(struct ixgbevf_q_vector *q_vector)
 {
-
 	spin_lock_init(&q_vector->lock);
 	q_vector->state = IXGBEVF_QV_STATE_IDLE;
 }
@@ -220,6 +226,7 @@ static inline void ixgbevf_qv_init_lock(struct ixgbevf_q_vector *q_vector)
 static inline bool ixgbevf_qv_lock_napi(struct ixgbevf_q_vector *q_vector)
 {
 	int rc = true;
+
 	spin_lock_bh(&q_vector->lock);
 	if (q_vector->state & IXGBEVF_QV_LOCKED) {
 		WARN_ON(q_vector->state & IXGBEVF_QV_STATE_NAPI);
@@ -240,6 +247,7 @@ static inline bool ixgbevf_qv_lock_napi(struct ixgbevf_q_vector *q_vector)
 static inline bool ixgbevf_qv_unlock_napi(struct ixgbevf_q_vector *q_vector)
 {
 	int rc = false;
+
 	spin_lock_bh(&q_vector->lock);
 	WARN_ON(q_vector->state & (IXGBEVF_QV_STATE_POLL |
 				   IXGBEVF_QV_STATE_NAPI_YIELD));
@@ -256,6 +264,7 @@ static inline bool ixgbevf_qv_unlock_napi(struct ixgbevf_q_vector *q_vector)
 static inline bool ixgbevf_qv_lock_poll(struct ixgbevf_q_vector *q_vector)
 {
 	int rc = true;
+
 	spin_lock_bh(&q_vector->lock);
 	if ((q_vector->state & IXGBEVF_QV_LOCKED)) {
 		q_vector->state |= IXGBEVF_QV_STATE_POLL_YIELD;
@@ -275,6 +284,7 @@ static inline bool ixgbevf_qv_lock_poll(struct ixgbevf_q_vector *q_vector)
 static inline bool ixgbevf_qv_unlock_poll(struct ixgbevf_q_vector *q_vector)
 {
 	int rc = false;
+
 	spin_lock_bh(&q_vector->lock);
 	WARN_ON(q_vector->state & (IXGBEVF_QV_STATE_NAPI));
 
@@ -297,6 +307,7 @@ static inline bool ixgbevf_qv_busy_polling(struct ixgbevf_q_vector *q_vector)
 static inline bool ixgbevf_qv_disable(struct ixgbevf_q_vector *q_vector)
 {
 	int rc = true;
+
 	spin_lock_bh(&q_vector->lock);
 	if (q_vector->state & IXGBEVF_QV_OWNED)
 		rc = false;
@@ -307,8 +318,7 @@ static inline bool ixgbevf_qv_disable(struct ixgbevf_q_vector *q_vector)
 
 #endif /* CONFIG_NET_RX_BUSY_POLL */
 
-/*
- * microsecond values for various ITR rates shifted by 2 to fit itr register
+/* microsecond values for various ITR rates shifted by 2 to fit itr register
  * with the first 3 bits reserved 0
  */
 #define IXGBE_MIN_RSC_ITR	24
@@ -345,22 +355,22 @@ static inline void ixgbevf_write_tail(struct ixgbevf_ring *ring, u32 value)
 	writel(value, ring->tail);
 }
 
-#define IXGBEVF_RX_DESC(R, i)	    \
+#define IXGBEVF_RX_DESC(R, i)	\
 	(&(((union ixgbe_adv_rx_desc *)((R)->desc))[i]))
-#define IXGBEVF_TX_DESC(R, i)	    \
+#define IXGBEVF_TX_DESC(R, i)	\
 	(&(((union ixgbe_adv_tx_desc *)((R)->desc))[i]))
-#define IXGBEVF_TX_CTXTDESC(R, i)	    \
+#define IXGBEVF_TX_CTXTDESC(R, i)	\
 	(&(((struct ixgbe_adv_tx_context_desc *)((R)->desc))[i]))
 
 #define IXGBE_MAX_JUMBO_FRAME_SIZE	9728 /* Maximum Supported Size 9.5KB */
 
-#define OTHER_VECTOR 1
-#define NON_Q_VECTORS (OTHER_VECTOR)
+#define OTHER_VECTOR	1
+#define NON_Q_VECTORS	(OTHER_VECTOR)
 
-#define MAX_MSIX_Q_VECTORS 2
+#define MAX_MSIX_Q_VECTORS	2
 
-#define MIN_MSIX_Q_VECTORS 1
-#define MIN_MSIX_COUNT (MIN_MSIX_Q_VECTORS + NON_Q_VECTORS)
+#define MIN_MSIX_Q_VECTORS	1
+#define MIN_MSIX_COUNT		(MIN_MSIX_Q_VECTORS + NON_Q_VECTORS)
 
 /* board specific private data structure */
 struct ixgbevf_adapter {
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index 4186981e562d..a16d267fbce4 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -25,7 +24,6 @@
 
 *******************************************************************************/
 
-
 /******************************************************************************
  Copyright (c)2006 - 2007 Myricom, Inc. for some LRO specific code
 ******************************************************************************/
@@ -170,12 +168,13 @@ u32 ixgbevf_read_reg(struct ixgbe_hw *hw, u32 reg)
  * @direction: 0 for Rx, 1 for Tx, -1 for other causes
  * @queue: queue to map the corresponding interrupt to
  * @msix_vector: the vector to map to the corresponding queue
- */
+ **/
 static void ixgbevf_set_ivar(struct ixgbevf_adapter *adapter, s8 direction,
 			     u8 queue, u8 msix_vector)
 {
 	u32 ivar, index;
 	struct ixgbe_hw *hw = &adapter->hw;
+
 	if (direction == -1) {
 		/* other causes */
 		msix_vector |= IXGBE_IVAR_ALLOC_VAL;
@@ -184,7 +183,7 @@ static void ixgbevf_set_ivar(struct ixgbevf_adapter *adapter, s8 direction,
 		ivar |= msix_vector;
 		IXGBE_WRITE_REG(hw, IXGBE_VTIVAR_MISC, ivar);
 	} else {
-		/* tx or rx causes */
+		/* Tx or Rx causes */
 		msix_vector |= IXGBE_IVAR_ALLOC_VAL;
 		index = ((16 * (queue & 1)) + (8 * direction));
 		ivar = IXGBE_READ_REG(hw, IXGBE_VTIVAR(queue >> 1));
@@ -458,11 +457,12 @@ static void ixgbevf_rx_skb(struct ixgbevf_q_vector *q_vector,
 	napi_gro_receive(&q_vector->napi, skb);
 }
 
-/* ixgbevf_rx_checksum - indicate in skb if hw indicated a good cksum
+/**
+ * ixgbevf_rx_checksum - indicate in skb if hw indicated a good cksum
  * @ring: structure containig ring specific data
  * @rx_desc: current Rx descriptor being processed
  * @skb: skb currently being received and modified
- */
+ **/
 static inline void ixgbevf_rx_checksum(struct ixgbevf_ring *ring,
 				       union ixgbe_adv_rx_desc *rx_desc,
 				       struct sk_buff *skb)
@@ -492,7 +492,8 @@ static inline void ixgbevf_rx_checksum(struct ixgbevf_ring *ring,
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 }
 
-/* ixgbevf_process_skb_fields - Populate skb header fields from Rx descriptor
+/**
+ * ixgbevf_process_skb_fields - Populate skb header fields from Rx descriptor
  * @rx_ring: rx descriptor ring packet is being transacted on
  * @rx_desc: pointer to the EOP Rx descriptor
  * @skb: pointer to current skb being populated
@@ -500,7 +501,7 @@ static inline void ixgbevf_rx_checksum(struct ixgbevf_ring *ring,
  * This function checks the ring, descriptor, and packet information in
  * order to populate the checksum, VLAN, protocol, and other fields within
  * the skb.
- */
+ **/
 static void ixgbevf_process_skb_fields(struct ixgbevf_ring *rx_ring,
 				       union ixgbe_adv_rx_desc *rx_desc,
 				       struct sk_buff *skb)
@@ -647,7 +648,8 @@ static void ixgbevf_alloc_rx_buffers(struct ixgbevf_ring *rx_ring,
 	}
 }
 
-/* ixgbevf_pull_tail - ixgbevf specific version of skb_pull_tail
+/**
+ * ixgbevf_pull_tail - ixgbevf specific version of skb_pull_tail
  * @rx_ring: rx descriptor ring packet is being transacted on
  * @skb: pointer to current skb being adjusted
  *
@@ -657,7 +659,7 @@ static void ixgbevf_alloc_rx_buffers(struct ixgbevf_ring *rx_ring,
  * that allow for significant optimizations versus the standard function.
  * As a result we can do things like drop a frag and maintain an accurate
  * truesize for the skb.
- */
+ **/
 static void ixgbevf_pull_tail(struct ixgbevf_ring *rx_ring,
 			      struct sk_buff *skb)
 {
@@ -686,7 +688,8 @@ static void ixgbevf_pull_tail(struct ixgbevf_ring *rx_ring,
 	skb->tail += pull_len;
 }
 
-/* ixgbevf_cleanup_headers - Correct corrupted or empty headers
+/**
+ * ixgbevf_cleanup_headers - Correct corrupted or empty headers
  * @rx_ring: rx descriptor ring packet is being transacted on
  * @rx_desc: pointer to the EOP Rx descriptor
  * @skb: pointer to current skb being fixed
@@ -702,7 +705,7 @@ static void ixgbevf_pull_tail(struct ixgbevf_ring *rx_ring,
  * it is large enough to qualify as a valid Ethernet frame.
  *
  * Returns true if an error was encountered and skb was freed.
- */
+ **/
 static bool ixgbevf_cleanup_headers(struct ixgbevf_ring *rx_ring,
 				    union ixgbe_adv_rx_desc *rx_desc,
 				    struct sk_buff *skb)
@@ -729,12 +732,13 @@ static bool ixgbevf_cleanup_headers(struct ixgbevf_ring *rx_ring,
 	return false;
 }
 
-/* ixgbevf_reuse_rx_page - page flip buffer and store it back on the ring
+/**
+ * ixgbevf_reuse_rx_page - page flip buffer and store it back on the ring
  * @rx_ring: rx descriptor ring to store buffers on
  * @old_buff: donor buffer to have page reused
  *
  * Synchronizes page for reuse by the adapter
- */
+ **/
 static void ixgbevf_reuse_rx_page(struct ixgbevf_ring *rx_ring,
 				  struct ixgbevf_rx_buffer *old_buff)
 {
@@ -764,7 +768,8 @@ static inline bool ixgbevf_page_is_reserved(struct page *page)
 	return (page_to_nid(page) != numa_mem_id()) || page->pfmemalloc;
 }
 
-/* ixgbevf_add_rx_frag - Add contents of Rx buffer to sk_buff
+/**
+ * ixgbevf_add_rx_frag - Add contents of Rx buffer to sk_buff
  * @rx_ring: rx descriptor ring to transact packets on
  * @rx_buffer: buffer containing page to add
  * @rx_desc: descriptor containing length of buffer written by hardware
@@ -777,7 +782,7 @@ static inline bool ixgbevf_page_is_reserved(struct page *page)
  *
  * The function will then update the page offset if necessary and return
  * true if the buffer can be reused by the adapter.
- */
+ **/
 static bool ixgbevf_add_rx_frag(struct ixgbevf_ring *rx_ring,
 				struct ixgbevf_rx_buffer *rx_buffer,
 				union ixgbe_adv_rx_desc *rx_desc,
@@ -958,7 +963,7 @@ static int ixgbevf_clean_rx_irq(struct ixgbevf_q_vector *q_vector,
 		 * source pruning.
 		 */
 		if ((skb->pkt_type == PACKET_BROADCAST ||
-		    skb->pkt_type == PACKET_MULTICAST) &&
+		     skb->pkt_type == PACKET_MULTICAST) &&
 		    ether_addr_equal(rx_ring->netdev->dev_addr,
 				     eth_hdr(skb)->h_source)) {
 			dev_kfree_skb_irq(skb);
@@ -1016,7 +1021,8 @@ static int ixgbevf_poll(struct napi_struct *napi, int budget)
 #endif
 
 	/* attempt to distribute budget to each queue fairly, but don't allow
-	 * the budget to go below 1 because we'll exit polling */
+	 * the budget to go below 1 because we'll exit polling
+	 */
 	if (q_vector->rx.count > 1)
 		per_ring_budget = max(budget/q_vector->rx.count, 1);
 	else
@@ -1049,7 +1055,7 @@ static int ixgbevf_poll(struct napi_struct *napi, int budget)
 /**
  * ixgbevf_write_eitr - write VTEITR register in hardware specific way
  * @q_vector: structure containing interrupt and ring information
- */
+ **/
 void ixgbevf_write_eitr(struct ixgbevf_q_vector *q_vector)
 {
 	struct ixgbevf_adapter *adapter = q_vector->adapter;
@@ -1057,8 +1063,7 @@ void ixgbevf_write_eitr(struct ixgbevf_q_vector *q_vector)
 	int v_idx = q_vector->v_idx;
 	u32 itr_reg = q_vector->itr & IXGBE_MAX_EITR;
 
-	/*
-	 * set the WDIS bit to not clear the timer bits and cause an
+	/* set the WDIS bit to not clear the timer bits and cause an
 	 * immediate assertion of the interrupt
 	 */
 	itr_reg |= IXGBE_EITR_CNT_WDIS;
@@ -1115,12 +1120,12 @@ static void ixgbevf_configure_msix(struct ixgbevf_adapter *adapter)
 	q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
 	adapter->eims_enable_mask = 0;
 
-	/*
-	 * Populate the IVAR table and set the ITR values to the
+	/* Populate the IVAR table and set the ITR values to the
 	 * corresponding register.
 	 */
 	for (v_idx = 0; v_idx < q_vectors; v_idx++) {
 		struct ixgbevf_ring *ring;
+
 		q_vector = adapter->q_vector[v_idx];
 
 		ixgbevf_for_each_ring(ring, q_vector->rx)
@@ -1130,13 +1135,13 @@ static void ixgbevf_configure_msix(struct ixgbevf_adapter *adapter)
 			ixgbevf_set_ivar(adapter, 1, ring->reg_idx, v_idx);
 
 		if (q_vector->tx.ring && !q_vector->rx.ring) {
-			/* tx only vector */
+			/* Tx only vector */
 			if (adapter->tx_itr_setting == 1)
 				q_vector->itr = IXGBE_10K_ITR;
 			else
 				q_vector->itr = adapter->tx_itr_setting;
 		} else {
-			/* rx or rx/tx vector */
+			/* Rx or Rx/Tx vector */
 			if (adapter->rx_itr_setting == 1)
 				q_vector->itr = IXGBE_20K_ITR;
 			else
@@ -1167,13 +1172,13 @@ enum latency_range {
  * @q_vector: structure containing interrupt and ring information
  * @ring_container: structure containing ring performance data
  *
- *      Stores a new ITR value based on packets and byte
- *      counts during the last interrupt.  The advantage of per interrupt
- *      computation is faster updates and more accurate ITR for the current
- *      traffic pattern.  Constants in this function were computed
- *      based on theoretical maximum wire speed and thresholds were set based
- *      on testing data as well as attempting to minimize response time
- *      while increasing bulk throughput.
+ * Stores a new ITR value based on packets and byte
+ * counts during the last interrupt.  The advantage of per interrupt
+ * computation is faster updates and more accurate ITR for the current
+ * traffic pattern.  Constants in this function were computed
+ * based on theoretical maximum wire speed and thresholds were set based
+ * on testing data as well as attempting to minimize response time
+ * while increasing bulk throughput.
  **/
 static void ixgbevf_update_itr(struct ixgbevf_q_vector *q_vector,
 			       struct ixgbevf_ring_container *ring_container)
@@ -1187,7 +1192,7 @@ static void ixgbevf_update_itr(struct ixgbevf_q_vector *q_vector,
 	if (packets == 0)
 		return;
 
-	/* simple throttlerate management
+	/* simple throttle rate management
 	 *    0-20MB/s lowest (100000 ints/s)
 	 *   20-100MB/s low   (20000 ints/s)
 	 *  100-1249MB/s bulk (8000 ints/s)
@@ -1330,8 +1335,7 @@ static int ixgbevf_map_rings_to_vectors(struct ixgbevf_adapter *adapter)
 
 	q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
 
-	/*
-	 * The ideal configuration...
+	/* The ideal configuration...
 	 * We have enough vectors to map one per queue.
 	 */
 	if (q_vectors == adapter->num_rx_queues + adapter->num_tx_queues) {
@@ -1343,8 +1347,7 @@ static int ixgbevf_map_rings_to_vectors(struct ixgbevf_adapter *adapter)
 		goto out;
 	}
 
-	/*
-	 * If we don't have enough vectors for a 1-to-1
+	/* If we don't have enough vectors for a 1-to-1
 	 * mapping, we'll have to group them so there are
 	 * multiple queues per vector.
 	 */
@@ -1406,8 +1409,8 @@ static int ixgbevf_request_msix_irqs(struct ixgbevf_adapter *adapter)
 				  q_vector->name, q_vector);
 		if (err) {
 			hw_dbg(&adapter->hw,
-			       "request_irq failed for MSIX interrupt "
-			       "Error: %d\n", err);
+			       "request_irq failed for MSIX interrupt Error: %d\n",
+			       err);
 			goto free_queue_irqs;
 		}
 	}
@@ -1415,8 +1418,8 @@ static int ixgbevf_request_msix_irqs(struct ixgbevf_adapter *adapter)
 	err = request_irq(adapter->msix_entries[vector].vector,
 			  &ixgbevf_msix_other, 0, netdev->name, adapter);
 	if (err) {
-		hw_dbg(&adapter->hw,
-		       "request_irq for msix_other failed: %d\n", err);
+		hw_dbg(&adapter->hw, "request_irq for msix_other failed: %d\n",
+		       err);
 		goto free_queue_irqs;
 	}
 
@@ -1448,6 +1451,7 @@ static inline void ixgbevf_reset_q_vectors(struct ixgbevf_adapter *adapter)
 
 	for (i = 0; i < q_vectors; i++) {
 		struct ixgbevf_q_vector *q_vector = adapter->q_vector[i];
+
 		q_vector->rx.ring = NULL;
 		q_vector->tx.ring = NULL;
 		q_vector->rx.count = 0;
@@ -1469,8 +1473,7 @@ static int ixgbevf_request_irq(struct ixgbevf_adapter *adapter)
 	err = ixgbevf_request_msix_irqs(adapter);
 
 	if (err)
-		hw_dbg(&adapter->hw,
-		       "request_irq failed, Error %d\n", err);
+		hw_dbg(&adapter->hw, "request_irq failed, Error %d\n", err);
 
 	return err;
 }
@@ -1659,7 +1662,7 @@ static void ixgbevf_disable_rx_queue(struct ixgbevf_adapter *adapter,
 	/* write value back with RXDCTL.ENABLE bit cleared */
 	IXGBE_WRITE_REG(hw, IXGBE_VFRXDCTL(reg_idx), rxdctl);
 
-	/* the hardware may take up to 100us to really disable the rx queue */
+	/* the hardware may take up to 100us to really disable the Rx queue */
 	do {
 		udelay(10);
 		rxdctl = IXGBE_READ_REG(hw, IXGBE_VFRXDCTL(reg_idx));
@@ -1786,7 +1789,8 @@ static void ixgbevf_configure_rx(struct ixgbevf_adapter *adapter)
 	ixgbevf_rlpml_set_vf(hw, netdev->mtu + ETH_HLEN + ETH_FCS_LEN);
 
 	/* Setup the HW Rx Head and Tail Descriptor Pointers and
-	 * the Base and Length of the Rx Descriptor Ring */
+	 * the Base and Length of the Rx Descriptor Ring
+	 */
 	for (i = 0; i < adapter->num_rx_queues; i++)
 		ixgbevf_configure_rx_ring(adapter, adapter->rx_ring[i]);
 }
@@ -1858,14 +1862,14 @@ static int ixgbevf_write_uc_addr_list(struct net_device *netdev)
 
 	if (!netdev_uc_empty(netdev)) {
 		struct netdev_hw_addr *ha;
+
 		netdev_for_each_uc_addr(ha, netdev) {
 			hw->mac.ops.set_uc_addr(hw, ++count, ha->addr);
 			udelay(200);
 		}
 	} else {
-		/*
-		 * If the list is empty then send message to PF driver to
-		 * clear all macvlans on this VF.
+		/* If the list is empty then send message to PF driver to
+		 * clear all MAC VLANs on this VF.
 		 */
 		hw->mac.ops.set_uc_addr(hw, 0, NULL);
 	}
@@ -2026,7 +2030,8 @@ static void ixgbevf_init_last_counter_stats(struct ixgbevf_adapter *adapter)
 static void ixgbevf_negotiate_api(struct ixgbevf_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	int api[] = { ixgbe_mbox_api_11,
+	int api[] = { ixgbe_mbox_api_12,
+		      ixgbe_mbox_api_11,
 		      ixgbe_mbox_api_10,
 		      ixgbe_mbox_api_unknown };
 	int err = 0, idx = 0;
@@ -2184,7 +2189,7 @@ void ixgbevf_down(struct ixgbevf_adapter *adapter)
 	if (test_and_set_bit(__IXGBEVF_DOWN, &adapter->state))
 		return; /* do nothing if already down */
 
-	/* disable all enabled rx queues */
+	/* disable all enabled Rx queues */
 	for (i = 0; i < adapter->num_rx_queues; i++)
 		ixgbevf_disable_rx_queue(adapter, adapter->rx_ring[i]);
 
@@ -2328,6 +2333,7 @@ static void ixgbevf_set_num_queues(struct ixgbevf_adapter *adapter)
 
 		switch (hw->api_version) {
 		case ixgbe_mbox_api_11:
+		case ixgbe_mbox_api_12:
 			adapter->num_rx_queues = rss;
 			adapter->num_tx_queues = rss;
 		default:
@@ -2406,8 +2412,7 @@ static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 	int err = 0;
 	int vector, v_budget;
 
-	/*
-	 * It's easy to be greedy for MSI-X vectors, but it really
+	/* It's easy to be greedy for MSI-X vectors, but it really
 	 * doesn't do us much good if we have a lot more vectors
 	 * than CPU's.  So let's be conservative and only ask for
 	 * (roughly) the same number of vectors as there are CPU's.
@@ -2418,7 +2423,8 @@ static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 	v_budget += NON_Q_VECTORS;
 
 	/* A failure in MSI-X entry allocation isn't fatal, but it does
-	 * mean we disable MSI-X capabilities of the adapter. */
+	 * mean we disable MSI-X capabilities of the adapter.
+	 */
 	adapter->msix_entries = kcalloc(v_budget,
 					sizeof(struct msix_entry), GFP_KERNEL);
 	if (!adapter->msix_entries) {
@@ -2544,8 +2550,7 @@ static int ixgbevf_init_interrupt_scheme(struct ixgbevf_adapter *adapter)
 
 	err = ixgbevf_alloc_q_vectors(adapter);
 	if (err) {
-		hw_dbg(&adapter->hw, "Unable to allocate memory for queue "
-		       "vectors\n");
+		hw_dbg(&adapter->hw, "Unable to allocate memory for queue vectors\n");
 		goto err_alloc_q_vectors;
 	}
 
@@ -2555,8 +2560,7 @@ static int ixgbevf_init_interrupt_scheme(struct ixgbevf_adapter *adapter)
 		goto err_alloc_queues;
 	}
 
-	hw_dbg(&adapter->hw, "Multiqueue %s: Rx Queue count = %u, "
-	       "Tx Queue count = %u\n",
+	hw_dbg(&adapter->hw, "Multiqueue %s: Rx Queue count = %u, Tx Queue count = %u\n",
 	       (adapter->num_rx_queues > 1) ? "Enabled" :
 	       "Disabled", adapter->num_rx_queues, adapter->num_tx_queues);
 
@@ -2600,7 +2604,6 @@ static void ixgbevf_clear_interrupt_scheme(struct ixgbevf_adapter *adapter)
 
 /**
  * ixgbevf_sw_init - Initialize general software structures
- * (struct ixgbevf_adapter)
  * @adapter: board private structure to initialize
  *
  * ixgbevf_sw_init initializes the Adapter private data structure.
@@ -2615,7 +2618,6 @@ static int ixgbevf_sw_init(struct ixgbevf_adapter *adapter)
 	int err;
 
 	/* PCI config space info */
-
 	hw->vendor_id = pdev->vendor;
 	hw->device_id = pdev->device;
 	hw->revision_id = pdev->revision;
@@ -2686,8 +2688,8 @@ out:
 	{								 \
 		u64 current_counter_lsb = IXGBE_READ_REG(hw, reg_lsb);	 \
 		u64 current_counter_msb = IXGBE_READ_REG(hw, reg_msb);	 \
-		u64 current_counter = (current_counter_msb << 32) |      \
-			current_counter_lsb;                             \
+		u64 current_counter = (current_counter_msb << 32) |	 \
+			current_counter_lsb;				 \
 		if (current_counter < last_counter)			 \
 			counter += 0x1000000000LL;			 \
 		last_counter = current_counter;				 \
@@ -2758,14 +2760,15 @@ static void ixgbevf_reset_subtask(struct ixgbevf_adapter *adapter)
 	ixgbevf_reinit_locked(adapter);
 }
 
-/* ixgbevf_check_hang_subtask - check for hung queues and dropped interrupts
- * @adapter - pointer to the device adapter structure
+/**
+ * ixgbevf_check_hang_subtask - check for hung queues and dropped interrupts
+ * @adapter: pointer to the device adapter structure
  *
  * This function serves two purposes.  First it strobes the interrupt lines
  * in order to make certain interrupts are occurring.  Secondly it sets the
  * bits needed to check for TX hangs.  As a result we should immediately
  * determine if a hang has occurred.
- */
+ **/
 static void ixgbevf_check_hang_subtask(struct ixgbevf_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
@@ -2783,7 +2786,7 @@ static void ixgbevf_check_hang_subtask(struct ixgbevf_adapter *adapter)
 			set_check_for_tx_hang(adapter->tx_ring[i]);
 	}
 
-	/* get one bit for every active tx/rx interrupt vector */
+	/* get one bit for every active Tx/Rx interrupt vector */
 	for (i = 0; i < adapter->num_msix_vectors - NON_Q_VECTORS; i++) {
 		struct ixgbevf_q_vector *qv = adapter->q_vector[i];
 
@@ -2797,7 +2800,7 @@ static void ixgbevf_check_hang_subtask(struct ixgbevf_adapter *adapter)
 
 /**
  * ixgbevf_watchdog_update_link - update the link status
- * @adapter - pointer to the device adapter structure
+ * @adapter: pointer to the device adapter structure
  **/
 static void ixgbevf_watchdog_update_link(struct ixgbevf_adapter *adapter)
 {
@@ -2825,7 +2828,7 @@ static void ixgbevf_watchdog_update_link(struct ixgbevf_adapter *adapter)
 /**
  * ixgbevf_watchdog_link_is_up - update netif_carrier status and
  *				 print link up message
- * @adapter - pointer to the device adapter structure
+ * @adapter: pointer to the device adapter structure
  **/
 static void ixgbevf_watchdog_link_is_up(struct ixgbevf_adapter *adapter)
 {
@@ -2850,7 +2853,7 @@ static void ixgbevf_watchdog_link_is_up(struct ixgbevf_adapter *adapter)
 /**
  * ixgbevf_watchdog_link_is_down - update netif_carrier status and
  *				   print link down message
- * @adapter - pointer to the adapter structure
+ * @adapter: pointer to the adapter structure
  **/
 static void ixgbevf_watchdog_link_is_down(struct ixgbevf_adapter *adapter)
 {
@@ -2956,7 +2959,7 @@ static void ixgbevf_free_all_tx_resources(struct ixgbevf_adapter *adapter)
 
 /**
  * ixgbevf_setup_tx_resources - allocate Tx resources (Descriptors)
- * @tx_ring:    tx descriptor ring (for a specific queue) to setup
+ * @tx_ring: Tx descriptor ring (for a specific queue) to setup
  *
  * Return 0 on success, negative on failure
  **/
@@ -2983,8 +2986,7 @@ int ixgbevf_setup_tx_resources(struct ixgbevf_ring *tx_ring)
 err:
 	vfree(tx_ring->tx_buffer_info);
 	tx_ring->tx_buffer_info = NULL;
-	hw_dbg(&adapter->hw, "Unable to allocate memory for the transmit "
-	       "descriptor ring\n");
+	hw_dbg(&adapter->hw, "Unable to allocate memory for the transmit descriptor ring\n");
 	return -ENOMEM;
 }
 
@@ -3006,8 +3008,7 @@ static int ixgbevf_setup_all_tx_resources(struct ixgbevf_adapter *adapter)
 		err = ixgbevf_setup_tx_resources(adapter->tx_ring[i]);
 		if (!err)
 			continue;
-		hw_dbg(&adapter->hw,
-		       "Allocation for Tx Queue %u failed\n", i);
+		hw_dbg(&adapter->hw, "Allocation for Tx Queue %u failed\n", i);
 		break;
 	}
 
@@ -3016,7 +3017,7 @@ static int ixgbevf_setup_all_tx_resources(struct ixgbevf_adapter *adapter)
 
 /**
  * ixgbevf_setup_rx_resources - allocate Rx resources (Descriptors)
- * @rx_ring:    rx descriptor ring (for a specific queue) to setup
+ * @rx_ring: Rx descriptor ring (for a specific queue) to setup
  *
  * Returns 0 on success, negative on failure
  **/
@@ -3065,8 +3066,7 @@ static int ixgbevf_setup_all_rx_resources(struct ixgbevf_adapter *adapter)
 		err = ixgbevf_setup_rx_resources(adapter->rx_ring[i]);
 		if (!err)
 			continue;
-		hw_dbg(&adapter->hw,
-		       "Allocation for Rx Queue %u failed\n", i);
+		hw_dbg(&adapter->hw, "Allocation for Rx Queue %u failed\n", i);
 		break;
 	}
 	return err;
@@ -3136,11 +3136,11 @@ static int ixgbevf_open(struct net_device *netdev)
 	if (hw->adapter_stopped) {
 		ixgbevf_reset(adapter);
 		/* if adapter is still stopped then PF isn't up and
-		 * the vf can't start. */
+		 * the VF can't start.
+		 */
 		if (hw->adapter_stopped) {
 			err = IXGBE_ERR_MBX;
-			pr_err("Unable to start - perhaps the PF Driver isn't "
-			       "up yet\n");
+			pr_err("Unable to start - perhaps the PF Driver isn't up yet\n");
 			goto err_setup_reset;
 		}
 	}
@@ -3163,8 +3163,7 @@ static int ixgbevf_open(struct net_device *netdev)
 
 	ixgbevf_configure(adapter);
 
-	/*
-	 * Map the Tx/Rx rings to the vectors we were allotted.
+	/* Map the Tx/Rx rings to the vectors we were allotted.
 	 * if request_irq will be called in this function map_rings
 	 * must be called *before* up_complete
 	 */
@@ -3288,6 +3287,7 @@ static int ixgbevf_tso(struct ixgbevf_ring *tx_ring,
 
 	if (first->protocol == htons(ETH_P_IP)) {
 		struct iphdr *iph = ip_hdr(skb);
+
 		iph->tot_len = 0;
 		iph->check = 0;
 		tcp_hdr(skb)->check = ~csum_tcpudp_magic(iph->saddr,
@@ -3313,7 +3313,7 @@ static int ixgbevf_tso(struct ixgbevf_ring *tx_ring,
 	*hdr_len += l4len;
 	*hdr_len = skb_transport_offset(skb) + l4len;
 
-	/* update gso size and bytecount with header size */
+	/* update GSO size and bytecount with header size */
 	first->gso_segs = skb_shinfo(skb)->gso_segs;
 	first->bytecount += (first->gso_segs - 1) * *hdr_len;
 
@@ -3343,6 +3343,7 @@ static void ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 
 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		u8 l4_hdr = 0;
+
 		switch (first->protocol) {
 		case htons(ETH_P_IP):
 			vlan_macip_lens |= skb_network_header_len(skb);
@@ -3356,8 +3357,8 @@ static void ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 		default:
 			if (unlikely(net_ratelimit())) {
 				dev_warn(tx_ring->dev,
-				 "partial checksum but proto=%x!\n",
-				 first->protocol);
+					 "partial checksum but proto=%x!\n",
+					 first->protocol);
 			}
 			break;
 		}
@@ -3380,8 +3381,8 @@ static void ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 		default:
 			if (unlikely(net_ratelimit())) {
 				dev_warn(tx_ring->dev,
-				 "partial checksum but l4 proto=%x!\n",
-				 l4_hdr);
+					 "partial checksum but l4 proto=%x!\n",
+					 l4_hdr);
 			}
 			break;
 		}
@@ -3405,7 +3406,7 @@ static __le32 ixgbevf_tx_cmd_type(u32 tx_flags)
 				      IXGBE_ADVTXD_DCMD_IFCS |
 				      IXGBE_ADVTXD_DCMD_DEXT);
 
-	/* set HW vlan bit if vlan is present */
+	/* set HW VLAN bit if VLAN is present */
 	if (tx_flags & IXGBE_TX_FLAGS_VLAN)
 		cmd_type |= cpu_to_le32(IXGBE_ADVTXD_DCMD_VLE);
 
@@ -3572,11 +3573,13 @@ static int __ixgbevf_maybe_stop_tx(struct ixgbevf_ring *tx_ring, int size)
 	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
 	/* Herbert's original patch had:
 	 *  smp_mb__after_netif_stop_queue();
-	 * but since that doesn't exist yet, just open code it. */
+	 * but since that doesn't exist yet, just open code it.
+	 */
 	smp_mb();
 
 	/* We need to check again in a case another CPU has just
-	 * made room available. */
+	 * made room available.
+	 */
 	if (likely(ixgbevf_desc_unused(tx_ring) < size))
 		return -EBUSY;
 
@@ -3615,8 +3618,7 @@ static int ixgbevf_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 
 	tx_ring = adapter->tx_ring[skb->queue_mapping];
 
-	/*
-	 * need: 1 descriptor per page * PAGE_SIZE/IXGBE_MAX_DATA_PER_TXD,
+	/* need: 1 descriptor per page * PAGE_SIZE/IXGBE_MAX_DATA_PER_TXD,
 	 *       + 1 desc for skb_headlen/IXGBE_MAX_DATA_PER_TXD,
 	 *       + 2 desc gap to keep tail from touching head,
 	 *       + 1 desc for context descriptor,
@@ -3712,6 +3714,7 @@ static int ixgbevf_change_mtu(struct net_device *netdev, int new_mtu)
 
 	switch (adapter->hw.api_version) {
 	case ixgbe_mbox_api_11:
+	case ixgbe_mbox_api_12:
 		max_possible_frame = IXGBE_MAX_JUMBO_FRAME_SIZE;
 		break;
 	default:
@@ -3794,8 +3797,7 @@ static int ixgbevf_resume(struct pci_dev *pdev)
 	u32 err;
 
 	pci_restore_state(pdev);
-	/*
-	 * pci_restore_state clears dev->state_saved so call
+	/* pci_restore_state clears dev->state_saved so call
 	 * pci_save_state to restore it.
 	 */
 	pci_save_state(pdev);
@@ -3930,8 +3932,7 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	} else {
 		err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
 		if (err) {
-			dev_err(&pdev->dev, "No usable DMA "
-				"configuration, aborting\n");
+			dev_err(&pdev->dev, "No usable DMA configuration, aborting\n");
 			goto err_dma;
 		}
 		pci_using_dac = 0;
@@ -3962,8 +3963,7 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	hw->back = adapter;
 	adapter->msg_enable = netif_msg_init(debug, DEFAULT_MSG_ENABLE);
 
-	/*
-	 * call save state here in standalone driver because it relies on
+	/* call save state here in standalone driver because it relies on
 	 * adapter struct to exist, and needs to call netdev_priv
 	 */
 	pci_save_state(pdev);
@@ -3978,7 +3978,7 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	ixgbevf_assign_netdev_ops(netdev);
 
-	/* Setup hw api */
+	/* Setup HW API */
 	memcpy(&hw->mac.ops, ii->mac_ops, sizeof(hw->mac.ops));
 	hw->mac.type  = ii->mac;
 
@@ -3998,11 +3998,11 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	netdev->hw_features = NETIF_F_SG |
-			   NETIF_F_IP_CSUM |
-			   NETIF_F_IPV6_CSUM |
-			   NETIF_F_TSO |
-			   NETIF_F_TSO6 |
-			   NETIF_F_RXCSUM;
+			      NETIF_F_IP_CSUM |
+			      NETIF_F_IPV6_CSUM |
+			      NETIF_F_TSO |
+			      NETIF_F_TSO6 |
+			      NETIF_F_RXCSUM;
 
 	netdev->features = netdev->hw_features |
 			   NETIF_F_HW_VLAN_CTAG_TX |
@@ -4131,7 +4131,7 @@ static void ixgbevf_remove(struct pci_dev *pdev)
  *
  * This function is called after a PCI bus error affecting
  * this device has been detected.
- */
+ **/
 static pci_ers_result_t ixgbevf_io_error_detected(struct pci_dev *pdev,
 						  pci_channel_state_t state)
 {
@@ -4166,7 +4166,7 @@ static pci_ers_result_t ixgbevf_io_error_detected(struct pci_dev *pdev,
  *
  * Restart the card from scratch, as if from a cold-boot. Implementation
  * resembles the first-half of the ixgbevf_resume routine.
- */
+ **/
 static pci_ers_result_t ixgbevf_io_slot_reset(struct pci_dev *pdev)
 {
 	struct net_device *netdev = pci_get_drvdata(pdev);
@@ -4194,7 +4194,7 @@ static pci_ers_result_t ixgbevf_io_slot_reset(struct pci_dev *pdev)
  * This callback is called when the error recovery driver tells us that
  * its OK to resume normal operation. Implementation resembles the
  * second-half of the ixgbevf_resume routine.
- */
+ **/
 static void ixgbevf_io_resume(struct pci_dev *pdev)
 {
 	struct net_device *netdev = pci_get_drvdata(pdev);
@@ -4214,17 +4214,17 @@ static const struct pci_error_handlers ixgbevf_err_handler = {
 };
 
 static struct pci_driver ixgbevf_driver = {
-	.name     = ixgbevf_driver_name,
-	.id_table = ixgbevf_pci_tbl,
-	.probe    = ixgbevf_probe,
-	.remove   = ixgbevf_remove,
+	.name		= ixgbevf_driver_name,
+	.id_table	= ixgbevf_pci_tbl,
+	.probe		= ixgbevf_probe,
+	.remove		= ixgbevf_remove,
 #ifdef CONFIG_PM
 	/* Power Management Hooks */
-	.suspend  = ixgbevf_suspend,
-	.resume   = ixgbevf_resume,
+	.suspend	= ixgbevf_suspend,
+	.resume		= ixgbevf_resume,
 #endif
-	.shutdown = ixgbevf_shutdown,
-	.err_handler = &ixgbevf_err_handler
+	.shutdown	= ixgbevf_shutdown,
+	.err_handler	= &ixgbevf_err_handler
 };
 
 /**
@@ -4236,6 +4236,7 @@ static struct pci_driver ixgbevf_driver = {
 static int __init ixgbevf_init_module(void)
 {
 	int ret;
+
 	pr_info("%s - version %s\n", ixgbevf_driver_string,
 		ixgbevf_driver_version);
 
@@ -4266,6 +4267,7 @@ static void __exit ixgbevf_exit_module(void)
 char *ixgbevf_get_hw_dev_name(struct ixgbe_hw *hw)
 {
 	struct ixgbevf_adapter *adapter = hw->back;
+
 	return adapter->netdev->name;
 }
 
diff --git a/drivers/net/ethernet/intel/ixgbevf/mbx.c b/drivers/net/ethernet/intel/ixgbevf/mbx.c
index d5028ddf4b31..dc68fea4894b 100644
--- a/drivers/net/ethernet/intel/ixgbevf/mbx.c
+++ b/drivers/net/ethernet/intel/ixgbevf/mbx.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2012 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -52,10 +51,10 @@ static s32 ixgbevf_poll_for_msg(struct ixgbe_hw *hw)
 }
 
 /**
- *  ixgbevf_poll_for_ack - Wait for message acknowledgement
+ *  ixgbevf_poll_for_ack - Wait for message acknowledgment
  *  @hw: pointer to the HW structure
  *
- *  returns 0 if it successfully received a message acknowledgement
+ *  returns 0 if it successfully received a message acknowledgment
  **/
 static s32 ixgbevf_poll_for_ack(struct ixgbe_hw *hw)
 {
@@ -213,7 +212,7 @@ static s32 ixgbevf_check_for_rst_vf(struct ixgbe_hw *hw)
 	s32 ret_val = IXGBE_ERR_MBX;
 
 	if (!ixgbevf_check_for_bit_vf(hw, (IXGBE_VFMAILBOX_RSTD |
-					 IXGBE_VFMAILBOX_RSTI))) {
+					   IXGBE_VFMAILBOX_RSTI))) {
 		ret_val = 0;
 		hw->mbx.stats.rsts++;
 	}
@@ -234,7 +233,7 @@ static s32 ixgbevf_obtain_mbx_lock_vf(struct ixgbe_hw *hw)
 	/* Take ownership of the buffer */
 	IXGBE_WRITE_REG(hw, IXGBE_VFMAILBOX, IXGBE_VFMAILBOX_VFU);
 
-	/* reserve mailbox for vf use */
+	/* reserve mailbox for VF use */
 	if (ixgbevf_read_v2p_mailbox(hw) & IXGBE_VFMAILBOX_VFU)
 		ret_val = 0;
 
@@ -254,8 +253,7 @@ static s32 ixgbevf_write_mbx_vf(struct ixgbe_hw *hw, u32 *msg, u16 size)
 	s32 ret_val;
 	u16 i;
 
-
-	/* lock the mailbox to prevent pf/vf race condition */
+	/* lock the mailbox to prevent PF/VF race condition */
 	ret_val = ixgbevf_obtain_mbx_lock_vf(hw);
 	if (ret_val)
 		goto out_no_write;
@@ -279,7 +277,7 @@ out_no_write:
 }
 
 /**
- *  ixgbevf_read_mbx_vf - Reads a message from the inbox intended for vf
+ *  ixgbevf_read_mbx_vf - Reads a message from the inbox intended for VF
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -291,7 +289,7 @@ static s32 ixgbevf_read_mbx_vf(struct ixgbe_hw *hw, u32 *msg, u16 size)
 	s32 ret_val = 0;
 	u16 i;
 
-	/* lock the mailbox to prevent pf/vf race condition */
+	/* lock the mailbox to prevent PF/VF race condition */
 	ret_val = ixgbevf_obtain_mbx_lock_vf(hw);
 	if (ret_val)
 		goto out_no_read;
@@ -311,17 +309,18 @@ out_no_read:
 }
 
 /**
- *  ixgbevf_init_mbx_params_vf - set initial values for vf mailbox
+ *  ixgbevf_init_mbx_params_vf - set initial values for VF mailbox
  *  @hw: pointer to the HW structure
  *
- *  Initializes the hw->mbx struct to correct values for vf mailbox
+ *  Initializes the hw->mbx struct to correct values for VF mailbox
  */
 static s32 ixgbevf_init_mbx_params_vf(struct ixgbe_hw *hw)
 {
 	struct ixgbe_mbx_info *mbx = &hw->mbx;
 
 	/* start mailbox as timed out and let the reset_hw call set the timeout
-	 * value to begin communications */
+	 * value to begin communications
+	 */
 	mbx->timeout = 0;
 	mbx->udelay = IXGBE_VF_MBX_INIT_DELAY;
 
@@ -337,13 +336,13 @@ static s32 ixgbevf_init_mbx_params_vf(struct ixgbe_hw *hw)
 }
 
 const struct ixgbe_mbx_operations ixgbevf_mbx_ops = {
-	.init_params   = ixgbevf_init_mbx_params_vf,
-	.read          = ixgbevf_read_mbx_vf,
-	.write         = ixgbevf_write_mbx_vf,
-	.read_posted   = ixgbevf_read_posted_mbx,
-	.write_posted  = ixgbevf_write_posted_mbx,
-	.check_for_msg = ixgbevf_check_for_msg_vf,
-	.check_for_ack = ixgbevf_check_for_ack_vf,
-	.check_for_rst = ixgbevf_check_for_rst_vf,
+	.init_params	= ixgbevf_init_mbx_params_vf,
+	.read		= ixgbevf_read_mbx_vf,
+	.write		= ixgbevf_write_mbx_vf,
+	.read_posted	= ixgbevf_read_posted_mbx,
+	.write_posted	= ixgbevf_write_posted_mbx,
+	.check_for_msg	= ixgbevf_check_for_msg_vf,
+	.check_for_ack	= ixgbevf_check_for_ack_vf,
+	.check_for_rst	= ixgbevf_check_for_rst_vf,
 };
 
diff --git a/drivers/net/ethernet/intel/ixgbevf/mbx.h b/drivers/net/ethernet/intel/ixgbevf/mbx.h
index 0bc30058ff82..82f44e06e5fc 100644
--- a/drivers/net/ethernet/intel/ixgbevf/mbx.h
+++ b/drivers/net/ethernet/intel/ixgbevf/mbx.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2012 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -30,71 +29,70 @@
 
 #include "vf.h"
 
-#define IXGBE_VFMAILBOX_SIZE        16 /* 16 32 bit words - 64 bytes */
-#define IXGBE_ERR_MBX               -100
+#define IXGBE_VFMAILBOX_SIZE	16 /* 16 32 bit words - 64 bytes */
+#define IXGBE_ERR_MBX		-100
 
-#define IXGBE_VFMAILBOX             0x002FC
-#define IXGBE_VFMBMEM               0x00200
+#define IXGBE_VFMAILBOX		0x002FC
+#define IXGBE_VFMBMEM		0x00200
 
 /* Define mailbox register bits */
-#define IXGBE_VFMAILBOX_REQ      0x00000001 /* Request for PF Ready bit */
-#define IXGBE_VFMAILBOX_ACK      0x00000002 /* Ack PF message received */
-#define IXGBE_VFMAILBOX_VFU      0x00000004 /* VF owns the mailbox buffer */
-#define IXGBE_VFMAILBOX_PFU      0x00000008 /* PF owns the mailbox buffer */
-#define IXGBE_VFMAILBOX_PFSTS    0x00000010 /* PF wrote a message in the MB */
-#define IXGBE_VFMAILBOX_PFACK    0x00000020 /* PF ack the previous VF msg */
-#define IXGBE_VFMAILBOX_RSTI     0x00000040 /* PF has reset indication */
-#define IXGBE_VFMAILBOX_RSTD     0x00000080 /* PF has indicated reset done */
+#define IXGBE_VFMAILBOX_REQ	0x00000001 /* Request for PF Ready bit */
+#define IXGBE_VFMAILBOX_ACK	0x00000002 /* Ack PF message received */
+#define IXGBE_VFMAILBOX_VFU	0x00000004 /* VF owns the mailbox buffer */
+#define IXGBE_VFMAILBOX_PFU	0x00000008 /* PF owns the mailbox buffer */
+#define IXGBE_VFMAILBOX_PFSTS	0x00000010 /* PF wrote a message in the MB */
+#define IXGBE_VFMAILBOX_PFACK	0x00000020 /* PF ack the previous VF msg */
+#define IXGBE_VFMAILBOX_RSTI	0x00000040 /* PF has reset indication */
+#define IXGBE_VFMAILBOX_RSTD	0x00000080 /* PF has indicated reset done */
 #define IXGBE_VFMAILBOX_R2C_BITS 0x000000B0 /* All read to clear bits */
 
-#define IXGBE_PFMAILBOX(x)          (0x04B00 + (4 * (x)))
-#define IXGBE_PFMBMEM(vfn)          (0x13000 + (64 * (vfn)))
+#define IXGBE_PFMAILBOX(x)	(0x04B00 + (4 * (x)))
+#define IXGBE_PFMBMEM(vfn)	(0x13000 + (64 * (vfn)))
 
-#define IXGBE_PFMAILBOX_STS   0x00000001 /* Initiate message send to VF */
-#define IXGBE_PFMAILBOX_ACK   0x00000002 /* Ack message recv'd from VF */
-#define IXGBE_PFMAILBOX_VFU   0x00000004 /* VF owns the mailbox buffer */
-#define IXGBE_PFMAILBOX_PFU   0x00000008 /* PF owns the mailbox buffer */
-#define IXGBE_PFMAILBOX_RVFU  0x00000010 /* Reset VFU - used when VF stuck */
+#define IXGBE_PFMAILBOX_STS	0x00000001 /* Initiate message send to VF */
+#define IXGBE_PFMAILBOX_ACK	0x00000002 /* Ack message recv'd from VF */
+#define IXGBE_PFMAILBOX_VFU	0x00000004 /* VF owns the mailbox buffer */
+#define IXGBE_PFMAILBOX_PFU	0x00000008 /* PF owns the mailbox buffer */
+#define IXGBE_PFMAILBOX_RVFU	0x00000010 /* Reset VFU - used when VF stuck */
 
 #define IXGBE_MBVFICR_VFREQ_MASK 0x0000FFFF /* bits for VF messages */
-#define IXGBE_MBVFICR_VFREQ_VF1  0x00000001 /* bit for VF 1 message */
+#define IXGBE_MBVFICR_VFREQ_VF1	0x00000001 /* bit for VF 1 message */
 #define IXGBE_MBVFICR_VFACK_MASK 0xFFFF0000 /* bits for VF acks */
-#define IXGBE_MBVFICR_VFACK_VF1  0x00010000 /* bit for VF 1 ack */
-
+#define IXGBE_MBVFICR_VFACK_VF1	0x00010000 /* bit for VF 1 ack */
 
 /* If it's a IXGBE_VF_* msg then it originates in the VF and is sent to the
  * PF.  The reverse is true if it is IXGBE_PF_*.
  * Message ACK's are the value or'd with 0xF0000000
  */
-#define IXGBE_VT_MSGTYPE_ACK      0x80000000  /* Messages below or'd with
-					       * this are the ACK */
-#define IXGBE_VT_MSGTYPE_NACK     0x40000000  /* Messages below or'd with
-					       * this are the NACK */
-#define IXGBE_VT_MSGTYPE_CTS      0x20000000  /* Indicates that VF is still
-					       *  clear to send requests */
-#define IXGBE_VT_MSGINFO_SHIFT    16
+/* Messages below or'd with this are the ACK */
+#define IXGBE_VT_MSGTYPE_ACK	0x80000000
+/* Messages below or'd with this are the NACK */
+#define IXGBE_VT_MSGTYPE_NACK	0x40000000
+/* Indicates that VF is still clear to send requests */
+#define IXGBE_VT_MSGTYPE_CTS	0x20000000
+#define IXGBE_VT_MSGINFO_SHIFT	16
 /* bits 23:16 are used for exra info for certain messages */
-#define IXGBE_VT_MSGINFO_MASK     (0xFF << IXGBE_VT_MSGINFO_SHIFT)
+#define IXGBE_VT_MSGINFO_MASK	(0xFF << IXGBE_VT_MSGINFO_SHIFT)
 
 /* definitions to support mailbox API version negotiation */
 
-/*
- * each element denotes a version of the API; existing numbers may not
+/* each element denotes a version of the API; existing numbers may not
  * change; any additions must go at the end
  */
 enum ixgbe_pfvf_api_rev {
 	ixgbe_mbox_api_10,	/* API version 1.0, linux/freebsd VF driver */
 	ixgbe_mbox_api_20,	/* API version 2.0, solaris Phase1 VF driver */
 	ixgbe_mbox_api_11,	/* API version 1.1, linux/freebsd VF driver */
+	ixgbe_mbox_api_12,	/* API version 1.2, linux/freebsd VF driver */
 	/* This value should always be last */
 	ixgbe_mbox_api_unknown,	/* indicates that API version is not known */
 };
 
 /* mailbox API, legacy requests */
-#define IXGBE_VF_RESET            0x01 /* VF requests reset */
-#define IXGBE_VF_SET_MAC_ADDR     0x02 /* VF requests PF to set MAC addr */
-#define IXGBE_VF_SET_MULTICAST    0x03 /* VF requests PF to set MC addr */
-#define IXGBE_VF_SET_VLAN         0x04 /* VF requests PF to set VLAN */
+#define IXGBE_VF_RESET		0x01 /* VF requests reset */
+#define IXGBE_VF_SET_MAC_ADDR	0x02 /* VF requests PF to set MAC addr */
+#define IXGBE_VF_SET_MULTICAST	0x03 /* VF requests PF to set MC addr */
+#define IXGBE_VF_SET_VLAN	0x04 /* VF requests PF to set VLAN */
 
 /* mailbox API, version 1.0 VF requests */
 #define IXGBE_VF_SET_LPE	0x05 /* VF requests PF to set VMOLR.LPE */
@@ -105,20 +103,24 @@ enum ixgbe_pfvf_api_rev {
 #define IXGBE_VF_GET_QUEUE	0x09 /* get queue configuration */
 
 /* GET_QUEUES return data indices within the mailbox */
-#define IXGBE_VF_TX_QUEUES	1	/* number of Tx queues supported */
-#define IXGBE_VF_RX_QUEUES	2	/* number of Rx queues supported */
-#define IXGBE_VF_TRANS_VLAN	3	/* Indication of port vlan */
-#define IXGBE_VF_DEF_QUEUE	4	/* Default queue offset */
+#define IXGBE_VF_TX_QUEUES	1 /* number of Tx queues supported */
+#define IXGBE_VF_RX_QUEUES	2 /* number of Rx queues supported */
+#define IXGBE_VF_TRANS_VLAN	3 /* Indication of port VLAN */
+#define IXGBE_VF_DEF_QUEUE	4 /* Default queue offset */
+
+/* mailbox API, version 1.2 VF requests */
+#define IXGBE_VF_GET_RETA	0x0a	/* VF request for RETA */
+#define IXGBE_VF_GET_RSS_KEY	0x0b	/* get RSS hash key */
 
 /* length of permanent address message returned from PF */
-#define IXGBE_VF_PERMADDR_MSG_LEN 4
+#define IXGBE_VF_PERMADDR_MSG_LEN	4
 /* word in permanent address message with the current multicast type */
-#define IXGBE_VF_MC_TYPE_WORD     3
+#define IXGBE_VF_MC_TYPE_WORD		3
 
-#define IXGBE_PF_CONTROL_MSG      0x0100 /* PF control message */
+#define IXGBE_PF_CONTROL_MSG		0x0100 /* PF control message */
 
-#define IXGBE_VF_MBX_INIT_TIMEOUT 2000 /* number of retries on mailbox */
-#define IXGBE_VF_MBX_INIT_DELAY   500  /* microseconds between retries */
+#define IXGBE_VF_MBX_INIT_TIMEOUT	2000 /* number of retries on mailbox */
+#define IXGBE_VF_MBX_INIT_DELAY		500  /* microseconds between retries */
 
 /* forward declaration of the HW struct */
 struct ixgbe_hw;
diff --git a/drivers/net/ethernet/intel/ixgbevf/regs.h b/drivers/net/ethernet/intel/ixgbevf/regs.h
index 3e712fd6e695..2764fd16261f 100644
--- a/drivers/net/ethernet/intel/ixgbevf/regs.h
+++ b/drivers/net/ethernet/intel/ixgbevf/regs.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -28,58 +27,58 @@
 #ifndef _IXGBEVF_REGS_H_
 #define _IXGBEVF_REGS_H_
 
-#define IXGBE_VFCTRL           0x00000
-#define IXGBE_VFSTATUS         0x00008
-#define IXGBE_VFLINKS          0x00010
-#define IXGBE_VFFRTIMER        0x00048
-#define IXGBE_VFRXMEMWRAP      0x03190
-#define IXGBE_VTEICR           0x00100
-#define IXGBE_VTEICS           0x00104
-#define IXGBE_VTEIMS           0x00108
-#define IXGBE_VTEIMC           0x0010C
-#define IXGBE_VTEIAC           0x00110
-#define IXGBE_VTEIAM           0x00114
-#define IXGBE_VTEITR(x)        (0x00820 + (4 * (x)))
-#define IXGBE_VTIVAR(x)        (0x00120 + (4 * (x)))
-#define IXGBE_VTIVAR_MISC      0x00140
-#define IXGBE_VTRSCINT(x)      (0x00180 + (4 * (x)))
-#define IXGBE_VFRDBAL(x)       (0x01000 + (0x40 * (x)))
-#define IXGBE_VFRDBAH(x)       (0x01004 + (0x40 * (x)))
-#define IXGBE_VFRDLEN(x)       (0x01008 + (0x40 * (x)))
-#define IXGBE_VFRDH(x)         (0x01010 + (0x40 * (x)))
-#define IXGBE_VFRDT(x)         (0x01018 + (0x40 * (x)))
-#define IXGBE_VFRXDCTL(x)      (0x01028 + (0x40 * (x)))
-#define IXGBE_VFSRRCTL(x)      (0x01014 + (0x40 * (x)))
-#define IXGBE_VFRSCCTL(x)      (0x0102C + (0x40 * (x)))
-#define IXGBE_VFPSRTYPE        0x00300
-#define IXGBE_VFTDBAL(x)       (0x02000 + (0x40 * (x)))
-#define IXGBE_VFTDBAH(x)       (0x02004 + (0x40 * (x)))
-#define IXGBE_VFTDLEN(x)       (0x02008 + (0x40 * (x)))
-#define IXGBE_VFTDH(x)         (0x02010 + (0x40 * (x)))
-#define IXGBE_VFTDT(x)         (0x02018 + (0x40 * (x)))
-#define IXGBE_VFTXDCTL(x)      (0x02028 + (0x40 * (x)))
-#define IXGBE_VFTDWBAL(x)      (0x02038 + (0x40 * (x)))
-#define IXGBE_VFTDWBAH(x)      (0x0203C + (0x40 * (x)))
-#define IXGBE_VFDCA_RXCTRL(x)  (0x0100C + (0x40 * (x)))
-#define IXGBE_VFDCA_TXCTRL(x)  (0x0200c + (0x40 * (x)))
-#define IXGBE_VFGPRC           0x0101C
-#define IXGBE_VFGPTC           0x0201C
-#define IXGBE_VFGORC_LSB       0x01020
-#define IXGBE_VFGORC_MSB       0x01024
-#define IXGBE_VFGOTC_LSB       0x02020
-#define IXGBE_VFGOTC_MSB       0x02024
-#define IXGBE_VFMPRC           0x01034
-#define IXGBE_VFMRQC           0x3000
-#define IXGBE_VFRSSRK(x)       (0x3100 + ((x) * 4))
-#define IXGBE_VFRETA(x)        (0x3200 + ((x) * 4))
+#define IXGBE_VFCTRL		0x00000
+#define IXGBE_VFSTATUS		0x00008
+#define IXGBE_VFLINKS		0x00010
+#define IXGBE_VFFRTIMER		0x00048
+#define IXGBE_VFRXMEMWRAP	0x03190
+#define IXGBE_VTEICR		0x00100
+#define IXGBE_VTEICS		0x00104
+#define IXGBE_VTEIMS		0x00108
+#define IXGBE_VTEIMC		0x0010C
+#define IXGBE_VTEIAC		0x00110
+#define IXGBE_VTEIAM		0x00114
+#define IXGBE_VTEITR(x)		(0x00820 + (4 * (x)))
+#define IXGBE_VTIVAR(x)		(0x00120 + (4 * (x)))
+#define IXGBE_VTIVAR_MISC	0x00140
+#define IXGBE_VTRSCINT(x)	(0x00180 + (4 * (x)))
+#define IXGBE_VFRDBAL(x)	(0x01000 + (0x40 * (x)))
+#define IXGBE_VFRDBAH(x)	(0x01004 + (0x40 * (x)))
+#define IXGBE_VFRDLEN(x)	(0x01008 + (0x40 * (x)))
+#define IXGBE_VFRDH(x)		(0x01010 + (0x40 * (x)))
+#define IXGBE_VFRDT(x)		(0x01018 + (0x40 * (x)))
+#define IXGBE_VFRXDCTL(x)	(0x01028 + (0x40 * (x)))
+#define IXGBE_VFSRRCTL(x)	(0x01014 + (0x40 * (x)))
+#define IXGBE_VFRSCCTL(x)	(0x0102C + (0x40 * (x)))
+#define IXGBE_VFPSRTYPE		0x00300
+#define IXGBE_VFTDBAL(x)	(0x02000 + (0x40 * (x)))
+#define IXGBE_VFTDBAH(x)	(0x02004 + (0x40 * (x)))
+#define IXGBE_VFTDLEN(x)	(0x02008 + (0x40 * (x)))
+#define IXGBE_VFTDH(x)		(0x02010 + (0x40 * (x)))
+#define IXGBE_VFTDT(x)		(0x02018 + (0x40 * (x)))
+#define IXGBE_VFTXDCTL(x)	(0x02028 + (0x40 * (x)))
+#define IXGBE_VFTDWBAL(x)	(0x02038 + (0x40 * (x)))
+#define IXGBE_VFTDWBAH(x)	(0x0203C + (0x40 * (x)))
+#define IXGBE_VFDCA_RXCTRL(x)	(0x0100C + (0x40 * (x)))
+#define IXGBE_VFDCA_TXCTRL(x)	(0x0200c + (0x40 * (x)))
+#define IXGBE_VFGPRC		0x0101C
+#define IXGBE_VFGPTC		0x0201C
+#define IXGBE_VFGORC_LSB	0x01020
+#define IXGBE_VFGORC_MSB	0x01024
+#define IXGBE_VFGOTC_LSB	0x02020
+#define IXGBE_VFGOTC_MSB	0x02024
+#define IXGBE_VFMPRC		0x01034
+#define IXGBE_VFMRQC		0x3000
+#define IXGBE_VFRSSRK(x)	(0x3100 + ((x) * 4))
+#define IXGBE_VFRETA(x)		(0x3200 + ((x) * 4))
 
 /* VFMRQC bits */
-#define IXGBE_VFMRQC_RSSEN              0x00000001  /* RSS Enable */
-#define IXGBE_VFMRQC_RSS_FIELD_IPV4_TCP 0x00010000
-#define IXGBE_VFMRQC_RSS_FIELD_IPV4     0x00020000
-#define IXGBE_VFMRQC_RSS_FIELD_IPV6     0x00100000
-#define IXGBE_VFMRQC_RSS_FIELD_IPV6_TCP 0x00200000
+#define IXGBE_VFMRQC_RSSEN		0x00000001  /* RSS Enable */
+#define IXGBE_VFMRQC_RSS_FIELD_IPV4_TCP	0x00010000
+#define IXGBE_VFMRQC_RSS_FIELD_IPV4	0x00020000
+#define IXGBE_VFMRQC_RSS_FIELD_IPV6	0x00100000
+#define IXGBE_VFMRQC_RSS_FIELD_IPV6_TCP	0x00200000
 
-#define IXGBE_WRITE_FLUSH(a) (IXGBE_READ_REG(a, IXGBE_VFSTATUS))
+#define IXGBE_WRITE_FLUSH(a)	(IXGBE_READ_REG(a, IXGBE_VFSTATUS))
 
 #endif /* _IXGBEVF_REGS_H_ */
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.c b/drivers/net/ethernet/intel/ixgbevf/vf.c
index cdb53be7d995..d1339b050627 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.c
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2012 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -65,7 +64,7 @@ static s32 ixgbevf_init_hw_vf(struct ixgbe_hw *hw)
  *  ixgbevf_reset_hw_vf - Performs hardware reset
  *  @hw: pointer to hardware structure
  *
- *  Resets the hardware by reseting the transmit and receive units, masks and
+ *  Resets the hardware by resetting the transmit and receive units, masks and
  *  clears all interrupts.
  **/
 static s32 ixgbevf_reset_hw_vf(struct ixgbe_hw *hw)
@@ -102,9 +101,10 @@ static s32 ixgbevf_reset_hw_vf(struct ixgbe_hw *hw)
 
 	mdelay(10);
 
-	/* set our "perm_addr" based on info provided by PF */
-	/* also set up the mc_filter_type which is piggy backed
-	 * on the mac address in word 3 */
+	/* set our "perm_addr" based on info provided by PF
+	 * also set up the mc_filter_type which is piggy backed
+	 * on the mac address in word 3
+	 */
 	ret_val = mbx->ops.read_posted(hw, msgbuf, IXGBE_VF_PERMADDR_MSG_LEN);
 	if (ret_val)
 		return ret_val;
@@ -117,7 +117,7 @@ static s32 ixgbevf_reset_hw_vf(struct ixgbe_hw *hw)
 	    msgbuf[0] != (IXGBE_VF_RESET | IXGBE_VT_MSGTYPE_NACK))
 		return IXGBE_ERR_INVALID_MAC_ADDR;
 
-	memcpy(hw->mac.perm_addr, addr, ETH_ALEN);
+	ether_addr_copy(hw->mac.perm_addr, addr);
 	hw->mac.mc_filter_type = msgbuf[IXGBE_VF_MC_TYPE_WORD];
 
 	return 0;
@@ -138,8 +138,7 @@ static s32 ixgbevf_stop_hw_vf(struct ixgbe_hw *hw)
 	u32 reg_val;
 	u16 i;
 
-	/*
-	 * Set the adapter_stopped flag so other driver functions stop touching
+	/* Set the adapter_stopped flag so other driver functions stop touching
 	 * the hardware
 	 */
 	hw->adapter_stopped = true;
@@ -182,7 +181,7 @@ static s32 ixgbevf_stop_hw_vf(struct ixgbe_hw *hw)
  *
  *  Extracts the 12 bits, from a multicast address, to determine which
  *  bit-vector to set in the multicast table. The hardware uses 12 bits, from
- *  incoming rx multicast addresses, to determine the bit-vector to check in
+ *  incoming Rx multicast addresses, to determine the bit-vector to check in
  *  the MTA. Which of the 4 combination, of 12-bits, the hardware uses is set
  *  by the MO field of the MCSTCTRL. The MO field is set during initialization
  *  to mc_filter_type.
@@ -220,7 +219,7 @@ static s32 ixgbevf_mta_vector(struct ixgbe_hw *hw, u8 *mc_addr)
  **/
 static s32 ixgbevf_get_mac_addr_vf(struct ixgbe_hw *hw, u8 *mac_addr)
 {
-	memcpy(mac_addr, hw->mac.perm_addr, ETH_ALEN);
+	ether_addr_copy(mac_addr, hw->mac.perm_addr);
 
 	return 0;
 }
@@ -233,8 +232,7 @@ static s32 ixgbevf_set_uc_addr_vf(struct ixgbe_hw *hw, u32 index, u8 *addr)
 	s32 ret_val;
 
 	memset(msgbuf, 0, sizeof(msgbuf));
-	/*
-	 * If index is one then this is the start of a new list and needs
+	/* If index is one then this is the start of a new list and needs
 	 * indication to the PF so it can do it's own list management.
 	 * If it is zero then that tells the PF to just clear all of
 	 * this VF's macvlans and there is no new list.
@@ -242,7 +240,7 @@ static s32 ixgbevf_set_uc_addr_vf(struct ixgbe_hw *hw, u32 index, u8 *addr)
 	msgbuf[0] |= index << IXGBE_VT_MSGINFO_SHIFT;
 	msgbuf[0] |= IXGBE_VF_SET_MACVLAN;
 	if (addr)
-		memcpy(msg_addr, addr, ETH_ALEN);
+		ether_addr_copy(msg_addr, addr);
 	ret_val = mbx->ops.write_posted(hw, msgbuf, 3);
 
 	if (!ret_val)
@@ -259,6 +257,129 @@ static s32 ixgbevf_set_uc_addr_vf(struct ixgbe_hw *hw, u32 index, u8 *addr)
 }
 
 /**
+ * ixgbevf_get_reta_locked - get the RSS redirection table (RETA) contents.
+ * @adapter: pointer to the port handle
+ * @reta: buffer to fill with RETA contents.
+ * @num_rx_queues: Number of Rx queues configured for this port
+ *
+ * The "reta" buffer should be big enough to contain 32 registers.
+ *
+ * Returns: 0 on success.
+ *          if API doesn't support this operation - (-EOPNOTSUPP).
+ */
+int ixgbevf_get_reta_locked(struct ixgbe_hw *hw, u32 *reta, int num_rx_queues)
+{
+	int err, i, j;
+	u32 msgbuf[IXGBE_VFMAILBOX_SIZE];
+	u32 *hw_reta = &msgbuf[1];
+	u32 mask = 0;
+
+	/* We have to use a mailbox for 82599 and x540 devices only.
+	 * For these devices RETA has 128 entries.
+	 * Also these VFs support up to 4 RSS queues. Therefore PF will compress
+	 * 16 RETA entries in each DWORD giving 2 bits to each entry.
+	 */
+	int dwords = IXGBEVF_82599_RETA_SIZE / 16;
+
+	/* We support the RSS querying for 82599 and x540 devices only.
+	 * Thus return an error if API doesn't support RETA querying or querying
+	 * is not supported for this device type.
+	 */
+	if (hw->api_version != ixgbe_mbox_api_12 ||
+	    hw->mac.type >= ixgbe_mac_X550_vf)
+		return -EOPNOTSUPP;
+
+	msgbuf[0] = IXGBE_VF_GET_RETA;
+
+	err = hw->mbx.ops.write_posted(hw, msgbuf, 1);
+
+	if (err)
+		return err;
+
+	err = hw->mbx.ops.read_posted(hw, msgbuf, dwords + 1);
+
+	if (err)
+		return err;
+
+	msgbuf[0] &= ~IXGBE_VT_MSGTYPE_CTS;
+
+	/* If the operation has been refused by a PF return -EPERM */
+	if (msgbuf[0] == (IXGBE_VF_GET_RETA | IXGBE_VT_MSGTYPE_NACK))
+		return -EPERM;
+
+	/* If we didn't get an ACK there must have been
+	 * some sort of mailbox error so we should treat it
+	 * as such.
+	 */
+	if (msgbuf[0] != (IXGBE_VF_GET_RETA | IXGBE_VT_MSGTYPE_ACK))
+		return IXGBE_ERR_MBX;
+
+	/* ixgbevf doesn't support more than 2 queues at the moment */
+	if (num_rx_queues > 1)
+		mask = 0x1;
+
+	for (i = 0; i < dwords; i++)
+		for (j = 0; j < 16; j++)
+			reta[i * 16 + j] = (hw_reta[i] >> (2 * j)) & mask;
+
+	return 0;
+}
+
+/**
+ * ixgbevf_get_rss_key_locked - get the RSS Random Key
+ * @hw: pointer to the HW structure
+ * @rss_key: buffer to fill with RSS Hash Key contents.
+ *
+ * The "rss_key" buffer should be big enough to contain 10 registers.
+ *
+ * Returns: 0 on success.
+ *          if API doesn't support this operation - (-EOPNOTSUPP).
+ */
+int ixgbevf_get_rss_key_locked(struct ixgbe_hw *hw, u8 *rss_key)
+{
+	int err;
+	u32 msgbuf[IXGBE_VFMAILBOX_SIZE];
+
+	/* We currently support the RSS Random Key retrieval for 82599 and x540
+	 * devices only.
+	 *
+	 * Thus return an error if API doesn't support RSS Random Key retrieval
+	 * or if the operation is not supported for this device type.
+	 */
+	if (hw->api_version != ixgbe_mbox_api_12 ||
+	    hw->mac.type >= ixgbe_mac_X550_vf)
+		return -EOPNOTSUPP;
+
+	msgbuf[0] = IXGBE_VF_GET_RSS_KEY;
+	err = hw->mbx.ops.write_posted(hw, msgbuf, 1);
+
+	if (err)
+		return err;
+
+	err = hw->mbx.ops.read_posted(hw, msgbuf, 11);
+
+	if (err)
+		return err;
+
+	msgbuf[0] &= ~IXGBE_VT_MSGTYPE_CTS;
+
+	/* If the operation has been refused by a PF return -EPERM */
+	if (msgbuf[0] == (IXGBE_VF_GET_RETA | IXGBE_VT_MSGTYPE_NACK))
+		return -EPERM;
+
+	/* If we didn't get an ACK there must have been
+	 * some sort of mailbox error so we should treat it
+	 * as such.
+	 */
+	if (msgbuf[0] != (IXGBE_VF_GET_RSS_KEY | IXGBE_VT_MSGTYPE_ACK))
+		return IXGBE_ERR_MBX;
+
+	memcpy(rss_key, msgbuf + 1, IXGBEVF_RSS_HASH_KEY_SIZE);
+
+	return 0;
+}
+
+/**
  *  ixgbevf_set_rar_vf - set device MAC address
  *  @hw: pointer to hardware structure
  *  @index: Receive address register to write
@@ -275,7 +396,7 @@ static s32 ixgbevf_set_rar_vf(struct ixgbe_hw *hw, u32 index, u8 *addr,
 
 	memset(msgbuf, 0, sizeof(msgbuf));
 	msgbuf[0] = IXGBE_VF_SET_MAC_ADDR;
-	memcpy(msg_addr, addr, ETH_ALEN);
+	ether_addr_copy(msg_addr, addr);
 	ret_val = mbx->ops.write_posted(hw, msgbuf, 3);
 
 	if (!ret_val)
@@ -292,7 +413,7 @@ static s32 ixgbevf_set_rar_vf(struct ixgbe_hw *hw, u32 index, u8 *addr,
 }
 
 static void ixgbevf_write_msg_read_ack(struct ixgbe_hw *hw,
-					u32 *msg, u16 size)
+				       u32 *msg, u16 size)
 {
 	struct ixgbe_mbx_info *mbx = &hw->mbx;
 	u32 retmsg[IXGBE_VFMAILBOX_SIZE];
@@ -348,7 +469,7 @@ static s32 ixgbevf_update_mc_addr_list_vf(struct ixgbe_hw *hw,
 }
 
 /**
- *  ixgbevf_set_vfta_vf - Set/Unset vlan filter table address
+ *  ixgbevf_set_vfta_vf - Set/Unset VLAN filter table address
  *  @hw: pointer to the HW structure
  *  @vlan: 12 bit VLAN ID
  *  @vind: unused by VF drivers
@@ -462,7 +583,8 @@ static s32 ixgbevf_check_mac_link_vf(struct ixgbe_hw *hw,
 	}
 
 	/* if the read failed it could just be a mailbox collision, best wait
-	 * until we are called again and don't report an error */
+	 * until we are called again and don't report an error
+	 */
 	if (mbx->ops.read(hw, &in_msg, 1))
 		goto out;
 
@@ -480,7 +602,8 @@ static s32 ixgbevf_check_mac_link_vf(struct ixgbe_hw *hw,
 	}
 
 	/* if we passed all the tests above then the link is up and we no
-	 * longer need to check for link */
+	 * longer need to check for link
+	 */
 	mac->get_link_status = false;
 
 out:
@@ -545,6 +668,7 @@ int ixgbevf_get_queues(struct ixgbe_hw *hw, unsigned int *num_tcs,
 	/* do nothing if API doesn't support ixgbevf_get_queues */
 	switch (hw->api_version) {
 	case ixgbe_mbox_api_11:
+	case ixgbe_mbox_api_12:
 		break;
 	default:
 		return 0;
@@ -561,8 +685,7 @@ int ixgbevf_get_queues(struct ixgbe_hw *hw, unsigned int *num_tcs,
 	if (!err) {
 		msg[0] &= ~IXGBE_VT_MSGTYPE_CTS;
 
-		/*
-		 * if we we didn't get an ACK there must have been
+		/* if we we didn't get an ACK there must have been
 		 * some sort of mailbox error so we should treat it
 		 * as such
 		 */
@@ -595,17 +718,17 @@ int ixgbevf_get_queues(struct ixgbe_hw *hw, unsigned int *num_tcs,
 }
 
 static const struct ixgbe_mac_operations ixgbevf_mac_ops = {
-	.init_hw             = ixgbevf_init_hw_vf,
-	.reset_hw            = ixgbevf_reset_hw_vf,
-	.start_hw            = ixgbevf_start_hw_vf,
-	.get_mac_addr        = ixgbevf_get_mac_addr_vf,
-	.stop_adapter        = ixgbevf_stop_hw_vf,
-	.setup_link          = ixgbevf_setup_mac_link_vf,
-	.check_link          = ixgbevf_check_mac_link_vf,
-	.set_rar             = ixgbevf_set_rar_vf,
-	.update_mc_addr_list = ixgbevf_update_mc_addr_list_vf,
-	.set_uc_addr         = ixgbevf_set_uc_addr_vf,
-	.set_vfta            = ixgbevf_set_vfta_vf,
+	.init_hw		= ixgbevf_init_hw_vf,
+	.reset_hw		= ixgbevf_reset_hw_vf,
+	.start_hw		= ixgbevf_start_hw_vf,
+	.get_mac_addr		= ixgbevf_get_mac_addr_vf,
+	.stop_adapter		= ixgbevf_stop_hw_vf,
+	.setup_link		= ixgbevf_setup_mac_link_vf,
+	.check_link		= ixgbevf_check_mac_link_vf,
+	.set_rar		= ixgbevf_set_rar_vf,
+	.update_mc_addr_list	= ixgbevf_update_mc_addr_list_vf,
+	.set_uc_addr		= ixgbevf_set_uc_addr_vf,
+	.set_vfta		= ixgbevf_set_vfta_vf,
 };
 
 const struct ixgbevf_info ixgbevf_82599_vf_info = {
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.h b/drivers/net/ethernet/intel/ixgbevf/vf.h
index 5b172427f459..d40f036b6df0 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 82599 Virtual Function driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 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,
@@ -13,8 +13,7 @@
   more details.
 
   You should have 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.
+  this program; if not, see <http://www.gnu.org/licenses/>.
 
   The full GNU General Public License is included in this distribution in
   the file called "COPYING".
@@ -169,7 +168,7 @@ struct ixgbevf_hw_stats {
 };
 
 struct ixgbevf_info {
-	enum ixgbe_mac_type		mac;
+	enum ixgbe_mac_type mac;
 	const struct ixgbe_mac_operations *mac_ops;
 };
 
@@ -185,28 +184,32 @@ static inline void ixgbe_write_reg(struct ixgbe_hw *hw, u32 reg, u32 value)
 		return;
 	writel(value, reg_addr + reg);
 }
+
 #define IXGBE_WRITE_REG(h, r, v) ixgbe_write_reg(h, r, v)
 
 u32 ixgbevf_read_reg(struct ixgbe_hw *hw, u32 reg);
 #define IXGBE_READ_REG(h, r) ixgbevf_read_reg(h, r)
 
 static inline void ixgbe_write_reg_array(struct ixgbe_hw *hw, u32 reg,
-					  u32 offset, u32 value)
+					 u32 offset, u32 value)
 {
 	ixgbe_write_reg(hw, reg + (offset << 2), value);
 }
+
 #define IXGBE_WRITE_REG_ARRAY(h, r, o, v) ixgbe_write_reg_array(h, r, o, v)
 
 static inline u32 ixgbe_read_reg_array(struct ixgbe_hw *hw, u32 reg,
-					u32 offset)
+				       u32 offset)
 {
 	return ixgbevf_read_reg(hw, reg + (offset << 2));
 }
+
 #define IXGBE_READ_REG_ARRAY(h, r, o) ixgbe_read_reg_array(h, r, o)
 
 void ixgbevf_rlpml_set_vf(struct ixgbe_hw *hw, u16 max_size);
 int ixgbevf_negotiate_api_version(struct ixgbe_hw *hw, int api);
 int ixgbevf_get_queues(struct ixgbe_hw *hw, unsigned int *num_tcs,
 		       unsigned int *default_tc);
+int ixgbevf_get_reta_locked(struct ixgbe_hw *hw, u32 *reta, int num_rx_queues);
+int ixgbevf_get_rss_key_locked(struct ixgbe_hw *hw, u8 *rss_key);
 #endif /* __IXGBE_VF_H__ */
-
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index 96208f17bb53..ce5f7f9cff06 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -100,6 +100,8 @@
 #define MVNETA_TXQ_CMD                           0x2448
 #define      MVNETA_TXQ_DISABLE_SHIFT            8
 #define      MVNETA_TXQ_ENABLE_MASK              0x000000ff
+#define MVNETA_GMAC_CLOCK_DIVIDER                0x24f4
+#define      MVNETA_GMAC_1MS_CLOCK_ENABLE        BIT(31)
 #define MVNETA_ACC_MODE                          0x2500
 #define MVNETA_CPU_MAP(cpu)                      (0x2540 + ((cpu) << 2))
 #define      MVNETA_CPU_RXQ_ACCESS_ALL_MASK      0x000000ff
@@ -122,6 +124,7 @@
 #define      MVNETA_TX_INTR_MASK_ALL             (0xff << 0)
 #define      MVNETA_RX_INTR_MASK(nr_rxqs)        (((1 << nr_rxqs) - 1) << 8)
 #define      MVNETA_RX_INTR_MASK_ALL             (0xff << 8)
+#define      MVNETA_MISCINTR_INTR_MASK           BIT(31)
 
 #define MVNETA_INTR_OLD_CAUSE                    0x25a8
 #define MVNETA_INTR_OLD_MASK                     0x25ac
@@ -165,6 +168,7 @@
 #define      MVNETA_GMAC_MAX_RX_SIZE_MASK        0x7ffc
 #define      MVNETA_GMAC0_PORT_ENABLE            BIT(0)
 #define MVNETA_GMAC_CTRL_2                       0x2c08
+#define      MVNETA_GMAC2_INBAND_AN_ENABLE       BIT(0)
 #define      MVNETA_GMAC2_PCS_ENABLE             BIT(3)
 #define      MVNETA_GMAC2_PORT_RGMII             BIT(4)
 #define      MVNETA_GMAC2_PORT_RESET             BIT(6)
@@ -180,9 +184,11 @@
 #define MVNETA_GMAC_AUTONEG_CONFIG               0x2c0c
 #define      MVNETA_GMAC_FORCE_LINK_DOWN         BIT(0)
 #define      MVNETA_GMAC_FORCE_LINK_PASS         BIT(1)
+#define      MVNETA_GMAC_INBAND_AN_ENABLE        BIT(2)
 #define      MVNETA_GMAC_CONFIG_MII_SPEED        BIT(5)
 #define      MVNETA_GMAC_CONFIG_GMII_SPEED       BIT(6)
 #define      MVNETA_GMAC_AN_SPEED_EN             BIT(7)
+#define      MVNETA_GMAC_AN_FLOW_CTRL_EN         BIT(11)
 #define      MVNETA_GMAC_CONFIG_FULL_DUPLEX      BIT(12)
 #define      MVNETA_GMAC_AN_DUPLEX_EN            BIT(13)
 #define MVNETA_MIB_COUNTERS_BASE                 0x3080
@@ -304,6 +310,7 @@ struct mvneta_port {
 	unsigned int link;
 	unsigned int duplex;
 	unsigned int speed;
+	int use_inband_status:1;
 };
 
 /* The mvneta_tx_desc and mvneta_rx_desc structures describe the
@@ -994,6 +1001,20 @@ static void mvneta_defaults_set(struct mvneta_port *pp)
 	val &= ~MVNETA_PHY_POLLING_ENABLE;
 	mvreg_write(pp, MVNETA_UNIT_CONTROL, val);
 
+	if (pp->use_inband_status) {
+		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
+		val &= ~(MVNETA_GMAC_FORCE_LINK_PASS |
+			 MVNETA_GMAC_FORCE_LINK_DOWN |
+			 MVNETA_GMAC_AN_FLOW_CTRL_EN);
+		val |= MVNETA_GMAC_INBAND_AN_ENABLE |
+		       MVNETA_GMAC_AN_SPEED_EN |
+		       MVNETA_GMAC_AN_DUPLEX_EN;
+		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
+		val = mvreg_read(pp, MVNETA_GMAC_CLOCK_DIVIDER);
+		val |= MVNETA_GMAC_1MS_CLOCK_ENABLE;
+		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER, val);
+	}
+
 	mvneta_set_ucast_table(pp, -1);
 	mvneta_set_special_mcast_table(pp, -1);
 	mvneta_set_other_mcast_table(pp, -1);
@@ -2043,6 +2064,28 @@ static irqreturn_t mvneta_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static int mvneta_fixed_link_update(struct mvneta_port *pp,
+				    struct phy_device *phy)
+{
+	struct fixed_phy_status status;
+	struct fixed_phy_status changed = {};
+	u32 gmac_stat = mvreg_read(pp, MVNETA_GMAC_STATUS);
+
+	status.link = !!(gmac_stat & MVNETA_GMAC_LINK_UP);
+	if (gmac_stat & MVNETA_GMAC_SPEED_1000)
+		status.speed = SPEED_1000;
+	else if (gmac_stat & MVNETA_GMAC_SPEED_100)
+		status.speed = SPEED_100;
+	else
+		status.speed = SPEED_10;
+	status.duplex = !!(gmac_stat & MVNETA_GMAC_FULL_DUPLEX);
+	changed.link = 1;
+	changed.speed = 1;
+	changed.duplex = 1;
+	fixed_phy_update_state(phy, &status, &changed);
+	return 0;
+}
+
 /* NAPI handler
  * Bits 0 - 7 of the causeRxTx register indicate that are transmitted
  * packets on the corresponding TXQ (Bit 0 is for TX queue 1).
@@ -2063,8 +2106,18 @@ static int mvneta_poll(struct napi_struct *napi, int budget)
 	}
 
 	/* Read cause register */
-	cause_rx_tx = mvreg_read(pp, MVNETA_INTR_NEW_CAUSE) &
-		(MVNETA_RX_INTR_MASK(rxq_number) | MVNETA_TX_INTR_MASK(txq_number));
+	cause_rx_tx = mvreg_read(pp, MVNETA_INTR_NEW_CAUSE);
+	if (cause_rx_tx & MVNETA_MISCINTR_INTR_MASK) {
+		u32 cause_misc = mvreg_read(pp, MVNETA_INTR_MISC_CAUSE);
+
+		mvreg_write(pp, MVNETA_INTR_MISC_CAUSE, 0);
+		if (pp->use_inband_status && (cause_misc &
+				(MVNETA_CAUSE_PHY_STATUS_CHANGE |
+				 MVNETA_CAUSE_LINK_CHANGE |
+				 MVNETA_CAUSE_PSC_SYNC_CHANGE))) {
+			mvneta_fixed_link_update(pp, pp->phy_dev);
+		}
+	}
 
 	/* Release Tx descriptors */
 	if (cause_rx_tx & MVNETA_TX_INTR_MASK_ALL) {
@@ -2109,7 +2162,9 @@ static int mvneta_poll(struct napi_struct *napi, int budget)
 		napi_complete(napi);
 		local_irq_save(flags);
 		mvreg_write(pp, MVNETA_INTR_NEW_MASK,
-			    MVNETA_RX_INTR_MASK(rxq_number) | MVNETA_TX_INTR_MASK(txq_number));
+			    MVNETA_RX_INTR_MASK(rxq_number) |
+			    MVNETA_TX_INTR_MASK(txq_number) |
+			    MVNETA_MISCINTR_INTR_MASK);
 		local_irq_restore(flags);
 	}
 
@@ -2373,7 +2428,13 @@ static void mvneta_start_dev(struct mvneta_port *pp)
 
 	/* Unmask interrupts */
 	mvreg_write(pp, MVNETA_INTR_NEW_MASK,
-		    MVNETA_RX_INTR_MASK(rxq_number) | MVNETA_TX_INTR_MASK(txq_number));
+		    MVNETA_RX_INTR_MASK(rxq_number) |
+		    MVNETA_TX_INTR_MASK(txq_number) |
+		    MVNETA_MISCINTR_INTR_MASK);
+	mvreg_write(pp, MVNETA_INTR_MISC_MASK,
+		    MVNETA_CAUSE_PHY_STATUS_CHANGE |
+		    MVNETA_CAUSE_LINK_CHANGE |
+		    MVNETA_CAUSE_PSC_SYNC_CHANGE);
 
 	phy_start(pp->phy_dev);
 	netif_tx_start_all_queues(pp->dev);
@@ -2523,9 +2584,7 @@ static void mvneta_adjust_link(struct net_device *ndev)
 			val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
 			val &= ~(MVNETA_GMAC_CONFIG_MII_SPEED |
 				 MVNETA_GMAC_CONFIG_GMII_SPEED |
-				 MVNETA_GMAC_CONFIG_FULL_DUPLEX |
-				 MVNETA_GMAC_AN_SPEED_EN |
-				 MVNETA_GMAC_AN_DUPLEX_EN);
+				 MVNETA_GMAC_CONFIG_FULL_DUPLEX);
 
 			if (phydev->duplex)
 				val |= MVNETA_GMAC_CONFIG_FULL_DUPLEX;
@@ -2554,12 +2613,24 @@ static void mvneta_adjust_link(struct net_device *ndev)
 
 	if (status_change) {
 		if (phydev->link) {
-			u32 val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
-			val |= (MVNETA_GMAC_FORCE_LINK_PASS |
-				MVNETA_GMAC_FORCE_LINK_DOWN);
-			mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
+			if (!pp->use_inband_status) {
+				u32 val = mvreg_read(pp,
+						  MVNETA_GMAC_AUTONEG_CONFIG);
+				val &= ~MVNETA_GMAC_FORCE_LINK_DOWN;
+				val |= MVNETA_GMAC_FORCE_LINK_PASS;
+				mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG,
+					    val);
+			}
 			mvneta_port_up(pp);
 		} else {
+			if (!pp->use_inband_status) {
+				u32 val = mvreg_read(pp,
+						  MVNETA_GMAC_AUTONEG_CONFIG);
+				val &= ~MVNETA_GMAC_FORCE_LINK_PASS;
+				val |= MVNETA_GMAC_FORCE_LINK_DOWN;
+				mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG,
+					    val);
+			}
 			mvneta_port_down(pp);
 		}
 		phy_print_status(phydev);
@@ -2658,16 +2729,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 */
@@ -2910,6 +2976,9 @@ static int mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
 		return -EINVAL;
 	}
 
+	if (pp->use_inband_status)
+		ctrl |= MVNETA_GMAC2_INBAND_AN_ENABLE;
+
 	/* Cancel Port Reset */
 	ctrl &= ~MVNETA_GMAC2_PORT_RESET;
 	mvreg_write(pp, MVNETA_GMAC_CTRL_2, ctrl);
@@ -2934,6 +3003,7 @@ static int mvneta_probe(struct platform_device *pdev)
 	char hw_mac_addr[ETH_ALEN];
 	const char *mac_from;
 	int phy_mode;
+	int fixed_phy = 0;
 	int err;
 
 	/* Our multiqueue support is not complete, so for now, only
@@ -2967,6 +3037,7 @@ static int mvneta_probe(struct platform_device *pdev)
 			dev_err(&pdev->dev, "cannot register fixed PHY\n");
 			goto err_free_irq;
 		}
+		fixed_phy = 1;
 
 		/* In the case of a fixed PHY, the DT node associated
 		 * to the PHY is the Ethernet MAC DT node.
@@ -2990,6 +3061,8 @@ static int mvneta_probe(struct platform_device *pdev)
 	pp = netdev_priv(dev);
 	pp->phy_node = phy_node;
 	pp->phy_interface = phy_mode;
+	pp->use_inband_status = (phy_mode == PHY_INTERFACE_MODE_SGMII) &&
+				fixed_phy;
 
 	pp->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(pp->clk)) {
@@ -3067,6 +3140,12 @@ static int mvneta_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, pp->dev);
 
+	if (pp->use_inband_status) {
+		struct phy_device *phy = of_phy_find_device(dn);
+
+		mvneta_fixed_link_update(pp, phy);
+	}
+
 	return 0;
 
 err_free_stats:
diff --git a/drivers/net/ethernet/marvell/mvpp2.c b/drivers/net/ethernet/marvell/mvpp2.c
index fdf3e382e464..3e8b1bfb1f2e 100644
--- a/drivers/net/ethernet/marvell/mvpp2.c
+++ b/drivers/net/ethernet/marvell/mvpp2.c
@@ -1423,7 +1423,7 @@ static void mvpp2_prs_mac_promisc_set(struct mvpp2 *priv, int port, bool add)
 {
 	struct mvpp2_prs_entry pe;
 
-	/* Promiscous mode - Accept unknown packets */
+	/* Promiscuous mode - Accept unknown packets */
 
 	if (priv->prs_shadow[MVPP2_PE_MAC_PROMISCUOUS].valid) {
 		/* Entry exist - update port only */
@@ -3402,7 +3402,7 @@ static void mvpp2_bm_bufs_free(struct mvpp2 *priv, struct mvpp2_bm_pool *bm_pool
 	for (i = 0; i < bm_pool->buf_num; i++) {
 		u32 vaddr;
 
-		/* Get buffer virtual adress (indirect access) */
+		/* Get buffer virtual address (indirect access) */
 		mvpp2_read(priv, MVPP2_BM_PHY_ALLOC_REG(bm_pool->id));
 		vaddr = mvpp2_read(priv, MVPP2_BM_VIRT_ALLOC_REG);
 		if (!vaddr)
diff --git a/drivers/net/ethernet/mellanox/mlx4/Makefile b/drivers/net/ethernet/mellanox/mlx4/Makefile
index 3e9c70f15b42..c82217e0d22d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx4/Makefile
@@ -1,7 +1,8 @@
 obj-$(CONFIG_MLX4_CORE)		+= mlx4_core.o
 
-mlx4_core-y :=	alloc.o catas.o cmd.o cq.o eq.o fw.o icm.o intf.o main.o mcg.o \
-		mr.o pd.o port.o profile.o qp.o reset.o sense.o srq.o resource_tracker.o
+mlx4_core-y :=	alloc.o catas.o cmd.o cq.o eq.o fw.o fw_qos.o icm.o intf.o \
+		main.o mcg.o mr.o pd.o port.o profile.o qp.o reset.o sense.o \
+		srq.o resource_tracker.o
 
 obj-$(CONFIG_MLX4_EN)               += mlx4_en.o
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index a681d7c0bb9f..f0fbb4ade85d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -48,6 +48,7 @@
 
 #include "mlx4.h"
 #include "fw.h"
+#include "fw_qos.h"
 
 #define CMD_POLL_TOKEN 0xffff
 #define INBOX_MASK	0xffffffffffffff00ULL
@@ -724,8 +725,10 @@ 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 &&
-		    op_modifier == 0 && context->fw_status == CMD_STAT_BAD_SIZE)
+		if (op == MLX4_CMD_SET_PORT &&
+		    (in_modifier == 1 || in_modifier == 2) &&
+		    op_modifier == MLX4_SET_PORT_IB_OPCODE &&
+		    context->fw_status == CMD_STAT_BAD_SIZE)
 			mlx4_dbg(dev, "command 0x%x failed: fw status = 0x%x\n",
 				 op, context->fw_status);
 		else
@@ -1455,6 +1458,24 @@ static struct mlx4_cmd_info cmd_info[] = {
 		.wrapper = mlx4_CMD_EPERM_wrapper,
 	},
 	{
+		.opcode = MLX4_CMD_ALLOCATE_VPP,
+		.has_inbox = false,
+		.has_outbox = true,
+		.out_is_imm = false,
+		.encode_slave_id = false,
+		.verify = NULL,
+		.wrapper = mlx4_CMD_EPERM_wrapper,
+	},
+	{
+		.opcode = MLX4_CMD_SET_VPORT_QOS,
+		.has_inbox = false,
+		.has_outbox = true,
+		.out_is_imm = false,
+		.encode_slave_id = false,
+		.verify = NULL,
+		.wrapper = mlx4_CMD_EPERM_wrapper,
+	},
+	{
 		.opcode = MLX4_CMD_CONF_SPECIAL_QP,
 		.has_inbox = false,
 		.has_outbox = false,
@@ -1499,6 +1520,15 @@ static struct mlx4_cmd_info cmd_info[] = {
 		.verify = NULL,
 		.wrapper = mlx4_ACCESS_REG_wrapper,
 	},
+	{
+		.opcode = MLX4_CMD_CONGESTION_CTRL_OPCODE,
+		.has_inbox = false,
+		.has_outbox = false,
+		.out_is_imm = false,
+		.encode_slave_id = false,
+		.verify = NULL,
+		.wrapper = mlx4_CMD_EPERM_wrapper,
+	},
 	/* Native multicast commands are not available for guests */
 	{
 		.opcode = MLX4_CMD_QP_ATTACH,
@@ -1781,7 +1811,8 @@ static int mlx4_master_immediate_activate_vlan_qos(struct mlx4_priv *priv,
 
 	if (vp_oper->state.default_vlan == vp_admin->default_vlan &&
 	    vp_oper->state.default_qos == vp_admin->default_qos &&
-	    vp_oper->state.link_state == vp_admin->link_state)
+	    vp_oper->state.link_state == vp_admin->link_state &&
+	    vp_oper->state.qos_vport == vp_admin->qos_vport)
 		return 0;
 
 	if (!(priv->mfunc.master.slave_state[slave].active &&
@@ -1839,6 +1870,7 @@ static int mlx4_master_immediate_activate_vlan_qos(struct mlx4_priv *priv,
 	vp_oper->state.default_vlan = vp_admin->default_vlan;
 	vp_oper->state.default_qos = vp_admin->default_qos;
 	vp_oper->state.link_state = vp_admin->link_state;
+	vp_oper->state.qos_vport = vp_admin->qos_vport;
 
 	if (vp_admin->link_state == IFLA_VF_LINK_STATE_DISABLE)
 		work->flags |= MLX4_VF_IMMED_VLAN_FLAG_LINK_DISABLE;
@@ -1847,6 +1879,7 @@ static int mlx4_master_immediate_activate_vlan_qos(struct mlx4_priv *priv,
 	work->port = port;
 	work->slave = slave;
 	work->qos = vp_oper->state.default_qos;
+	work->qos_vport = vp_oper->state.qos_vport;
 	work->vlan_id = vp_oper->state.default_vlan;
 	work->vlan_ix = vp_oper->vlan_idx;
 	work->priv = priv;
@@ -1856,6 +1889,63 @@ static int mlx4_master_immediate_activate_vlan_qos(struct mlx4_priv *priv,
 	return 0;
 }
 
+static void mlx4_set_default_port_qos(struct mlx4_dev *dev, int port)
+{
+	struct mlx4_qos_manager *port_qos_ctl;
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	port_qos_ctl = &priv->mfunc.master.qos_ctl[port];
+	bitmap_zero(port_qos_ctl->priority_bm, MLX4_NUM_UP);
+
+	/* Enable only default prio at PF init routine */
+	set_bit(MLX4_DEFAULT_QOS_PRIO, port_qos_ctl->priority_bm);
+}
+
+static void mlx4_allocate_port_vpps(struct mlx4_dev *dev, int port)
+{
+	int i;
+	int err;
+	int num_vfs;
+	u16 availible_vpp;
+	u8 vpp_param[MLX4_NUM_UP];
+	struct mlx4_qos_manager *port_qos;
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	err = mlx4_ALLOCATE_VPP_get(dev, port, &availible_vpp, vpp_param);
+	if (err) {
+		mlx4_info(dev, "Failed query availible VPPs\n");
+		return;
+	}
+
+	port_qos = &priv->mfunc.master.qos_ctl[port];
+	num_vfs = (availible_vpp /
+		   bitmap_weight(port_qos->priority_bm, MLX4_NUM_UP));
+
+	for (i = 0; i < MLX4_NUM_UP; i++) {
+		if (test_bit(i, port_qos->priority_bm))
+			vpp_param[i] = num_vfs;
+	}
+
+	err = mlx4_ALLOCATE_VPP_set(dev, port, vpp_param);
+	if (err) {
+		mlx4_info(dev, "Failed allocating VPPs\n");
+		return;
+	}
+
+	/* Query actual allocated VPP, just to make sure */
+	err = mlx4_ALLOCATE_VPP_get(dev, port, &availible_vpp, vpp_param);
+	if (err) {
+		mlx4_info(dev, "Failed query availible VPPs\n");
+		return;
+	}
+
+	port_qos->num_of_qos_vfs = num_vfs;
+	mlx4_dbg(dev, "Port %d Availible VPPs %d\n", port, availible_vpp);
+
+	for (i = 0; i < MLX4_NUM_UP; i++)
+		mlx4_dbg(dev, "Port %d UP %d Allocated %d VPPs\n", port, i,
+			 vpp_param[i]);
+}
 
 static int mlx4_master_activate_admin_state(struct mlx4_priv *priv, int slave)
 {
@@ -1993,7 +2083,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)
@@ -2204,6 +2293,9 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 	}
 
 	if (mlx4_is_master(dev)) {
+		struct mlx4_vf_oper_state *vf_oper;
+		struct mlx4_vf_admin_state *vf_admin;
+
 		priv->mfunc.master.slave_state =
 			kzalloc(dev->num_slaves *
 				sizeof(struct mlx4_slave_state), GFP_KERNEL);
@@ -2223,8 +2315,11 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 			goto err_comm_oper;
 
 		for (i = 0; i < dev->num_slaves; ++i) {
+			vf_admin = &priv->mfunc.master.vf_admin[i];
+			vf_oper = &priv->mfunc.master.vf_oper[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,
@@ -2233,6 +2328,9 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 				     &priv->mfunc.comm[i].slave_read);
 			mmiowb();
 			for (port = 1; port <= MLX4_MAX_PORTS; port++) {
+				struct mlx4_vport_state *admin_vport;
+				struct mlx4_vport_state *oper_vport;
+
 				s_state->vlan_filter[port] =
 					kzalloc(sizeof(struct mlx4_vlan_fltr),
 						GFP_KERNEL);
@@ -2241,15 +2339,30 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 						kfree(s_state->vlan_filter[port]);
 					goto err_slaves;
 				}
+
+				admin_vport = &vf_admin->vport[port];
+				oper_vport = &vf_oper->vport[port].state;
 				INIT_LIST_HEAD(&s_state->mcast_filters[port]);
-				priv->mfunc.master.vf_admin[i].vport[port].default_vlan = MLX4_VGT;
-				priv->mfunc.master.vf_oper[i].vport[port].state.default_vlan = MLX4_VGT;
-				priv->mfunc.master.vf_oper[i].vport[port].vlan_idx = NO_INDX;
-				priv->mfunc.master.vf_oper[i].vport[port].mac_idx = NO_INDX;
+				admin_vport->default_vlan = MLX4_VGT;
+				oper_vport->default_vlan = MLX4_VGT;
+				admin_vport->qos_vport =
+						MLX4_VPP_DEFAULT_VPORT;
+				oper_vport->qos_vport = MLX4_VPP_DEFAULT_VPORT;
+				vf_oper->vport[port].vlan_idx = NO_INDX;
+				vf_oper->vport[port].mac_idx = NO_INDX;
 			}
 			spin_lock_init(&s_state->lock);
 		}
 
+		if (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_QOS_VPP) {
+			for (port = 1; port <= dev->caps.num_ports; port++) {
+				if (mlx4_is_eth(dev, port)) {
+					mlx4_set_default_port_qos(dev, port);
+					mlx4_allocate_port_vpps(dev, port);
+				}
+			}
+		}
+
 		memset(&priv->mfunc.master.cmd_eqe, 0, dev->caps.eqe_size);
 		priv->mfunc.master.cmd_eqe.type = MLX4_EVENT_TYPE_CMD;
 		INIT_WORK(&priv->mfunc.master.comm_work,
@@ -2670,6 +2783,103 @@ static int mlx4_slaves_closest_port(struct mlx4_dev *dev, int slave, int port)
 	return port;
 }
 
+static int mlx4_set_vport_qos(struct mlx4_priv *priv, int slave, int port,
+			      int max_tx_rate)
+{
+	int i;
+	int err;
+	struct mlx4_qos_manager *port_qos;
+	struct mlx4_dev *dev = &priv->dev;
+	struct mlx4_vport_qos_param vpp_qos[MLX4_NUM_UP];
+
+	port_qos = &priv->mfunc.master.qos_ctl[port];
+	memset(vpp_qos, 0, sizeof(struct mlx4_vport_qos_param) * MLX4_NUM_UP);
+
+	if (slave > port_qos->num_of_qos_vfs) {
+		mlx4_info(dev, "No availible VPP resources for this VF\n");
+		return -EINVAL;
+	}
+
+	/* Query for default QoS values from Vport 0 is needed */
+	err = mlx4_SET_VPORT_QOS_get(dev, port, 0, vpp_qos);
+	if (err) {
+		mlx4_info(dev, "Failed to query Vport 0 QoS values\n");
+		return err;
+	}
+
+	for (i = 0; i < MLX4_NUM_UP; i++) {
+		if (test_bit(i, port_qos->priority_bm) && max_tx_rate) {
+			vpp_qos[i].max_avg_bw = max_tx_rate;
+			vpp_qos[i].enable = 1;
+		} else {
+			/* if user supplied tx_rate == 0, meaning no rate limit
+			 * configuration is required. so we are leaving the
+			 * value of max_avg_bw as queried from Vport 0.
+			 */
+			vpp_qos[i].enable = 0;
+		}
+	}
+
+	err = mlx4_SET_VPORT_QOS_set(dev, port, slave, vpp_qos);
+	if (err) {
+		mlx4_info(dev, "Failed to set Vport %d QoS values\n", slave);
+		return err;
+	}
+
+	return 0;
+}
+
+static bool mlx4_is_vf_vst_and_prio_qos(struct mlx4_dev *dev, int port,
+					struct mlx4_vport_state *vf_admin)
+{
+	struct mlx4_qos_manager *info;
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	if (!mlx4_is_master(dev) ||
+	    !(dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_QOS_VPP))
+		return false;
+
+	info = &priv->mfunc.master.qos_ctl[port];
+
+	if (vf_admin->default_vlan != MLX4_VGT &&
+	    test_bit(vf_admin->default_qos, info->priority_bm))
+		return true;
+
+	return false;
+}
+
+static bool mlx4_valid_vf_state_change(struct mlx4_dev *dev, int port,
+				       struct mlx4_vport_state *vf_admin,
+				       int vlan, int qos)
+{
+	struct mlx4_vport_state dummy_admin = {0};
+
+	if (!mlx4_is_vf_vst_and_prio_qos(dev, port, vf_admin) ||
+	    !vf_admin->tx_rate)
+		return true;
+
+	dummy_admin.default_qos = qos;
+	dummy_admin.default_vlan = vlan;
+
+	/* VF wants to move to other VST state which is valid with current
+	 * rate limit. Either differnt default vlan in VST or other
+	 * supported QoS priority. Otherwise we don't allow this change when
+	 * the TX rate is still configured.
+	 */
+	if (mlx4_is_vf_vst_and_prio_qos(dev, port, &dummy_admin))
+		return true;
+
+	mlx4_info(dev, "Cannot change VF state to %s while rate is set\n",
+		  (vlan == MLX4_VGT) ? "VGT" : "VST");
+
+	if (vlan != MLX4_VGT)
+		mlx4_info(dev, "VST priority %d not supported for QoS\n", qos);
+
+	mlx4_info(dev, "Please set rate to 0 prior to this VF state change\n");
+
+	return false;
+}
+
 int mlx4_set_vf_mac(struct mlx4_dev *dev, int port, int vf, u64 mac)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -2713,12 +2923,22 @@ int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
 	port = mlx4_slaves_closest_port(dev, slave, port);
 	vf_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
 
+	if (!mlx4_valid_vf_state_change(dev, port, vf_admin, vlan, qos))
+		return -EPERM;
+
 	if ((0 == vlan) && (0 == qos))
 		vf_admin->default_vlan = MLX4_VGT;
 	else
 		vf_admin->default_vlan = vlan;
 	vf_admin->default_qos = qos;
 
+	/* If rate was configured prior to VST, we saved the configured rate
+	 * in vf_admin->rate and now, if priority supported we enforce the QoS
+	 */
+	if (mlx4_is_vf_vst_and_prio_qos(dev, port, vf_admin) &&
+	    vf_admin->tx_rate)
+		vf_admin->qos_vport = slave;
+
 	if (mlx4_master_immediate_activate_vlan_qos(priv, slave, port))
 		mlx4_info(dev,
 			  "updating vf %d port %d config will take effect on next VF restart\n",
@@ -2727,6 +2947,69 @@ int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
 }
 EXPORT_SYMBOL_GPL(mlx4_set_vf_vlan);
 
+int mlx4_set_vf_rate(struct mlx4_dev *dev, int port, int vf, int min_tx_rate,
+		     int max_tx_rate)
+{
+	int err;
+	int slave;
+	struct mlx4_vport_state *vf_admin;
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	if (!mlx4_is_master(dev) ||
+	    !(dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_QOS_VPP))
+		return -EPROTONOSUPPORT;
+
+	if (min_tx_rate) {
+		mlx4_info(dev, "Minimum BW share not supported\n");
+		return -EPROTONOSUPPORT;
+	}
+
+	slave = mlx4_get_slave_indx(dev, vf);
+	if (slave < 0)
+		return -EINVAL;
+
+	port = mlx4_slaves_closest_port(dev, slave, port);
+	vf_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
+
+	err = mlx4_set_vport_qos(priv, slave, port, max_tx_rate);
+	if (err) {
+		mlx4_info(dev, "vf %d failed to set rate %d\n", vf,
+			  max_tx_rate);
+		return err;
+	}
+
+	vf_admin->tx_rate = max_tx_rate;
+	/* if VF is not in supported mode (VST with supported prio),
+	 * we do not change vport configuration for its QPs, but save
+	 * the rate, so it will be enforced when it moves to supported
+	 * mode next time.
+	 */
+	if (!mlx4_is_vf_vst_and_prio_qos(dev, port, vf_admin)) {
+		mlx4_info(dev,
+			  "rate set for VF %d when not in valid state\n", vf);
+
+		if (vf_admin->default_vlan != MLX4_VGT)
+			mlx4_info(dev, "VST priority not supported by QoS\n");
+		else
+			mlx4_info(dev, "VF in VGT mode (needed VST)\n");
+
+		mlx4_info(dev,
+			  "rate %d take affect when VF moves to valid state\n",
+			  max_tx_rate);
+		return 0;
+	}
+
+	/* If user sets rate 0 assigning default vport for its QPs */
+	vf_admin->qos_vport = max_tx_rate ? slave : MLX4_VPP_DEFAULT_VPORT;
+
+	if (priv->mfunc.master.slave_state[slave].active &&
+	    dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_UPDATE_QP)
+		mlx4_master_immediate_activate_vlan_qos(priv, slave, port);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx4_set_vf_rate);
+
  /* mlx4_get_slave_default_vlan -
  * return true if VST ( default vlan)
  * if VST, will return vlan & qos (if not NULL)
@@ -2800,7 +3083,12 @@ int mlx4_get_vf_config(struct mlx4_dev *dev, int port, int vf, struct ifla_vf_in
 
 	ivf->vlan		= s_info->default_vlan;
 	ivf->qos		= s_info->default_qos;
-	ivf->max_tx_rate	= s_info->tx_rate;
+
+	if (mlx4_is_vf_vst_and_prio_qos(dev, port, s_info))
+		ivf->max_tx_rate = s_info->tx_rate;
+	else
+		ivf->max_tx_rate = 0;
+
 	ivf->min_tx_rate	= 0;
 	ivf->spoofchk		= s_info->spoofchk;
 	ivf->linkstate		= s_info->link_state;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_clock.c b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
index 90b5309cdb5c..8a083d73efdb 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_clock.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
@@ -164,20 +164,19 @@ static int mlx4_en_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)
  * Read the timecounter and return the correct value in ns after converting
  * it into a struct timespec.
  **/
-static int mlx4_en_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int mlx4_en_phc_gettime(struct ptp_clock_info *ptp,
+			       struct timespec64 *ts)
 {
 	struct mlx4_en_dev *mdev = container_of(ptp, struct mlx4_en_dev,
 						ptp_clock_info);
 	unsigned long flags;
-	u32 remainder;
 	u64 ns;
 
 	write_lock_irqsave(&mdev->clock_lock, flags);
 	ns = timecounter_read(&mdev->clock);
 	write_unlock_irqrestore(&mdev->clock_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, NSEC_PER_SEC, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
@@ -191,11 +190,11 @@ static int mlx4_en_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
  * wall timer value.
  **/
 static int mlx4_en_phc_settime(struct ptp_clock_info *ptp,
-			       const struct timespec *ts)
+			       const struct timespec64 *ts)
 {
 	struct mlx4_en_dev *mdev = container_of(ptp, struct mlx4_en_dev,
 						ptp_clock_info);
-	u64 ns = timespec_to_ns(ts);
+	u64 ns = timespec64_to_ns(ts);
 	unsigned long flags;
 
 	/* reset the timecounter */
@@ -232,8 +231,8 @@ static const struct ptp_clock_info mlx4_en_ptp_clock_info = {
 	.pps		= 0,
 	.adjfreq	= mlx4_en_phc_adjfreq,
 	.adjtime	= mlx4_en_phc_adjtime,
-	.gettime	= mlx4_en_phc_gettime,
-	.settime	= mlx4_en_phc_settime,
+	.gettime64	= mlx4_en_phc_gettime,
+	.settime64	= mlx4_en_phc_settime,
 	.enable		= mlx4_en_phc_enable,
 };
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
index c95ca252187c..f01918c63f28 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
@@ -35,6 +35,50 @@
 #include <linux/math64.h>
 
 #include "mlx4_en.h"
+#include "fw_qos.h"
+
+/* Definitions for QCN
+ */
+
+struct mlx4_congestion_control_mb_prio_802_1_qau_params {
+	__be32 modify_enable_high;
+	__be32 modify_enable_low;
+	__be32 reserved1;
+	__be32 extended_enable;
+	__be32 rppp_max_rps;
+	__be32 rpg_time_reset;
+	__be32 rpg_byte_reset;
+	__be32 rpg_threshold;
+	__be32 rpg_max_rate;
+	__be32 rpg_ai_rate;
+	__be32 rpg_hai_rate;
+	__be32 rpg_gd;
+	__be32 rpg_min_dec_fac;
+	__be32 rpg_min_rate;
+	__be32 max_time_rise;
+	__be32 max_byte_rise;
+	__be32 max_qdelta;
+	__be32 min_qoffset;
+	__be32 gd_coefficient;
+	__be32 reserved2[5];
+	__be32 cp_sample_base;
+	__be32 reserved3[39];
+};
+
+struct mlx4_congestion_control_mb_prio_802_1_qau_statistics {
+	__be64 rppp_rp_centiseconds;
+	__be32 reserved1;
+	__be32 ignored_cnm;
+	__be32 rppp_created_rps;
+	__be32 estimated_total_rate;
+	__be32 max_active_rate_limiter_index;
+	__be32 dropped_cnms_busy_fw;
+	__be32 reserved2;
+	__be32 cnms_handled_successfully;
+	__be32 min_total_limiters_rate;
+	__be32 max_total_limiters_rate;
+	__be32 reserved3[4];
+};
 
 static int mlx4_en_dcbnl_ieee_getets(struct net_device *dev,
 				   struct ieee_ets *ets)
@@ -183,6 +227,10 @@ static int mlx4_en_dcbnl_ieee_setpfc(struct net_device *dev,
 				    prof->rx_ppp);
 	if (err)
 		en_err(priv, "Failed setting pause params\n");
+	else
+		mlx4_en_update_pfc_stats_bitmap(mdev->dev, &priv->stats_bitmap,
+						prof->rx_ppp, prof->rx_pause,
+						prof->tx_ppp, prof->tx_pause);
 
 	return err;
 }
@@ -242,6 +290,178 @@ static int mlx4_en_dcbnl_ieee_setmaxrate(struct net_device *dev,
 	return 0;
 }
 
+#define RPG_ENABLE_BIT	31
+#define CN_TAG_BIT	30
+
+static int mlx4_en_dcbnl_ieee_getqcn(struct net_device *dev,
+				     struct ieee_qcn *qcn)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_congestion_control_mb_prio_802_1_qau_params *hw_qcn;
+	struct mlx4_cmd_mailbox *mailbox_out = NULL;
+	u64 mailbox_in_dma = 0;
+	u32 inmod = 0;
+	int i, err;
+
+	if (!(priv->mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_QCN))
+		return -EOPNOTSUPP;
+
+	mailbox_out = mlx4_alloc_cmd_mailbox(priv->mdev->dev);
+	if (IS_ERR(mailbox_out))
+		return -ENOMEM;
+	hw_qcn =
+	(struct mlx4_congestion_control_mb_prio_802_1_qau_params *)
+	mailbox_out->buf;
+
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
+		inmod = priv->port | ((1 << i) << 8) |
+			 (MLX4_CTRL_ALGO_802_1_QAU_REACTION_POINT << 16);
+		err = mlx4_cmd_box(priv->mdev->dev, mailbox_in_dma,
+				   mailbox_out->dma,
+				   inmod, MLX4_CONGESTION_CONTROL_GET_PARAMS,
+				   MLX4_CMD_CONGESTION_CTRL_OPCODE,
+				   MLX4_CMD_TIME_CLASS_C,
+				   MLX4_CMD_NATIVE);
+		if (err) {
+			mlx4_free_cmd_mailbox(priv->mdev->dev, mailbox_out);
+			return err;
+		}
+
+		qcn->rpg_enable[i] =
+			be32_to_cpu(hw_qcn->extended_enable) >> RPG_ENABLE_BIT;
+		qcn->rppp_max_rps[i] =
+			be32_to_cpu(hw_qcn->rppp_max_rps);
+		qcn->rpg_time_reset[i] =
+			be32_to_cpu(hw_qcn->rpg_time_reset);
+		qcn->rpg_byte_reset[i] =
+			be32_to_cpu(hw_qcn->rpg_byte_reset);
+		qcn->rpg_threshold[i] =
+			be32_to_cpu(hw_qcn->rpg_threshold);
+		qcn->rpg_max_rate[i] =
+			be32_to_cpu(hw_qcn->rpg_max_rate);
+		qcn->rpg_ai_rate[i] =
+			be32_to_cpu(hw_qcn->rpg_ai_rate);
+		qcn->rpg_hai_rate[i] =
+			be32_to_cpu(hw_qcn->rpg_hai_rate);
+		qcn->rpg_gd[i] =
+			be32_to_cpu(hw_qcn->rpg_gd);
+		qcn->rpg_min_dec_fac[i] =
+			be32_to_cpu(hw_qcn->rpg_min_dec_fac);
+		qcn->rpg_min_rate[i] =
+			be32_to_cpu(hw_qcn->rpg_min_rate);
+		qcn->cndd_state_machine[i] =
+			priv->cndd_state[i];
+	}
+	mlx4_free_cmd_mailbox(priv->mdev->dev, mailbox_out);
+	return 0;
+}
+
+static int mlx4_en_dcbnl_ieee_setqcn(struct net_device *dev,
+				     struct ieee_qcn *qcn)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_congestion_control_mb_prio_802_1_qau_params *hw_qcn;
+	struct mlx4_cmd_mailbox *mailbox_in = NULL;
+	u64 mailbox_in_dma = 0;
+	u32 inmod = 0;
+	int i, err;
+#define MODIFY_ENABLE_HIGH_MASK 0xc0000000
+#define MODIFY_ENABLE_LOW_MASK 0xffc00000
+
+	if (!(priv->mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_QCN))
+		return -EOPNOTSUPP;
+
+	mailbox_in = mlx4_alloc_cmd_mailbox(priv->mdev->dev);
+	if (IS_ERR(mailbox_in))
+		return -ENOMEM;
+
+	mailbox_in_dma = mailbox_in->dma;
+	hw_qcn =
+	(struct mlx4_congestion_control_mb_prio_802_1_qau_params *)mailbox_in->buf;
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
+		inmod = priv->port | ((1 << i) << 8) |
+			 (MLX4_CTRL_ALGO_802_1_QAU_REACTION_POINT << 16);
+
+		/* Before updating QCN parameter,
+		 * need to set it's modify enable bit to 1
+		 */
+
+		hw_qcn->modify_enable_high = cpu_to_be32(
+						MODIFY_ENABLE_HIGH_MASK);
+		hw_qcn->modify_enable_low = cpu_to_be32(MODIFY_ENABLE_LOW_MASK);
+
+		hw_qcn->extended_enable = cpu_to_be32(qcn->rpg_enable[i] << RPG_ENABLE_BIT);
+		hw_qcn->rppp_max_rps = cpu_to_be32(qcn->rppp_max_rps[i]);
+		hw_qcn->rpg_time_reset = cpu_to_be32(qcn->rpg_time_reset[i]);
+		hw_qcn->rpg_byte_reset = cpu_to_be32(qcn->rpg_byte_reset[i]);
+		hw_qcn->rpg_threshold = cpu_to_be32(qcn->rpg_threshold[i]);
+		hw_qcn->rpg_max_rate = cpu_to_be32(qcn->rpg_max_rate[i]);
+		hw_qcn->rpg_ai_rate = cpu_to_be32(qcn->rpg_ai_rate[i]);
+		hw_qcn->rpg_hai_rate = cpu_to_be32(qcn->rpg_hai_rate[i]);
+		hw_qcn->rpg_gd = cpu_to_be32(qcn->rpg_gd[i]);
+		hw_qcn->rpg_min_dec_fac = cpu_to_be32(qcn->rpg_min_dec_fac[i]);
+		hw_qcn->rpg_min_rate = cpu_to_be32(qcn->rpg_min_rate[i]);
+		priv->cndd_state[i] = qcn->cndd_state_machine[i];
+		if (qcn->cndd_state_machine[i] == DCB_CNDD_INTERIOR_READY)
+			hw_qcn->extended_enable |= cpu_to_be32(1 << CN_TAG_BIT);
+
+		err = mlx4_cmd(priv->mdev->dev, mailbox_in_dma, inmod,
+			       MLX4_CONGESTION_CONTROL_SET_PARAMS,
+			       MLX4_CMD_CONGESTION_CTRL_OPCODE,
+			       MLX4_CMD_TIME_CLASS_C,
+			       MLX4_CMD_NATIVE);
+		if (err) {
+			mlx4_free_cmd_mailbox(priv->mdev->dev, mailbox_in);
+			return err;
+		}
+	}
+	mlx4_free_cmd_mailbox(priv->mdev->dev, mailbox_in);
+	return 0;
+}
+
+static int mlx4_en_dcbnl_ieee_getqcnstats(struct net_device *dev,
+					  struct ieee_qcn_stats *qcn_stats)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_congestion_control_mb_prio_802_1_qau_statistics *hw_qcn_stats;
+	struct mlx4_cmd_mailbox *mailbox_out = NULL;
+	u64 mailbox_in_dma = 0;
+	u32 inmod = 0;
+	int i, err;
+
+	if (!(priv->mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_QCN))
+		return -EOPNOTSUPP;
+
+	mailbox_out = mlx4_alloc_cmd_mailbox(priv->mdev->dev);
+	if (IS_ERR(mailbox_out))
+		return -ENOMEM;
+
+	hw_qcn_stats =
+	(struct mlx4_congestion_control_mb_prio_802_1_qau_statistics *)
+	mailbox_out->buf;
+
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
+		inmod = priv->port | ((1 << i) << 8) |
+			 (MLX4_CTRL_ALGO_802_1_QAU_REACTION_POINT << 16);
+		err = mlx4_cmd_box(priv->mdev->dev, mailbox_in_dma,
+				   mailbox_out->dma, inmod,
+				   MLX4_CONGESTION_CONTROL_GET_STATISTICS,
+				   MLX4_CMD_CONGESTION_CTRL_OPCODE,
+				   MLX4_CMD_TIME_CLASS_C,
+				   MLX4_CMD_NATIVE);
+		if (err) {
+			mlx4_free_cmd_mailbox(priv->mdev->dev, mailbox_out);
+			return err;
+		}
+		qcn_stats->rppp_rp_centiseconds[i] =
+			be64_to_cpu(hw_qcn_stats->rppp_rp_centiseconds);
+		qcn_stats->rppp_created_rps[i] =
+			be32_to_cpu(hw_qcn_stats->rppp_created_rps);
+	}
+	mlx4_free_cmd_mailbox(priv->mdev->dev, mailbox_out);
+	return 0;
+}
+
 const struct dcbnl_rtnl_ops mlx4_en_dcbnl_ops = {
 	.ieee_getets	= mlx4_en_dcbnl_ieee_getets,
 	.ieee_setets	= mlx4_en_dcbnl_ieee_setets,
@@ -252,6 +472,9 @@ const struct dcbnl_rtnl_ops mlx4_en_dcbnl_ops = {
 
 	.getdcbx	= mlx4_en_dcbnl_getdcbx,
 	.setdcbx	= mlx4_en_dcbnl_setdcbx,
+	.ieee_getqcn	= mlx4_en_dcbnl_ieee_getqcn,
+	.ieee_setqcn	= mlx4_en_dcbnl_ieee_setqcn,
+	.ieee_getqcnstats = mlx4_en_dcbnl_ieee_getqcnstats,
 };
 
 const struct dcbnl_rtnl_ops mlx4_en_dcbnl_pfc_ops = {
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
index a7b58ba8492b..3f44e2bbb982 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -38,6 +38,7 @@
 #include <linux/mlx4/device.h>
 #include <linux/in.h>
 #include <net/ip.h>
+#include <linux/bitmap.h>
 
 #include "mlx4_en.h"
 #include "en_port.h"
@@ -104,6 +105,7 @@ static const char mlx4_en_priv_flags[][ETH_GSTRING_LEN] = {
 };
 
 static const char main_strings[][ETH_GSTRING_LEN] = {
+	/* main statistics */
 	"rx_packets", "tx_packets", "rx_bytes", "tx_bytes", "rx_errors",
 	"tx_errors", "rx_dropped", "tx_dropped", "multicast", "collisions",
 	"rx_length_errors", "rx_over_errors", "rx_crc_errors",
@@ -117,14 +119,76 @@ static const char main_strings[][ETH_GSTRING_LEN] = {
 	"queue_stopped", "wake_queue", "tx_timeout", "rx_alloc_failed",
 	"rx_csum_good", "rx_csum_none", "rx_csum_complete", "tx_chksum_offload",
 
+	/* priority flow control statistics rx */
+	"rx_pause_prio_0", "rx_pause_duration_prio_0",
+	"rx_pause_transition_prio_0",
+	"rx_pause_prio_1", "rx_pause_duration_prio_1",
+	"rx_pause_transition_prio_1",
+	"rx_pause_prio_2", "rx_pause_duration_prio_2",
+	"rx_pause_transition_prio_2",
+	"rx_pause_prio_3", "rx_pause_duration_prio_3",
+	"rx_pause_transition_prio_3",
+	"rx_pause_prio_4", "rx_pause_duration_prio_4",
+	"rx_pause_transition_prio_4",
+	"rx_pause_prio_5", "rx_pause_duration_prio_5",
+	"rx_pause_transition_prio_5",
+	"rx_pause_prio_6", "rx_pause_duration_prio_6",
+	"rx_pause_transition_prio_6",
+	"rx_pause_prio_7", "rx_pause_duration_prio_7",
+	"rx_pause_transition_prio_7",
+
+	/* flow control statistics rx */
+	"rx_pause", "rx_pause_duration", "rx_pause_transition",
+
+	/* priority flow control statistics tx */
+	"tx_pause_prio_0", "tx_pause_duration_prio_0",
+	"tx_pause_transition_prio_0",
+	"tx_pause_prio_1", "tx_pause_duration_prio_1",
+	"tx_pause_transition_prio_1",
+	"tx_pause_prio_2", "tx_pause_duration_prio_2",
+	"tx_pause_transition_prio_2",
+	"tx_pause_prio_3", "tx_pause_duration_prio_3",
+	"tx_pause_transition_prio_3",
+	"tx_pause_prio_4", "tx_pause_duration_prio_4",
+	"tx_pause_transition_prio_4",
+	"tx_pause_prio_5", "tx_pause_duration_prio_5",
+	"tx_pause_transition_prio_5",
+	"tx_pause_prio_6", "tx_pause_duration_prio_6",
+	"tx_pause_transition_prio_6",
+	"tx_pause_prio_7", "tx_pause_duration_prio_7",
+	"tx_pause_transition_prio_7",
+
+	/* flow control statistics tx */
+	"tx_pause", "tx_pause_duration", "tx_pause_transition",
+
 	/* packet statistics */
-	"broadcast", "rx_prio_0", "rx_prio_1", "rx_prio_2", "rx_prio_3",
-	"rx_prio_4", "rx_prio_5", "rx_prio_6", "rx_prio_7", "tx_prio_0",
-	"tx_prio_1", "tx_prio_2", "tx_prio_3", "tx_prio_4", "tx_prio_5",
-	"tx_prio_6", "tx_prio_7",
+	"rx_multicast_packets",
+	"rx_broadcast_packets",
+	"rx_jabbers",
+	"rx_in_range_length_error",
+	"rx_out_range_length_error",
+	"tx_multicast_packets",
+	"tx_broadcast_packets",
+	"rx_prio_0_packets", "rx_prio_0_bytes",
+	"rx_prio_1_packets", "rx_prio_1_bytes",
+	"rx_prio_2_packets", "rx_prio_2_bytes",
+	"rx_prio_3_packets", "rx_prio_3_bytes",
+	"rx_prio_4_packets", "rx_prio_4_bytes",
+	"rx_prio_5_packets", "rx_prio_5_bytes",
+	"rx_prio_6_packets", "rx_prio_6_bytes",
+	"rx_prio_7_packets", "rx_prio_7_bytes",
+	"rx_novlan_packets", "rx_novlan_bytes",
+	"tx_prio_0_packets", "tx_prio_0_bytes",
+	"tx_prio_1_packets", "tx_prio_1_bytes",
+	"tx_prio_2_packets", "tx_prio_2_bytes",
+	"tx_prio_3_packets", "tx_prio_3_bytes",
+	"tx_prio_4_packets", "tx_prio_4_bytes",
+	"tx_prio_5_packets", "tx_prio_5_bytes",
+	"tx_prio_6_packets", "tx_prio_6_bytes",
+	"tx_prio_7_packets", "tx_prio_7_bytes",
+	"tx_novlan_packets", "tx_novlan_bytes",
+
 };
-#define NUM_MAIN_STATS	21
-#define NUM_ALL_STATS	(NUM_MAIN_STATS + NUM_PORT_STATS + NUM_PKT_STATS + NUM_PERF_STATS)
 
 static const char mlx4_en_test_names[][ETH_GSTRING_LEN]= {
 	"Interrupt Test",
@@ -224,14 +288,50 @@ static int mlx4_en_set_wol(struct net_device *netdev,
 	return err;
 }
 
+struct bitmap_iterator {
+	unsigned long *stats_bitmap;
+	unsigned int count;
+	unsigned int iterator;
+	bool advance_array; /* if set, force no increments */
+};
+
+static inline void bitmap_iterator_init(struct bitmap_iterator *h,
+					unsigned long *stats_bitmap,
+					int count)
+{
+	h->iterator = 0;
+	h->advance_array = !bitmap_empty(stats_bitmap, count);
+	h->count = h->advance_array ? bitmap_weight(stats_bitmap, count)
+		: count;
+	h->stats_bitmap = stats_bitmap;
+}
+
+static inline int bitmap_iterator_test(struct bitmap_iterator *h)
+{
+	return !h->advance_array ? 1 : test_bit(h->iterator, h->stats_bitmap);
+}
+
+static inline int bitmap_iterator_inc(struct bitmap_iterator *h)
+{
+	return h->iterator++;
+}
+
+static inline unsigned int
+bitmap_iterator_count(struct bitmap_iterator *h)
+{
+	return h->count;
+}
+
 static int mlx4_en_get_sset_count(struct net_device *dev, int sset)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
-	int bit_count = hweight64(priv->stats_bitmap);
+	struct bitmap_iterator it;
+
+	bitmap_iterator_init(&it, priv->stats_bitmap.bitmap, NUM_ALL_STATS);
 
 	switch (sset) {
 	case ETH_SS_STATS:
-		return (priv->stats_bitmap ? bit_count : NUM_ALL_STATS) +
+		return bitmap_iterator_count(&it) +
 			(priv->tx_ring_num * 2) +
 #ifdef CONFIG_NET_RX_BUSY_POLL
 			(priv->rx_ring_num * 5);
@@ -253,34 +353,45 @@ static void mlx4_en_get_ethtool_stats(struct net_device *dev,
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	int index = 0;
-	int i, j = 0;
+	int i;
+	struct bitmap_iterator it;
+
+	bitmap_iterator_init(&it, priv->stats_bitmap.bitmap, NUM_ALL_STATS);
 
 	spin_lock_bh(&priv->stats_lock);
 
-	if (!(priv->stats_bitmap)) {
-		for (i = 0; i < NUM_MAIN_STATS; i++)
-			data[index++] =
-				((unsigned long *) &priv->stats)[i];
-		for (i = 0; i < NUM_PORT_STATS; i++)
+	for (i = 0; i < NUM_MAIN_STATS; i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
+			data[index++] = ((unsigned long *)&priv->stats)[i];
+
+	for (i = 0; i < NUM_PORT_STATS; i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
+			data[index++] = ((unsigned long *)&priv->port_stats)[i];
+
+	for (i = 0; i < NUM_FLOW_PRIORITY_STATS_RX;
+	     i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
 			data[index++] =
-				((unsigned long *) &priv->port_stats)[i];
-		for (i = 0; i < NUM_PKT_STATS; i++)
+				((u64 *)&priv->rx_priority_flowstats)[i];
+
+	for (i = 0; i < NUM_FLOW_STATS_RX; i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
+			data[index++] = ((u64 *)&priv->rx_flowstats)[i];
+
+	for (i = 0; i < NUM_FLOW_PRIORITY_STATS_TX;
+	     i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
 			data[index++] =
-				((unsigned long *) &priv->pkstats)[i];
-	} else {
-		for (i = 0; i < NUM_MAIN_STATS; i++) {
-			if ((priv->stats_bitmap >> j) & 1)
-				data[index++] =
-				((unsigned long *) &priv->stats)[i];
-			j++;
-		}
-		for (i = 0; i < NUM_PORT_STATS; i++) {
-			if ((priv->stats_bitmap >> j) & 1)
-				data[index++] =
-				((unsigned long *) &priv->port_stats)[i];
-			j++;
-		}
-	}
+				((u64 *)&priv->tx_priority_flowstats)[i];
+
+	for (i = 0; i < NUM_FLOW_STATS_TX; i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
+			data[index++] = ((u64 *)&priv->tx_flowstats)[i];
+
+	for (i = 0; i < NUM_PKT_STATS; i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
+			data[index++] = ((unsigned long *)&priv->pkstats)[i];
+
 	for (i = 0; i < priv->tx_ring_num; i++) {
 		data[index++] = priv->tx_ring[i]->packets;
 		data[index++] = priv->tx_ring[i]->bytes;
@@ -309,7 +420,10 @@ static void mlx4_en_get_strings(struct net_device *dev,
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	int index = 0;
-	int i;
+	int i, strings = 0;
+	struct bitmap_iterator it;
+
+	bitmap_iterator_init(&it, priv->stats_bitmap.bitmap, NUM_ALL_STATS);
 
 	switch (stringset) {
 	case ETH_SS_TEST:
@@ -322,29 +436,30 @@ static void mlx4_en_get_strings(struct net_device *dev,
 
 	case ETH_SS_STATS:
 		/* Add main counters */
-		if (!priv->stats_bitmap) {
-			for (i = 0; i < NUM_MAIN_STATS; i++)
+		for (i = 0; i < NUM_MAIN_STATS; i++, strings++,
+		     bitmap_iterator_inc(&it))
+			if (bitmap_iterator_test(&it))
+				strcpy(data + (index++) * ETH_GSTRING_LEN,
+				       main_strings[strings]);
+
+		for (i = 0; i < NUM_PORT_STATS; i++, strings++,
+		     bitmap_iterator_inc(&it))
+			if (bitmap_iterator_test(&it))
 				strcpy(data + (index++) * ETH_GSTRING_LEN,
-					main_strings[i]);
-			for (i = 0; i < NUM_PORT_STATS; i++)
+				       main_strings[strings]);
+
+		for (i = 0; i < NUM_FLOW_STATS; i++, strings++,
+		     bitmap_iterator_inc(&it))
+			if (bitmap_iterator_test(&it))
 				strcpy(data + (index++) * ETH_GSTRING_LEN,
-					main_strings[i +
-					NUM_MAIN_STATS]);
-			for (i = 0; i < NUM_PKT_STATS; i++)
+				       main_strings[strings]);
+
+		for (i = 0; i < NUM_PKT_STATS; i++, strings++,
+		     bitmap_iterator_inc(&it))
+			if (bitmap_iterator_test(&it))
 				strcpy(data + (index++) * ETH_GSTRING_LEN,
-					main_strings[i +
-					NUM_MAIN_STATS +
-					NUM_PORT_STATS]);
-		} else
-			for (i = 0; i < NUM_MAIN_STATS + NUM_PORT_STATS; i++) {
-				if ((priv->stats_bitmap >> i) & 1) {
-					strcpy(data +
-					       (index++) * ETH_GSTRING_LEN,
-					       main_strings[i]);
-				}
-				if (!(priv->stats_bitmap >> i))
-					break;
-			}
+				       main_strings[strings]);
+
 		for (i = 0; i < priv->tx_ring_num; i++) {
 			sprintf(data + (index++) * ETH_GSTRING_LEN,
 				"tx%d_packets", i);
@@ -885,6 +1000,12 @@ static int mlx4_en_set_pauseparam(struct net_device *dev,
 				    priv->prof->rx_ppp);
 	if (err)
 		en_err(priv, "Failed setting pause params\n");
+	else
+		mlx4_en_update_pfc_stats_bitmap(mdev->dev, &priv->stats_bitmap,
+						priv->prof->rx_ppp,
+						priv->prof->rx_pause,
+						priv->prof->tx_ppp,
+						priv->prof->tx_pause);
 
 	return err;
 }
@@ -1818,6 +1939,32 @@ static int mlx4_en_get_module_eeprom(struct net_device *dev,
 	return 0;
 }
 
+static int mlx4_en_set_phys_id(struct net_device *dev,
+			       enum ethtool_phys_id_state state)
+{
+	int err;
+	u16 beacon_duration;
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+
+	if (!(mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_PORT_BEACON))
+		return -EOPNOTSUPP;
+
+	switch (state) {
+	case ETHTOOL_ID_ACTIVE:
+		beacon_duration = PORT_BEACON_MAX_LIMIT;
+		break;
+	case ETHTOOL_ID_INACTIVE:
+		beacon_duration = 0;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	err = mlx4_SET_PORT_BEACON(mdev->dev, priv->port, beacon_duration);
+	return err;
+}
+
 const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.get_drvinfo = mlx4_en_get_drvinfo,
 	.get_settings = mlx4_en_get_settings,
@@ -1827,6 +1974,7 @@ const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.get_sset_count = mlx4_en_get_sset_count,
 	.get_ethtool_stats = mlx4_en_get_ethtool_stats,
 	.self_test = mlx4_en_self_test,
+	.set_phys_id = mlx4_en_set_phys_id,
 	.get_wol = mlx4_en_get_wol,
 	.set_wol = mlx4_en_set_wol,
 	.get_msglevel = mlx4_en_get_msglevel,
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_main.c b/drivers/net/ethernet/mellanox/mlx4/en_main.c
index 58d5a07d0ff4..913b716ed2e1 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_main.c
@@ -103,6 +103,11 @@ void mlx4_en_update_loopback_state(struct net_device *dev,
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 
+	if (features & NETIF_F_LOOPBACK)
+		priv->ctrl_flags |= cpu_to_be32(MLX4_WQE_CTRL_FORCE_LOOPBACK);
+	else
+		priv->ctrl_flags &= cpu_to_be32(~MLX4_WQE_CTRL_FORCE_LOOPBACK);
+
 	priv->flags &= ~(MLX4_EN_FLAG_RX_FILTER_NEEDED|
 			MLX4_EN_FLAG_ENABLE_HW_LOOPBACK);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 2a210c4efb89..0f1afc085d58 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -1685,7 +1685,7 @@ int mlx4_en_start_port(struct net_device *dev)
 	}
 
 	/* Attach rx QP to bradcast address */
-	memset(&mc_list[10], 0xff, ETH_ALEN);
+	eth_broadcast_addr(&mc_list[10]);
 	mc_list[5] = priv->port; /* needed for B0 steering support */
 	if (mlx4_multicast_attach(mdev->dev, &priv->rss_map.indir_qp, mc_list,
 				  priv->port, 0, MLX4_PROT_ETH,
@@ -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);
@@ -1788,7 +1786,7 @@ void mlx4_en_stop_port(struct net_device *dev, int detach)
 	}
 
 	/* Detach All multicasts */
-	memset(&mc_list[10], 0xff, ETH_ALEN);
+	eth_broadcast_addr(&mc_list[10]);
 	mc_list[5] = priv->port; /* needed for B0 steering support */
 	mlx4_multicast_detach(mdev->dev, &priv->rss_map.indir_qp, mc_list,
 			      MLX4_PROT_ETH, priv->broadcast_id);
@@ -1890,6 +1888,12 @@ static void mlx4_en_clear_stats(struct net_device *dev)
 	memset(&priv->pstats, 0, sizeof(priv->pstats));
 	memset(&priv->pkstats, 0, sizeof(priv->pkstats));
 	memset(&priv->port_stats, 0, sizeof(priv->port_stats));
+	memset(&priv->rx_flowstats, 0, sizeof(priv->rx_flowstats));
+	memset(&priv->tx_flowstats, 0, sizeof(priv->tx_flowstats));
+	memset(&priv->rx_priority_flowstats, 0,
+	       sizeof(priv->rx_priority_flowstats));
+	memset(&priv->tx_priority_flowstats, 0,
+	       sizeof(priv->tx_priority_flowstats));
 
 	for (i = 0; i < priv->tx_ring_num; i++) {
 		priv->tx_ring[i]->bytes = 0;
@@ -2191,31 +2195,50 @@ static int mlx4_en_set_features(struct net_device *netdev,
 		netdev_features_t features)
 {
 	struct mlx4_en_priv *priv = netdev_priv(netdev);
+	bool reset = false;
 	int ret = 0;
 
+	if (DEV_FEATURE_CHANGED(netdev, features, NETIF_F_RXFCS)) {
+		en_info(priv, "Turn %s RX-FCS\n",
+			(features & NETIF_F_RXFCS) ? "ON" : "OFF");
+		reset = true;
+	}
+
+	if (DEV_FEATURE_CHANGED(netdev, features, NETIF_F_RXALL)) {
+		u8 ignore_fcs_value = (features & NETIF_F_RXALL) ? 1 : 0;
+
+		en_info(priv, "Turn %s RX-ALL\n",
+			ignore_fcs_value ? "ON" : "OFF");
+		ret = mlx4_SET_PORT_fcs_check(priv->mdev->dev,
+					      priv->port, ignore_fcs_value);
+		if (ret)
+			return ret;
+	}
+
 	if (DEV_FEATURE_CHANGED(netdev, features, NETIF_F_HW_VLAN_CTAG_RX)) {
 		en_info(priv, "Turn %s RX vlan strip offload\n",
 			(features & NETIF_F_HW_VLAN_CTAG_RX) ? "ON" : "OFF");
-		ret = mlx4_en_reset_config(netdev, priv->hwtstamp_config,
-					   features);
-		if (ret)
-			return ret;
+		reset = true;
 	}
 
 	if (DEV_FEATURE_CHANGED(netdev, features, NETIF_F_HW_VLAN_CTAG_TX))
 		en_info(priv, "Turn %s TX vlan strip offload\n",
 			(features & NETIF_F_HW_VLAN_CTAG_TX) ? "ON" : "OFF");
 
-	if (features & NETIF_F_LOOPBACK)
-		priv->ctrl_flags |= cpu_to_be32(MLX4_WQE_CTRL_FORCE_LOOPBACK);
-	else
-		priv->ctrl_flags &=
-			cpu_to_be32(~MLX4_WQE_CTRL_FORCE_LOOPBACK);
+	if (DEV_FEATURE_CHANGED(netdev, features, NETIF_F_LOOPBACK)) {
+		en_info(priv, "Turn %s loopback\n",
+			(features & NETIF_F_LOOPBACK) ? "ON" : "OFF");
+		mlx4_en_update_loopback_state(netdev, features);
+	}
 
-	mlx4_en_update_loopback_state(netdev, features);
+	if (reset) {
+		ret = mlx4_en_reset_config(netdev, priv->hwtstamp_config,
+					   features);
+		if (ret)
+			return ret;
+	}
 
 	return 0;
-
 }
 
 static int mlx4_en_set_vf_mac(struct net_device *dev, int queue, u8 *mac)
@@ -2238,6 +2261,16 @@ static int mlx4_en_set_vf_vlan(struct net_device *dev, int vf, u16 vlan, u8 qos)
 	return mlx4_set_vf_vlan(mdev->dev, en_priv->port, vf, vlan, qos);
 }
 
+static int mlx4_en_set_vf_rate(struct net_device *dev, int vf, int min_tx_rate,
+			       int max_tx_rate)
+{
+	struct mlx4_en_priv *en_priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = en_priv->mdev;
+
+	return mlx4_set_vf_rate(mdev->dev, en_priv->port, vf, min_tx_rate,
+				max_tx_rate);
+}
+
 static int mlx4_en_set_vf_spoofchk(struct net_device *dev, int vf, bool setting)
 {
 	struct mlx4_en_priv *en_priv = netdev_priv(dev);
@@ -2375,10 +2408,38 @@ static netdev_features_t mlx4_en_features_check(struct sk_buff *skb,
 						struct net_device *dev,
 						netdev_features_t features)
 {
+	features = vlan_features_check(skb, features);
 	return vxlan_features_check(skb, features);
 }
 #endif
 
+static int mlx4_en_set_tx_maxrate(struct net_device *dev, int queue_index, u32 maxrate)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_tx_ring *tx_ring = priv->tx_ring[queue_index];
+	struct mlx4_update_qp_params params;
+	int err;
+
+	if (!(priv->mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_QP_RATE_LIMIT))
+		return -EOPNOTSUPP;
+
+	/* rate provided to us in Mbs, check if it fits into 12 bits, if not use Gbs */
+	if (maxrate >> 12) {
+		params.rate_unit = MLX4_QP_RATE_LIMIT_GBS;
+		params.rate_val  = maxrate / 1000;
+	} else if (maxrate) {
+		params.rate_unit = MLX4_QP_RATE_LIMIT_MBS;
+		params.rate_val  = maxrate;
+	} else { /* zero serves to revoke the QP rate-limitation */
+		params.rate_unit = 0;
+		params.rate_val  = 0;
+	}
+
+	err = mlx4_update_qp(priv->mdev->dev, tx_ring->qpn, MLX4_UPDATE_QP_RATE_LIMIT,
+			     &params);
+	return err;
+}
+
 static const struct net_device_ops mlx4_netdev_ops = {
 	.ndo_open		= mlx4_en_open,
 	.ndo_stop		= mlx4_en_close,
@@ -2410,6 +2471,7 @@ static const struct net_device_ops mlx4_netdev_ops = {
 	.ndo_del_vxlan_port	= mlx4_en_del_vxlan_port,
 	.ndo_features_check	= mlx4_en_features_check,
 #endif
+	.ndo_set_tx_maxrate	= mlx4_en_set_tx_maxrate,
 };
 
 static const struct net_device_ops mlx4_netdev_ops_master = {
@@ -2427,6 +2489,7 @@ static const struct net_device_ops mlx4_netdev_ops_master = {
 	.ndo_vlan_rx_kill_vid	= mlx4_en_vlan_rx_kill_vid,
 	.ndo_set_vf_mac		= mlx4_en_set_vf_mac,
 	.ndo_set_vf_vlan	= mlx4_en_set_vf_vlan,
+	.ndo_set_vf_rate	= mlx4_en_set_vf_rate,
 	.ndo_set_vf_spoofchk	= mlx4_en_set_vf_spoofchk,
 	.ndo_set_vf_link_state	= mlx4_en_set_vf_link_state,
 	.ndo_get_vf_config	= mlx4_en_get_vf_config,
@@ -2444,6 +2507,7 @@ static const struct net_device_ops mlx4_netdev_ops_master = {
 	.ndo_del_vxlan_port	= mlx4_en_del_vxlan_port,
 	.ndo_features_check	= mlx4_en_features_check,
 #endif
+	.ndo_set_tx_maxrate	= mlx4_en_set_tx_maxrate,
 };
 
 struct mlx4_en_bond {
@@ -2620,6 +2684,82 @@ int mlx4_en_netdev_event(struct notifier_block *this,
 	return NOTIFY_DONE;
 }
 
+void mlx4_en_update_pfc_stats_bitmap(struct mlx4_dev *dev,
+				     struct mlx4_en_stats_bitmap *stats_bitmap,
+				     u8 rx_ppp, u8 rx_pause,
+				     u8 tx_ppp, u8 tx_pause)
+{
+	int last_i = NUM_MAIN_STATS + NUM_PORT_STATS;
+
+	if (!mlx4_is_slave(dev) &&
+	    (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_FLOWSTATS_EN)) {
+		mutex_lock(&stats_bitmap->mutex);
+		bitmap_clear(stats_bitmap->bitmap, last_i, NUM_FLOW_STATS);
+
+		if (rx_ppp)
+			bitmap_set(stats_bitmap->bitmap, last_i,
+				   NUM_FLOW_PRIORITY_STATS_RX);
+		last_i += NUM_FLOW_PRIORITY_STATS_RX;
+
+		if (rx_pause && !(rx_ppp))
+			bitmap_set(stats_bitmap->bitmap, last_i,
+				   NUM_FLOW_STATS_RX);
+		last_i += NUM_FLOW_STATS_RX;
+
+		if (tx_ppp)
+			bitmap_set(stats_bitmap->bitmap, last_i,
+				   NUM_FLOW_PRIORITY_STATS_TX);
+		last_i += NUM_FLOW_PRIORITY_STATS_TX;
+
+		if (tx_pause && !(tx_ppp))
+			bitmap_set(stats_bitmap->bitmap, last_i,
+				   NUM_FLOW_STATS_TX);
+		last_i += NUM_FLOW_STATS_TX;
+
+		mutex_unlock(&stats_bitmap->mutex);
+	}
+}
+
+void mlx4_en_set_stats_bitmap(struct mlx4_dev *dev,
+			      struct mlx4_en_stats_bitmap *stats_bitmap,
+			      u8 rx_ppp, u8 rx_pause,
+			      u8 tx_ppp, u8 tx_pause)
+{
+	int last_i = 0;
+
+	mutex_init(&stats_bitmap->mutex);
+	bitmap_zero(stats_bitmap->bitmap, NUM_ALL_STATS);
+
+	if (mlx4_is_slave(dev)) {
+		bitmap_set(stats_bitmap->bitmap, last_i +
+					 MLX4_FIND_NETDEV_STAT(rx_packets), 1);
+		bitmap_set(stats_bitmap->bitmap, last_i +
+					 MLX4_FIND_NETDEV_STAT(tx_packets), 1);
+		bitmap_set(stats_bitmap->bitmap, last_i +
+					 MLX4_FIND_NETDEV_STAT(rx_bytes), 1);
+		bitmap_set(stats_bitmap->bitmap, last_i +
+					 MLX4_FIND_NETDEV_STAT(tx_bytes), 1);
+		bitmap_set(stats_bitmap->bitmap, last_i +
+					 MLX4_FIND_NETDEV_STAT(rx_dropped), 1);
+		bitmap_set(stats_bitmap->bitmap, last_i +
+					 MLX4_FIND_NETDEV_STAT(tx_dropped), 1);
+	} else {
+		bitmap_set(stats_bitmap->bitmap, last_i, NUM_MAIN_STATS);
+	}
+	last_i += NUM_MAIN_STATS;
+
+	bitmap_set(stats_bitmap->bitmap, last_i, NUM_PORT_STATS);
+	last_i += NUM_PORT_STATS;
+
+	mlx4_en_update_pfc_stats_bitmap(dev, stats_bitmap,
+					rx_ppp, rx_pause,
+					tx_ppp, tx_pause);
+	last_i += NUM_FLOW_STATS;
+
+	if (!mlx4_is_slave(dev))
+		bitmap_set(stats_bitmap->bitmap, last_i, NUM_PKT_STATS);
+}
+
 int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 			struct mlx4_en_port_profile *prof)
 {
@@ -2695,7 +2835,7 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	priv->msg_enable = MLX4_EN_MSG_LEVEL;
 #ifdef CONFIG_MLX4_EN_DCB
 	if (!mlx4_is_slave(priv->mdev->dev)) {
-		if (mdev->dev->caps.flags & MLX4_DEV_CAP_FLAG_SET_ETH_SCHED) {
+		if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ETS_CFG) {
 			dev->dcbnl_ops = &mlx4_en_dcbnl_ops;
 		} else {
 			en_info(priv, "enabling only PFC DCB ops\n");
@@ -2782,6 +2922,12 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	dev->hw_features |= NETIF_F_LOOPBACK |
 			NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 
+	if (mdev->dev->caps.flags & MLX4_DEV_CAP_FLAG_FCS_KEEP)
+		dev->hw_features |= NETIF_F_RXFCS;
+
+	if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_IGNORE_FCS)
+		dev->hw_features |= NETIF_F_RXALL;
+
 	if (mdev->dev->caps.steering_mode ==
 	    MLX4_STEERING_MODE_DEVICE_MANAGED &&
 	    mdev->dev->caps.dmfs_high_steer_mode != MLX4_STEERING_DMFS_A0_STATIC)
@@ -2807,13 +2953,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 +2992,20 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 		queue_delayed_work(mdev->workqueue, &priv->service_task,
 				   SERVICE_TASK_DELAY);
 
+	mlx4_en_set_stats_bitmap(mdev->dev, &priv->stats_bitmap,
+				 mdev->profile.prof[priv->port].rx_ppp,
+				 mdev->profile.prof[priv->port].rx_pause,
+				 mdev->profile.prof[priv->port].tx_ppp,
+				 mdev->profile.prof[priv->port].tx_pause);
+
+	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:
@@ -2871,7 +3024,8 @@ int mlx4_en_reset_config(struct net_device *dev,
 
 	if (priv->hwtstamp_config.tx_type == ts_config.tx_type &&
 	    priv->hwtstamp_config.rx_filter == ts_config.rx_filter &&
-	    !DEV_FEATURE_CHANGED(dev, features, NETIF_F_HW_VLAN_CTAG_RX))
+	    !DEV_FEATURE_CHANGED(dev, features, NETIF_F_HW_VLAN_CTAG_RX) &&
+	    !DEV_FEATURE_CHANGED(dev, features, NETIF_F_RXFCS))
 		return 0; /* Nothing to change */
 
 	if (DEV_FEATURE_CHANGED(dev, features, NETIF_F_HW_VLAN_CTAG_RX) &&
@@ -2910,6 +3064,13 @@ int mlx4_en_reset_config(struct net_device *dev,
 			dev->features &= ~NETIF_F_HW_VLAN_CTAG_RX;
 	}
 
+	if (DEV_FEATURE_CHANGED(dev, features, NETIF_F_RXFCS)) {
+		if (features & NETIF_F_RXFCS)
+			dev->features |= NETIF_F_RXFCS;
+		else
+			dev->features &= ~NETIF_F_RXFCS;
+	}
+
 	/* RX vlan offload and RX time-stamping can't co-exist !
 	 * Regardless of the caller's choice,
 	 * Turn Off RX vlan offload in case of time-stamping is ON
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_port.c b/drivers/net/ethernet/mellanox/mlx4/en_port.c
index 6cb80072af6c..54f0e5ab2e55 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_port.c
@@ -128,9 +128,29 @@ out:
 	return err;
 }
 
+/* Each counter set is located in struct mlx4_en_stat_out_mbox
+ * with a const offset between its prio components.
+ * This function runs over a counter set and sum all of it's prio components.
+ */
+static unsigned long en_stats_adder(__be64 *start, __be64 *next, int num)
+{
+	__be64 *curr = start;
+	unsigned long ret = 0;
+	int i;
+	int offset = next - start;
+
+	for (i = 0; i <= num; i++) {
+		ret += be64_to_cpu(*curr);
+		curr += offset;
+	}
+
+	return ret;
+}
+
 int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 {
 	struct mlx4_en_stat_out_mbox *mlx4_en_stats;
+	struct mlx4_en_stat_out_flow_control_mbox *flowstats;
 	struct mlx4_en_priv *priv = netdev_priv(mdev->pndev[port]);
 	struct net_device_stats *stats = &priv->stats;
 	struct mlx4_cmd_mailbox *mailbox;
@@ -183,22 +203,25 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 		priv->port_stats.xmit_more         += ring->xmit_more;
 	}
 
+	/* net device stats */
 	stats->rx_errors = be64_to_cpu(mlx4_en_stats->PCS) +
-			   be32_to_cpu(mlx4_en_stats->RdropLength) +
 			   be32_to_cpu(mlx4_en_stats->RJBBR) +
 			   be32_to_cpu(mlx4_en_stats->RCRC) +
-			   be32_to_cpu(mlx4_en_stats->RRUNT);
-	stats->tx_errors = be32_to_cpu(mlx4_en_stats->TDROP);
-	stats->multicast = be64_to_cpu(mlx4_en_stats->MCAST_prio_0) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_prio_1) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_prio_2) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_prio_3) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_prio_4) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_prio_5) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_prio_6) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_prio_7) +
-			   be64_to_cpu(mlx4_en_stats->MCAST_novlan);
+			   be32_to_cpu(mlx4_en_stats->RRUNT) +
+			   be64_to_cpu(mlx4_en_stats->RInRangeLengthErr) +
+			   be64_to_cpu(mlx4_en_stats->ROutRangeLengthErr) +
+			   be32_to_cpu(mlx4_en_stats->RSHORT) +
+			   en_stats_adder(&mlx4_en_stats->RGIANT_prio_0,
+					  &mlx4_en_stats->RGIANT_prio_1,
+					  NUM_PRIORITIES);
+	stats->tx_errors = en_stats_adder(&mlx4_en_stats->TGIANT_prio_0,
+					  &mlx4_en_stats->TGIANT_prio_1,
+					  NUM_PRIORITIES);
+	stats->multicast = en_stats_adder(&mlx4_en_stats->MCAST_prio_0,
+					  &mlx4_en_stats->MCAST_prio_1,
+					  NUM_PRIORITIES);
 	stats->collisions = 0;
+	stats->rx_dropped = be32_to_cpu(mlx4_en_stats->RDROP);
 	stats->rx_length_errors = be32_to_cpu(mlx4_en_stats->RdropLength);
 	stats->rx_over_errors = be32_to_cpu(mlx4_en_stats->RdropOvflw);
 	stats->rx_crc_errors = be32_to_cpu(mlx4_en_stats->RCRC);
@@ -210,33 +233,116 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 	stats->tx_fifo_errors = 0;
 	stats->tx_heartbeat_errors = 0;
 	stats->tx_window_errors = 0;
+	stats->tx_dropped = be32_to_cpu(mlx4_en_stats->TDROP);
+
+	/* RX stats */
+	priv->pkstats.rx_multicast_packets = stats->multicast;
+	priv->pkstats.rx_broadcast_packets =
+			en_stats_adder(&mlx4_en_stats->RBCAST_prio_0,
+				       &mlx4_en_stats->RBCAST_prio_1,
+				       NUM_PRIORITIES);
+	priv->pkstats.rx_jabbers = be32_to_cpu(mlx4_en_stats->RJBBR);
+	priv->pkstats.rx_in_range_length_error =
+		be64_to_cpu(mlx4_en_stats->RInRangeLengthErr);
+	priv->pkstats.rx_out_range_length_error =
+		be64_to_cpu(mlx4_en_stats->ROutRangeLengthErr);
+
+	/* Tx stats */
+	priv->pkstats.tx_multicast_packets =
+		en_stats_adder(&mlx4_en_stats->TMCAST_prio_0,
+			       &mlx4_en_stats->TMCAST_prio_1,
+			       NUM_PRIORITIES);
+	priv->pkstats.tx_broadcast_packets =
+		en_stats_adder(&mlx4_en_stats->TBCAST_prio_0,
+			       &mlx4_en_stats->TBCAST_prio_1,
+			       NUM_PRIORITIES);
+
+	priv->pkstats.rx_prio[0][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_0);
+	priv->pkstats.rx_prio[0][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_0);
+	priv->pkstats.rx_prio[1][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_1);
+	priv->pkstats.rx_prio[1][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_1);
+	priv->pkstats.rx_prio[2][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_2);
+	priv->pkstats.rx_prio[2][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_2);
+	priv->pkstats.rx_prio[3][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_3);
+	priv->pkstats.rx_prio[3][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_3);
+	priv->pkstats.rx_prio[4][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_4);
+	priv->pkstats.rx_prio[4][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_4);
+	priv->pkstats.rx_prio[5][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_5);
+	priv->pkstats.rx_prio[5][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_5);
+	priv->pkstats.rx_prio[6][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_6);
+	priv->pkstats.rx_prio[6][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_6);
+	priv->pkstats.rx_prio[7][0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_7);
+	priv->pkstats.rx_prio[7][1] = be64_to_cpu(mlx4_en_stats->ROCT_prio_7);
+	priv->pkstats.rx_prio[8][0] = be64_to_cpu(mlx4_en_stats->RTOT_novlan);
+	priv->pkstats.rx_prio[8][1] = be64_to_cpu(mlx4_en_stats->ROCT_novlan);
+	priv->pkstats.tx_prio[0][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_0);
+	priv->pkstats.tx_prio[0][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_0);
+	priv->pkstats.tx_prio[1][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_1);
+	priv->pkstats.tx_prio[1][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_1);
+	priv->pkstats.tx_prio[2][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_2);
+	priv->pkstats.tx_prio[2][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_2);
+	priv->pkstats.tx_prio[3][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_3);
+	priv->pkstats.tx_prio[3][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_3);
+	priv->pkstats.tx_prio[4][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_4);
+	priv->pkstats.tx_prio[4][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_4);
+	priv->pkstats.tx_prio[5][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_5);
+	priv->pkstats.tx_prio[5][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_5);
+	priv->pkstats.tx_prio[6][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_6);
+	priv->pkstats.tx_prio[6][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_6);
+	priv->pkstats.tx_prio[7][0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_7);
+	priv->pkstats.tx_prio[7][1] = be64_to_cpu(mlx4_en_stats->TOCT_prio_7);
+	priv->pkstats.tx_prio[8][0] = be64_to_cpu(mlx4_en_stats->TTOT_novlan);
+	priv->pkstats.tx_prio[8][1] = be64_to_cpu(mlx4_en_stats->TOCT_novlan);
+
+	spin_unlock_bh(&priv->stats_lock);
+
+	/* 0xffs indicates invalid value */
+	memset(mailbox->buf, 0xff, sizeof(*flowstats) * MLX4_NUM_PRIORITIES);
+
+	if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_FLOWSTATS_EN) {
+		memset(mailbox->buf, 0,
+		       sizeof(*flowstats) * MLX4_NUM_PRIORITIES);
+		err = mlx4_cmd_box(mdev->dev, 0, mailbox->dma,
+				   in_mod | MLX4_DUMP_ETH_STATS_FLOW_CONTROL,
+				   0, MLX4_CMD_DUMP_ETH_STATS,
+				   MLX4_CMD_TIME_CLASS_B, MLX4_CMD_WRAPPED);
+		if (err)
+			goto out;
+	}
+
+	flowstats = mailbox->buf;
+
+	spin_lock_bh(&priv->stats_lock);
+
+	for (i = 0; i < MLX4_NUM_PRIORITIES; i++)	{
+		priv->rx_priority_flowstats[i].rx_pause =
+			be64_to_cpu(flowstats[i].rx_pause);
+		priv->rx_priority_flowstats[i].rx_pause_duration =
+			be64_to_cpu(flowstats[i].rx_pause_duration);
+		priv->rx_priority_flowstats[i].rx_pause_transition =
+			be64_to_cpu(flowstats[i].rx_pause_transition);
+		priv->tx_priority_flowstats[i].tx_pause =
+			be64_to_cpu(flowstats[i].tx_pause);
+		priv->tx_priority_flowstats[i].tx_pause_duration =
+			be64_to_cpu(flowstats[i].tx_pause_duration);
+		priv->tx_priority_flowstats[i].tx_pause_transition =
+			be64_to_cpu(flowstats[i].tx_pause_transition);
+	}
+
+	/* if pfc is not in use, all priorities counters have the same value */
+	priv->rx_flowstats.rx_pause =
+		be64_to_cpu(flowstats[0].rx_pause);
+	priv->rx_flowstats.rx_pause_duration =
+		be64_to_cpu(flowstats[0].rx_pause_duration);
+	priv->rx_flowstats.rx_pause_transition =
+		be64_to_cpu(flowstats[0].rx_pause_transition);
+	priv->tx_flowstats.tx_pause =
+		be64_to_cpu(flowstats[0].tx_pause);
+	priv->tx_flowstats.tx_pause_duration =
+		be64_to_cpu(flowstats[0].tx_pause_duration);
+	priv->tx_flowstats.tx_pause_transition =
+		be64_to_cpu(flowstats[0].tx_pause_transition);
 
-	priv->pkstats.broadcast =
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_0) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_1) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_2) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_3) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_4) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_5) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_6) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_prio_7) +
-				be64_to_cpu(mlx4_en_stats->RBCAST_novlan);
-	priv->pkstats.rx_prio[0] = be64_to_cpu(mlx4_en_stats->RTOT_prio_0);
-	priv->pkstats.rx_prio[1] = be64_to_cpu(mlx4_en_stats->RTOT_prio_1);
-	priv->pkstats.rx_prio[2] = be64_to_cpu(mlx4_en_stats->RTOT_prio_2);
-	priv->pkstats.rx_prio[3] = be64_to_cpu(mlx4_en_stats->RTOT_prio_3);
-	priv->pkstats.rx_prio[4] = be64_to_cpu(mlx4_en_stats->RTOT_prio_4);
-	priv->pkstats.rx_prio[5] = be64_to_cpu(mlx4_en_stats->RTOT_prio_5);
-	priv->pkstats.rx_prio[6] = be64_to_cpu(mlx4_en_stats->RTOT_prio_6);
-	priv->pkstats.rx_prio[7] = be64_to_cpu(mlx4_en_stats->RTOT_prio_7);
-	priv->pkstats.tx_prio[0] = be64_to_cpu(mlx4_en_stats->TTOT_prio_0);
-	priv->pkstats.tx_prio[1] = be64_to_cpu(mlx4_en_stats->TTOT_prio_1);
-	priv->pkstats.tx_prio[2] = be64_to_cpu(mlx4_en_stats->TTOT_prio_2);
-	priv->pkstats.tx_prio[3] = be64_to_cpu(mlx4_en_stats->TTOT_prio_3);
-	priv->pkstats.tx_prio[4] = be64_to_cpu(mlx4_en_stats->TTOT_prio_4);
-	priv->pkstats.tx_prio[5] = be64_to_cpu(mlx4_en_stats->TTOT_prio_5);
-	priv->pkstats.tx_prio[6] = be64_to_cpu(mlx4_en_stats->TTOT_prio_6);
-	priv->pkstats.tx_prio[7] = be64_to_cpu(mlx4_en_stats->TTOT_prio_7);
 	spin_unlock_bh(&priv->stats_lock);
 
 out:
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index 698d60de1255..4fdd3c37e47b 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -771,7 +771,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 		/*
 		 * make sure we read the CQE after we read the ownership bit
 		 */
-		rmb();
+		dma_rmb();
 
 		/* Drop packet on bad receive or bad checksum */
 		if (unlikely((cqe->owner_sr_opcode & MLX4_CQE_OPCODE_MASK) ==
@@ -1116,7 +1116,10 @@ static int mlx4_en_config_rss_qp(struct mlx4_en_priv *priv, int qpn,
 	/* Cancel FCS removal if FW allows */
 	if (mdev->dev->caps.flags & MLX4_DEV_CAP_FLAG_FCS_KEEP) {
 		context->param3 |= cpu_to_be32(1 << 29);
-		ring->fcs_del = ETH_FCS_LEN;
+		if (priv->dev->features & NETIF_F_RXFCS)
+			ring->fcs_del = 0;
+		else
+			ring->fcs_del = ETH_FCS_LEN;
 	} else
 		ring->fcs_del = 0;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
index a61009f4b2df..b66e03d9711f 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
@@ -66,7 +66,7 @@ static int mlx4_en_test_loopback_xmit(struct mlx4_en_priv *priv)
 	ethh = (struct ethhdr *)skb_put(skb, sizeof(struct ethhdr));
 	packet	= (unsigned char *)skb_put(skb, packet_size);
 	memcpy(ethh->h_dest, priv->dev->dev_addr, ETH_ALEN);
-	memset(ethh->h_source, 0, ETH_ALEN);
+	eth_zero_addr(ethh->h_source);
 	ethh->h_proto = htons(ETH_P_ARP);
 	skb_set_mac_header(skb, 0);
 	for (i = 0; i < packet_size; ++i)	/* fill our packet */
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index 55f9f5c5344e..1783705273d8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -416,7 +416,7 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 		 * make sure we read the CQE after we read the
 		 * ownership bit
 		 */
-		rmb();
+		dma_rmb();
 
 		if (unlikely((cqe->owner_sr_opcode & MLX4_CQE_OPCODE_MASK) ==
 			     MLX4_CQE_OPCODE_ERROR)) {
@@ -667,7 +667,7 @@ static void build_inline_wqe(struct mlx4_en_tx_desc *tx_desc,
 				       skb_frag_size(&shinfo->frags[0]));
 		}
 
-		wmb();
+		dma_wmb();
 		inl->byte_count = cpu_to_be32(1 << 31 | (skb->len - spc));
 	}
 }
@@ -804,7 +804,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 
 			data->addr = cpu_to_be64(dma);
 			data->lkey = ring->mr_key;
-			wmb();
+			dma_wmb();
 			data->byte_count = cpu_to_be32(byte_count);
 			--data;
 		}
@@ -821,7 +821,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 
 			data->addr = cpu_to_be64(dma);
 			data->lkey = ring->mr_key;
-			wmb();
+			dma_wmb();
 			data->byte_count = cpu_to_be32(byte_count);
 		}
 		/* tx completion can avoid cache line miss for common cases */
@@ -938,7 +938,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 		/* Ensure new descriptor hits memory
 		 * before setting ownership of this descriptor to HW
 		 */
-		wmb();
+		dma_wmb();
 		tx_desc->ctrl.owner_opcode = op_own;
 
 		wmb();
@@ -958,7 +958,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 		/* Ensure new descriptor hits memory
 		 * before setting ownership of this descriptor to HW
 		 */
-		wmb();
+		dma_wmb();
 		tx_desc->ctrl.owner_opcode = op_own;
 		if (send_doorbell) {
 			wmb();
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 264bc15c1ff2..190fd624bdfe 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))
@@ -190,7 +188,7 @@ static void slave_event(struct mlx4_dev *dev, u8 slave, struct mlx4_eqe *eqe)
 	memcpy(s_eqe, eqe, dev->caps.eqe_size - 1);
 	s_eqe->slave_id = slave;
 	/* ensure all information is written before setting the ownersip bit */
-	wmb();
+	dma_wmb();
 	s_eqe->owner = !!(slave_eq->prod & SLAVE_EVENT_EQ_SIZE) ? 0x0 : 0x80;
 	++slave_eq->prod;
 
@@ -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);
 }
@@ -477,7 +473,7 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 		 * Make sure we read EQ entry contents after we've
 		 * checked the ownership bit.
 		 */
-		rmb();
+		dma_rmb();
 
 		switch (eqe->type) {
 		case MLX4_EVENT_TYPE_COMP:
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index 5a21e5dc94cb..b9881fc1252f 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -49,9 +49,9 @@ enum {
 extern void __buggy_use_of_MLX4_GET(void);
 extern void __buggy_use_of_MLX4_PUT(void);
 
-static bool enable_qos;
+static bool enable_qos = true;
 module_param(enable_qos, bool, 0444);
-MODULE_PARM_DESC(enable_qos, "Enable Quality of Service support in the HCA (default: off)");
+MODULE_PARM_DESC(enable_qos, "Enable Enhanced QoS support (default: on)");
 
 #define MLX4_GET(dest, source, offset)				      \
 	do {							      \
@@ -105,6 +105,7 @@ static void dump_dev_cap_flags(struct mlx4_dev *dev, u64 flags)
 		[41] = "Unicast VEP steering support",
 		[42] = "Multicast VEP steering support",
 		[48] = "Counters support",
+		[52] = "RSS IP fragments support",
 		[53] = "Port ETS Scheduler support",
 		[55] = "Port link type sensing support",
 		[59] = "Port management change event support",
@@ -143,7 +144,14 @@ static void dump_dev_cap_flags2(struct mlx4_dev *dev, u64 flags)
 		[18] = "More than 80 VFs support",
 		[19] = "Performance optimized for limited rule configuration flow steering support",
 		[20] = "Recoverable error events support",
-		[21] = "Port Remap support"
+		[21] = "Port Remap support",
+		[22] = "QCN support",
+		[23] = "QP rate limiting support",
+		[24] = "Ethernet Flow control statistics support",
+		[25] = "Granular QoS per VF support",
+		[26] = "Port ETS Scheduler support",
+		[27] = "Port beacon support",
+		[28] = "RX-ALL support",
 	};
 	int i;
 
@@ -641,6 +649,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_RSS_OFFSET		0x2e
 #define QUERY_DEV_CAP_MAX_RDMA_OFFSET		0x2f
 #define QUERY_DEV_CAP_RSZ_SRQ_OFFSET		0x33
+#define QUERY_DEV_CAP_PORT_BEACON_OFFSET	0x34
 #define QUERY_DEV_CAP_ACK_DELAY_OFFSET		0x35
 #define QUERY_DEV_CAP_MTU_WIDTH_OFFSET		0x36
 #define QUERY_DEV_CAP_VL_PORT_OFFSET		0x37
@@ -670,12 +679,13 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_RSVD_XRC_OFFSET		0x66
 #define QUERY_DEV_CAP_MAX_XRC_OFFSET		0x67
 #define QUERY_DEV_CAP_MAX_COUNTERS_OFFSET	0x68
+#define QUERY_DEV_CAP_PORT_FLOWSTATS_COUNTERS_OFFSET	0x70
 #define QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET	0x70
 #define QUERY_DEV_CAP_FLOW_STEERING_IPOIB_OFFSET	0x74
 #define QUERY_DEV_CAP_FLOW_STEERING_RANGE_EN_OFFSET	0x76
 #define QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET	0x77
 #define QUERY_DEV_CAP_CQ_EQ_CACHE_LINE_STRIDE	0x7a
-#define QUERY_DEV_CAP_ETH_PROT_CTRL_OFFSET	0x7a
+#define QUERY_DEV_CAP_ECN_QCN_VER_OFFSET	0x7b
 #define QUERY_DEV_CAP_RDMARC_ENTRY_SZ_OFFSET	0x80
 #define QUERY_DEV_CAP_QPC_ENTRY_SZ_OFFSET	0x82
 #define QUERY_DEV_CAP_AUX_ENTRY_SZ_OFFSET	0x84
@@ -696,6 +706,10 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_MAD_DEMUX_OFFSET		0xb0
 #define QUERY_DEV_CAP_DMFS_HIGH_RATE_QPN_BASE_OFFSET	0xa8
 #define QUERY_DEV_CAP_DMFS_HIGH_RATE_QPN_RANGE_OFFSET	0xac
+#define QUERY_DEV_CAP_QP_RATE_LIMIT_NUM_OFFSET	0xcc
+#define QUERY_DEV_CAP_QP_RATE_LIMIT_MAX_OFFSET	0xd0
+#define QUERY_DEV_CAP_QP_RATE_LIMIT_MIN_OFFSET	0xd2
+
 
 	dev_cap->flags2 = 0;
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
@@ -767,16 +781,25 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_VL_PORT_OFFSET);
 	dev_cap->num_ports = field & 0xf;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_MSG_SZ_OFFSET);
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_PORT_FLOWSTATS_COUNTERS_OFFSET);
+	if (field & 0x10)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_FLOWSTATS_EN;
 	dev_cap->max_msg_sz = 1 << (field & 0x1f);
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_FLOW_STEERING_RANGE_EN_OFFSET);
 	if (field & 0x80)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_FS_EN;
 	dev_cap->fs_log_max_ucast_qp_range_size = field & 0x1f;
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_PORT_BEACON_OFFSET);
+	if (field & 0x80)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_PORT_BEACON;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_FLOW_STEERING_IPOIB_OFFSET);
 	if (field & 0x80)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_DMFS_IPOIB;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET);
 	dev_cap->fs_max_num_qp_per_entry = field;
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_ECN_QCN_VER_OFFSET);
+	if (field & 0x1)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_QCN;
 	MLX4_GET(stat_rate, outbox, QUERY_DEV_CAP_RATE_SUPPORT_OFFSET);
 	dev_cap->stat_rate_support = stat_rate;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_CQ_TS_SUPPORT_OFFSET);
@@ -856,6 +879,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	MLX4_GET(size, outbox, QUERY_DEV_CAP_MAX_DESC_SZ_RQ_OFFSET);
 	dev_cap->max_rq_desc_sz = size;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_CQ_EQ_CACHE_LINE_STRIDE);
+	if (field & (1 << 4))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_QOS_VPP;
 	if (field & (1 << 5))
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_ETH_PROT_CTRL;
 	if (field & (1 << 6))
@@ -869,6 +894,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_CONFIG_DEV_OFFSET);
 	if (field & 0x20)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_CONFIG_DEV;
+	if (field & (1 << 2))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_IGNORE_FCS;
 	MLX4_GET(dev_cap->reserved_lkey, outbox,
 		 QUERY_DEV_CAP_RSVD_LKEY_OFFSET);
 	MLX4_GET(field32, outbox, QUERY_DEV_CAP_ETH_BACKPL_OFFSET);
@@ -882,6 +909,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_VXLAN);
 	if (field & 1<<3)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_VXLAN_OFFLOADS;
+	if (field & (1 << 5))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_ETS_CFG;
 	MLX4_GET(dev_cap->max_icm_sz, outbox,
 		 QUERY_DEV_CAP_MAX_ICM_SZ_OFFSET);
 	if (dev_cap->flags & MLX4_DEV_CAP_FLAG_COUNTERS)
@@ -900,6 +929,18 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		 QUERY_DEV_CAP_DMFS_HIGH_RATE_QPN_RANGE_OFFSET);
 	dev_cap->dmfs_high_rate_qpn_range &= MGM_QPN_MASK;
 
+	MLX4_GET(size, outbox, QUERY_DEV_CAP_QP_RATE_LIMIT_NUM_OFFSET);
+	dev_cap->rl_caps.num_rates = size;
+	if (dev_cap->rl_caps.num_rates) {
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_QP_RATE_LIMIT;
+		MLX4_GET(size, outbox, QUERY_DEV_CAP_QP_RATE_LIMIT_MAX_OFFSET);
+		dev_cap->rl_caps.max_val  = size & 0xfff;
+		dev_cap->rl_caps.max_unit = size >> 14;
+		MLX4_GET(size, outbox, QUERY_DEV_CAP_QP_RATE_LIMIT_MIN_OFFSET);
+		dev_cap->rl_caps.min_val  = size & 0xfff;
+		dev_cap->rl_caps.min_unit = size >> 14;
+	}
+
 	MLX4_GET(field32, outbox, QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET);
 	if (field32 & (1 << 16))
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_UPDATE_QP;
@@ -975,6 +1016,15 @@ void mlx4_dev_cap_dump(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		 dev_cap->dmfs_high_rate_qpn_base);
 	mlx4_dbg(dev, "DMFS high rate steer QPn range: %d\n",
 		 dev_cap->dmfs_high_rate_qpn_range);
+
+	if (dev_cap->flags2 & MLX4_DEV_CAP_FLAG2_QP_RATE_LIMIT) {
+		struct mlx4_rate_limit_caps *rl_caps = &dev_cap->rl_caps;
+
+		mlx4_dbg(dev, "QP Rate-Limit: #rates %d, unit/val max %d/%d, min %d/%d\n",
+			 rl_caps->num_rates, rl_caps->max_unit, rl_caps->max_val,
+			 rl_caps->min_unit, rl_caps->min_val);
+	}
+
 	dump_dev_cap_flags(dev, dev_cap->flags);
 	dump_dev_cap_flags2(dev, dev_cap->flags2);
 }
@@ -1058,6 +1108,7 @@ out:
 	return err;
 }
 
+#define DEV_CAP_EXT_2_FLAG_PFC_COUNTERS	(1 << 28)
 #define DEV_CAP_EXT_2_FLAG_VLAN_CONTROL (1 << 26)
 #define DEV_CAP_EXT_2_FLAG_80_VFS	(1 << 21)
 #define DEV_CAP_EXT_2_FLAG_FSM		(1 << 20)
@@ -1071,6 +1122,7 @@ int mlx4_QUERY_DEV_CAP_wrapper(struct mlx4_dev *dev, int slave,
 	u64	flags;
 	int	err = 0;
 	u8	field;
+	u16	field16;
 	u32	bmme_flags, field32;
 	int	real_port;
 	int	slave_port;
@@ -1101,6 +1153,9 @@ int mlx4_QUERY_DEV_CAP_wrapper(struct mlx4_dev *dev, int slave,
 	}
 	for (; slave_port < dev->caps.num_ports; ++slave_port)
 		flags &= ~(MLX4_DEV_CAP_FLAG_WOL_PORT1 << slave_port);
+
+	/* Not exposing RSS IP fragments to guests */
+	flags &= ~MLX4_DEV_CAP_FLAG_RSS_IP_FRAG;
 	MLX4_PUT(outbox->buf, flags, QUERY_DEV_CAP_EXT_FLAGS_OFFSET);
 
 	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_VL_PORT_OFFSET);
@@ -1113,11 +1168,16 @@ int mlx4_QUERY_DEV_CAP_wrapper(struct mlx4_dev *dev, int slave,
 	field &= 0x7f;
 	MLX4_PUT(outbox->buf, field, QUERY_DEV_CAP_CQ_TS_SUPPORT_OFFSET);
 
-	/* For guests, disable vxlan tunneling */
+	/* For guests, disable vxlan tunneling and QoS support */
 	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_VXLAN);
-	field &= 0xf7;
+	field &= 0xd7;
 	MLX4_PUT(outbox->buf, field, QUERY_DEV_CAP_VXLAN);
 
+	/* For guests, disable port BEACON */
+	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_PORT_BEACON_OFFSET);
+	field &= 0x7f;
+	MLX4_PUT(outbox->buf, field, QUERY_DEV_CAP_PORT_BEACON_OFFSET);
+
 	/* For guests, report Blueflame disabled */
 	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_BF_OFFSET);
 	field &= 0x7f;
@@ -1146,9 +1206,28 @@ int mlx4_QUERY_DEV_CAP_wrapper(struct mlx4_dev *dev, int slave,
 	/* turn off host side virt features (VST, FSM, etc) for guests */
 	MLX4_GET(field32, outbox->buf, QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET);
 	field32 &= ~(DEV_CAP_EXT_2_FLAG_VLAN_CONTROL | DEV_CAP_EXT_2_FLAG_80_VFS |
-		     DEV_CAP_EXT_2_FLAG_FSM);
+		     DEV_CAP_EXT_2_FLAG_FSM | DEV_CAP_EXT_2_FLAG_PFC_COUNTERS);
 	MLX4_PUT(outbox->buf, field32, QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET);
 
+	/* turn off QCN for guests */
+	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_ECN_QCN_VER_OFFSET);
+	field &= 0xfe;
+	MLX4_PUT(outbox->buf, field, QUERY_DEV_CAP_ECN_QCN_VER_OFFSET);
+
+	/* turn off QP max-rate limiting for guests */
+	field16 = 0;
+	MLX4_PUT(outbox->buf, field16, QUERY_DEV_CAP_QP_RATE_LIMIT_NUM_OFFSET);
+
+	/* turn off QoS per VF support for guests */
+	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_CQ_EQ_CACHE_LINE_STRIDE);
+	field &= 0xef;
+	MLX4_PUT(outbox->buf, field, QUERY_DEV_CAP_CQ_EQ_CACHE_LINE_STRIDE);
+
+	/* turn off ignore FCS feature for guests */
+	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_CONFIG_DEV_OFFSET);
+	field &= 0xfb;
+	MLX4_PUT(outbox->buf, field, QUERY_DEV_CAP_CONFIG_DEV_OFFSET);
+
 	return 0;
 }
 
@@ -1648,13 +1727,17 @@ int mlx4_INIT_HCA(struct mlx4_dev *dev, struct mlx4_init_hca_param *param)
 		*(inbox + INIT_HCA_FLAGS_OFFSET / 4) |= cpu_to_be32(1 << 3);
 
 	/* Enable QoS support if module parameter set */
-	if (enable_qos)
+	if (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ETS_CFG && enable_qos)
 		*(inbox + INIT_HCA_FLAGS_OFFSET / 4) |= cpu_to_be32(1 << 2);
 
 	/* enable counters */
 	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_COUNTERS)
 		*(inbox + INIT_HCA_FLAGS_OFFSET / 4) |= cpu_to_be32(1 << 4);
 
+	/* Enable RSS spread to fragmented IP packets when supported */
+	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_RSS_IP_FRAG)
+		*(inbox + INIT_HCA_FLAGS_OFFSET / 4) |= cpu_to_be32(1 << 13);
+
 	/* CX3 is capable of extending CQEs/EQEs from 32 to 64 bytes */
 	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_64B_EQE) {
 		*(inbox + INIT_HCA_EQE_CQE_OFFSETS / 4) |= cpu_to_be32(1 << 29);
@@ -1843,6 +1926,10 @@ int mlx4_QUERY_HCA(struct mlx4_dev *dev,
 		else
 			param->steering_mode = MLX4_STEERING_MODE_A0;
 	}
+
+	if (dword_field & (1 << 13))
+		param->rss_ip_frags = 1;
+
 	/* steering attributes */
 	if (param->steering_mode == MLX4_STEERING_MODE_DEVICE_MANAGED) {
 		MLX4_GET(param->mc_base, outbox, INIT_HCA_FS_BASE_OFFSET);
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index f44f7f6017ed..07cb7c2461ad 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -127,6 +127,7 @@ struct mlx4_dev_cap {
 	u32 max_counters;
 	u32 dmfs_high_rate_qpn_base;
 	u32 dmfs_high_rate_qpn_range;
+	struct mlx4_rate_limit_caps rl_caps;
 	struct mlx4_port_cap port_cap[MLX4_MAX_PORTS + 1];
 };
 
@@ -202,6 +203,7 @@ struct mlx4_init_hca_param {
 	u64 dev_cap_enabled;
 	u16 cqe_size; /* For use only when CQE stride feature enabled */
 	u16 eqe_size; /* For use only when EQE stride feature enabled */
+	u8 rss_ip_frags;
 };
 
 struct mlx4_init_ib_param {
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw_qos.c b/drivers/net/ethernet/mellanox/mlx4/fw_qos.c
new file mode 100644
index 000000000000..8f2fde0487c4
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx4/fw_qos.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies.
+ * All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/export.h>
+#include "fw_qos.h"
+#include "fw.h"
+
+enum {
+	/* allocate vpp opcode modifiers */
+	MLX4_ALLOCATE_VPP_ALLOCATE	= 0x0,
+	MLX4_ALLOCATE_VPP_QUERY		= 0x1
+};
+
+enum {
+	/* set vport qos opcode modifiers */
+	MLX4_SET_VPORT_QOS_SET		= 0x0,
+	MLX4_SET_VPORT_QOS_QUERY	= 0x1
+};
+
+struct mlx4_set_port_prio2tc_context {
+	u8 prio2tc[4];
+};
+
+struct mlx4_port_scheduler_tc_cfg_be {
+	__be16 pg;
+	__be16 bw_precentage;
+	__be16 max_bw_units; /* 3-100Mbps, 4-1Gbps, other values - reserved */
+	__be16 max_bw_value;
+};
+
+struct mlx4_set_port_scheduler_context {
+	struct mlx4_port_scheduler_tc_cfg_be tc[MLX4_NUM_TC];
+};
+
+/* Granular Qos (per VF) section */
+struct mlx4_alloc_vpp_param {
+	__be32 availible_vpp;
+	__be32 vpp_p_up[MLX4_NUM_UP];
+};
+
+struct mlx4_prio_qos_param {
+	__be32 bw_share;
+	__be32 max_avg_bw;
+	__be32 reserved;
+	__be32 enable;
+	__be32 reserved1[4];
+};
+
+struct mlx4_set_vport_context {
+	__be32 reserved[8];
+	struct mlx4_prio_qos_param qos_p_up[MLX4_NUM_UP];
+};
+
+int mlx4_SET_PORT_PRIO2TC(struct mlx4_dev *dev, u8 port, u8 *prio2tc)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_set_port_prio2tc_context *context;
+	int err;
+	u32 in_mod;
+	int i;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	context = mailbox->buf;
+
+	for (i = 0; i < MLX4_NUM_UP; i += 2)
+		context->prio2tc[i >> 1] = prio2tc[i] << 4 | prio2tc[i + 1];
+
+	in_mod = MLX4_SET_PORT_PRIO2TC << 8 | port;
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
+		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_SET_PORT_PRIO2TC);
+
+int mlx4_SET_PORT_SCHEDULER(struct mlx4_dev *dev, u8 port, u8 *tc_tx_bw,
+			    u8 *pg, u16 *ratelimit)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_set_port_scheduler_context *context;
+	int err;
+	u32 in_mod;
+	int i;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	context = mailbox->buf;
+
+	for (i = 0; i < MLX4_NUM_TC; i++) {
+		struct mlx4_port_scheduler_tc_cfg_be *tc = &context->tc[i];
+		u16 r;
+
+		if (ratelimit && ratelimit[i]) {
+			if (ratelimit[i] <= MLX4_MAX_100M_UNITS_VAL) {
+				r = ratelimit[i];
+				tc->max_bw_units =
+					htons(MLX4_RATELIMIT_100M_UNITS);
+			} else {
+				r = ratelimit[i] / 10;
+				tc->max_bw_units =
+					htons(MLX4_RATELIMIT_1G_UNITS);
+			}
+			tc->max_bw_value = htons(r);
+		} else {
+			tc->max_bw_value = htons(MLX4_RATELIMIT_DEFAULT);
+			tc->max_bw_units = htons(MLX4_RATELIMIT_1G_UNITS);
+		}
+
+		tc->pg = htons(pg[i]);
+		tc->bw_precentage = htons(tc_tx_bw[i]);
+	}
+
+	in_mod = MLX4_SET_PORT_SCHEDULER << 8 | port;
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
+		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_SET_PORT_SCHEDULER);
+
+int mlx4_ALLOCATE_VPP_get(struct mlx4_dev *dev, u8 port,
+			  u16 *availible_vpp, u8 *vpp_p_up)
+{
+	int i;
+	int err;
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_alloc_vpp_param *out_param;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	out_param = mailbox->buf;
+
+	err = mlx4_cmd_box(dev, 0, mailbox->dma, port,
+			   MLX4_ALLOCATE_VPP_QUERY,
+			   MLX4_CMD_ALLOCATE_VPP,
+			   MLX4_CMD_TIME_CLASS_A,
+			   MLX4_CMD_NATIVE);
+	if (err)
+		goto out;
+
+	/* Total number of supported VPPs */
+	*availible_vpp = (u16)be32_to_cpu(out_param->availible_vpp);
+
+	for (i = 0; i < MLX4_NUM_UP; i++)
+		vpp_p_up[i] = (u8)be32_to_cpu(out_param->vpp_p_up[i]);
+
+out:
+	mlx4_free_cmd_mailbox(dev, mailbox);
+
+	return err;
+}
+EXPORT_SYMBOL(mlx4_ALLOCATE_VPP_get);
+
+int mlx4_ALLOCATE_VPP_set(struct mlx4_dev *dev, u8 port, u8 *vpp_p_up)
+{
+	int i;
+	int err;
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_alloc_vpp_param *in_param;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	in_param = mailbox->buf;
+
+	for (i = 0; i < MLX4_NUM_UP; i++)
+		in_param->vpp_p_up[i] = cpu_to_be32(vpp_p_up[i]);
+
+	err = mlx4_cmd(dev, mailbox->dma, port,
+		       MLX4_ALLOCATE_VPP_ALLOCATE,
+		       MLX4_CMD_ALLOCATE_VPP,
+		       MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_ALLOCATE_VPP_set);
+
+int mlx4_SET_VPORT_QOS_get(struct mlx4_dev *dev, u8 port, u8 vport,
+			   struct mlx4_vport_qos_param *out_param)
+{
+	int i;
+	int err;
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_set_vport_context *ctx;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	ctx = mailbox->buf;
+
+	err = mlx4_cmd_box(dev, 0, mailbox->dma, (vport << 8) | port,
+			   MLX4_SET_VPORT_QOS_QUERY,
+			   MLX4_CMD_SET_VPORT_QOS,
+			   MLX4_CMD_TIME_CLASS_A,
+			   MLX4_CMD_NATIVE);
+	if (err)
+		goto out;
+
+	for (i = 0; i < MLX4_NUM_UP; i++) {
+		out_param[i].bw_share = be32_to_cpu(ctx->qos_p_up[i].bw_share);
+		out_param[i].max_avg_bw =
+			be32_to_cpu(ctx->qos_p_up[i].max_avg_bw);
+		out_param[i].enable =
+			!!(be32_to_cpu(ctx->qos_p_up[i].enable) & 31);
+	}
+
+out:
+	mlx4_free_cmd_mailbox(dev, mailbox);
+
+	return err;
+}
+EXPORT_SYMBOL(mlx4_SET_VPORT_QOS_get);
+
+int mlx4_SET_VPORT_QOS_set(struct mlx4_dev *dev, u8 port, u8 vport,
+			   struct mlx4_vport_qos_param *in_param)
+{
+	int i;
+	int err;
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_set_vport_context *ctx;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	ctx = mailbox->buf;
+
+	for (i = 0; i < MLX4_NUM_UP; i++) {
+		ctx->qos_p_up[i].bw_share = cpu_to_be32(in_param[i].bw_share);
+		ctx->qos_p_up[i].max_avg_bw =
+				cpu_to_be32(in_param[i].max_avg_bw);
+		ctx->qos_p_up[i].enable =
+				cpu_to_be32(in_param[i].enable << 31);
+	}
+
+	err = mlx4_cmd(dev, mailbox->dma, (vport << 8) | port,
+		       MLX4_SET_VPORT_QOS_SET,
+		       MLX4_CMD_SET_VPORT_QOS,
+		       MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_SET_VPORT_QOS_set);
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw_qos.h b/drivers/net/ethernet/mellanox/mlx4/fw_qos.h
new file mode 100644
index 000000000000..ac1f331878e6
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx4/fw_qos.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
+ * Copyright (c) 2005, 2006, 2007, 2008 Mellanox Technologies.
+ * All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef MLX4_FW_QOS_H
+#define MLX4_FW_QOS_H
+
+#include <linux/mlx4/cmd.h>
+#include <linux/mlx4/device.h>
+
+#define MLX4_NUM_UP 8
+#define MLX4_NUM_TC 8
+
+/* Default supported priorities for VPP allocation */
+#define MLX4_DEFAULT_QOS_PRIO (0)
+
+/* Derived from FW feature definition, 0 is the default vport fo all QPs */
+#define MLX4_VPP_DEFAULT_VPORT (0)
+
+struct mlx4_vport_qos_param {
+	u32 bw_share;
+	u32 max_avg_bw;
+	u8 enable;
+};
+
+/**
+ * mlx4_SET_PORT_PRIO2TC - This routine maps user priorities to traffic
+ * classes of a given port and device.
+ *
+ * @dev: mlx4_dev.
+ * @port: Physical port number.
+ * @prio2tc: Array of TC associated with each priorities.
+ *
+ * Returns 0 on success or a negative mlx4_core errno code.
+ **/
+int mlx4_SET_PORT_PRIO2TC(struct mlx4_dev *dev, u8 port, u8 *prio2tc);
+
+/**
+ * mlx4_SET_PORT_SCHEDULER - This routine configures the arbitration between
+ * traffic classes (ETS) and configured rate limit for traffic classes.
+ * tc_tx_bw, pg and ratelimit are arrays where each index represents a TC.
+ * The description for those parameters below refers to a single TC.
+ *
+ * @dev: mlx4_dev.
+ * @port: Physical port number.
+ * @tc_tx_bw: The percentage of the bandwidth allocated for traffic class
+ *  within a TC group. The sum of the bw_percentage of all the traffic
+ *  classes within a TC group must equal 100% for correct operation.
+ * @pg: The TC group the traffic class is associated with.
+ * @ratelimit: The maximal bandwidth allowed for the use by this traffic class.
+ *
+ * Returns 0 on success or a negative mlx4_core errno code.
+ **/
+int mlx4_SET_PORT_SCHEDULER(struct mlx4_dev *dev, u8 port, u8 *tc_tx_bw,
+			    u8 *pg, u16 *ratelimit);
+/**
+ * mlx4_ALLOCATE_VPP_get - Query port VPP availible resources and allocation.
+ * Before distribution of VPPs to priorities, only availible_vpp is returned.
+ * After initialization it returns the distribution of VPPs among priorities.
+ *
+ * @dev: mlx4_dev.
+ * @port: Physical port number.
+ * @availible_vpp: Pointer to variable where number of availible VPPs is stored
+ * @vpp_p_up: Distribution of VPPs to priorities is stored in this array
+ *
+ * Returns 0 on success or a negative mlx4_core errno code.
+ **/
+int mlx4_ALLOCATE_VPP_get(struct mlx4_dev *dev, u8 port,
+			  u16 *availible_vpp, u8 *vpp_p_up);
+/**
+ * mlx4_ALLOCATE_VPP_set - Distribution of VPPs among differnt priorities.
+ * The total number of VPPs assigned to all for a port must not exceed
+ * the value reported by availible_vpp in mlx4_ALLOCATE_VPP_get.
+ * VPP allocation is allowed only after the port type has been set,
+ * and while no QPs are open for this port.
+ *
+ * @dev: mlx4_dev.
+ * @port: Physical port number.
+ * @vpp_p_up: Allocation of VPPs to different priorities.
+ *
+ * Returns 0 on success or a negative mlx4_core errno code.
+ **/
+int mlx4_ALLOCATE_VPP_set(struct mlx4_dev *dev, u8 port, u8 *vpp_p_up);
+
+/**
+ * mlx4_SET_VPORT_QOS_get - Query QoS proporties of a Vport.
+ * Each priority allowed for the Vport is assigned with a share of the BW,
+ * and a BW limitation. This commands query the current QoS values.
+ *
+ * @dev: mlx4_dev.
+ * @port: Physical port number.
+ * @vport: Vport id.
+ * @out_param: Array of mlx4_vport_qos_param that will contain the values.
+ *
+ * Returns 0 on success or a negative mlx4_core errno code.
+ **/
+int mlx4_SET_VPORT_QOS_get(struct mlx4_dev *dev, u8 port, u8 vport,
+			   struct mlx4_vport_qos_param *out_param);
+
+/**
+ * mlx4_SET_VPORT_QOS_set - Set QoS proporties of a Vport.
+ * QoS parameters can be modified at any time, but must be initialized
+ * before any QP is associated with the VPort.
+ *
+ * @dev: mlx4_dev.
+ * @port: Physical port number.
+ * @vport: Vport id.
+ * @out_param: Array of mlx4_vport_qos_param which holds the requested values.
+ *
+ * Returns 0 on success or a negative mlx4_core errno code.
+ **/
+int mlx4_SET_VPORT_QOS_set(struct mlx4_dev *dev, u8 port, u8 vport,
+			   struct mlx4_vport_qos_param *in_param);
+
+#endif /* MLX4_FW_QOS_H */
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 7e487223489a..acceb75e8c44 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -297,6 +297,25 @@ static int mlx4_dev_port(struct mlx4_dev *dev, int port,
 	return err;
 }
 
+static inline void mlx4_enable_ignore_fcs(struct mlx4_dev *dev)
+{
+	if (!(dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_IGNORE_FCS))
+		return;
+
+	if (mlx4_is_mfunc(dev)) {
+		mlx4_dbg(dev, "SRIOV mode - Disabling Ignore FCS");
+		dev->caps.flags2 &= ~MLX4_DEV_CAP_FLAG2_IGNORE_FCS;
+		return;
+	}
+
+	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_FCS_KEEP)) {
+		mlx4_dbg(dev,
+			 "Keep FCS is not supported - Disabling Ignore FCS");
+		dev->caps.flags2 &= ~MLX4_DEV_CAP_FLAG2_IGNORE_FCS;
+		return;
+	}
+}
+
 #define MLX4_A0_STEERING_TABLE_SIZE	256
 static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 {
@@ -489,6 +508,8 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		dev->caps.dmfs_high_rate_qpn_range = MLX4_A0_STEERING_TABLE_SIZE;
 	}
 
+	dev->caps.rl_caps = dev_cap->rl_caps;
+
 	dev->caps.reserved_qps_cnt[MLX4_QP_REGION_RSS_RAW_ETH] =
 		dev->caps.dmfs_high_rate_qpn_range;
 
@@ -526,10 +547,20 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		dev->caps.alloc_res_qp_mask =
 			(dev->caps.bf_reg_size ? MLX4_RESERVE_ETH_BF_QP : 0) |
 			MLX4_RESERVE_A0_QP;
+
+		if (!(dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ETS_CFG) &&
+		    dev->caps.flags & MLX4_DEV_CAP_FLAG_SET_ETH_SCHED) {
+			mlx4_warn(dev, "Old device ETS support detected\n");
+			mlx4_warn(dev, "Consider upgrading device FW.\n");
+			dev->caps.flags2 |= MLX4_DEV_CAP_FLAG2_ETS_CFG;
+		}
+
 	} else {
 		dev->caps.alloc_res_qp_mask = 0;
 	}
 
+	mlx4_enable_ignore_fcs(dev);
+
 	return 0;
 }
 
@@ -883,6 +914,8 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 	mlx4_warn(dev, "Timestamping is not supported in slave mode\n");
 
 	slave_adjust_steering_mode(dev, &dev_cap, &hca_param);
+	mlx4_dbg(dev, "RSS support for IP fragments is %s\n",
+		 hca_param.rss_ip_frags ? "on" : "off");
 
 	if (func_cap.extra_flags & MLX4_QUERY_FUNC_FLAGS_BF_RES_QP &&
 	    dev->caps.bf_reg_size)
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index 1409d0cd6143..f30eeb730a86 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -50,6 +50,7 @@
 #include <linux/mlx4/driver.h>
 #include <linux/mlx4/doorbell.h>
 #include <linux/mlx4/cmd.h>
+#include "fw_qos.h"
 
 #define DRV_NAME	"mlx4_core"
 #define PFX		DRV_NAME ": "
@@ -64,21 +65,6 @@
 
 #define INIT_HCA_TPT_MW_ENABLE          (1 << 7)
 
-struct mlx4_set_port_prio2tc_context {
-	u8 prio2tc[4];
-};
-
-struct mlx4_port_scheduler_tc_cfg_be {
-	__be16 pg;
-	__be16 bw_precentage;
-	__be16 max_bw_units; /* 3-100Mbps, 4-1Gbps, other values - reserved */
-	__be16 max_bw_value;
-};
-
-struct mlx4_set_port_scheduler_context {
-	struct mlx4_port_scheduler_tc_cfg_be tc[MLX4_NUM_TC];
-};
-
 enum {
 	MLX4_HCR_BASE		= 0x80680,
 	MLX4_HCR_SIZE		= 0x0001c,
@@ -175,7 +161,7 @@ enum mlx4_res_tracker_free_type {
 
 /*
  *Virtual HCR structures.
- * mlx4_vhcr is the sw representation, in machine endianess
+ * mlx4_vhcr is the sw representation, in machine endianness
  *
  * mlx4_vhcr_cmd is the formalized structure, the one that is passed
  * to FW to go through communication channel.
@@ -512,6 +498,7 @@ struct mlx4_vport_state {
 	u32 tx_rate;
 	bool spoofchk;
 	u32 link_state;
+	u8 qos_vport;
 };
 
 struct mlx4_vf_admin_state {
@@ -568,6 +555,11 @@ struct mlx4_slave_event_eq {
 	struct mlx4_eqe event_eqe[SLAVE_EVENT_EQ_SIZE];
 };
 
+struct mlx4_qos_manager {
+	int num_of_qos_vfs;
+	DECLARE_BITMAP(priority_bm, MLX4_NUM_UP);
+};
+
 struct mlx4_master_qp0_state {
 	int proxy_qp0_active;
 	int qp0_active;
@@ -592,6 +584,7 @@ struct mlx4_mfunc_master_ctx {
 	struct mlx4_eqe		cmd_eqe;
 	struct mlx4_slave_event_eq slave_eq;
 	struct mutex		gen_eqe_mutex[MLX4_MFUNC_MAX];
+	struct mlx4_qos_manager qos_ctl[MLX4_MAX_PORTS + 1];
 };
 
 struct mlx4_mfunc {
@@ -644,6 +637,7 @@ struct mlx4_vf_immed_vlan_work {
 	int			orig_vlan_ix;
 	u8			port;
 	u8			qos;
+	u8                      qos_vport;
 	u16			vlan_id;
 	u16			orig_vlan_id;
 };
@@ -769,9 +763,11 @@ enum {
 
 
 struct mlx4_set_port_general_context {
-	u8 reserved[3];
+	u16 reserved1;
+	u8 v_ignore_fcs;
 	u8 flags;
-	u16 reserved2;
+	u8 ignore_fcs;
+	u8 reserved2;
 	__be16 mtu;
 	u8 pptx;
 	u8 pfctx;
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 2a8268e6be15..9de30216b146 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -55,6 +55,7 @@
 #include <linux/mlx4/cmd.h>
 
 #include "en_port.h"
+#include "mlx4_stats.h"
 
 #define DRV_NAME	"mlx4_en"
 #define DRV_VERSION	"2.2-1"
@@ -171,7 +172,6 @@ enum {
 /* Number of samples to 'average' */
 #define AVG_SIZE			128
 #define AVG_FACTOR			1024
-#define NUM_PERF_STATS			NUM_PERF_COUNTERS
 
 #define INC_PERF_COUNTER(cnt)		(++(cnt))
 #define ADD_PERF_COUNTER(cnt, add)	((cnt) += (add))
@@ -182,7 +182,6 @@ enum {
 
 #else
 
-#define NUM_PERF_STATS			0
 #define INC_PERF_COUNTER(cnt)		do {} while (0)
 #define ADD_PERF_COUNTER(cnt, add)	do {} while (0)
 #define AVG_PERF_COUNTER(cnt, sample)	do {} while (0)
@@ -435,37 +434,6 @@ struct mlx4_en_port_state {
 	u32 flags;
 };
 
-struct mlx4_en_pkt_stats {
-	unsigned long broadcast;
-	unsigned long rx_prio[8];
-	unsigned long tx_prio[8];
-#define NUM_PKT_STATS		17
-};
-
-struct mlx4_en_port_stats {
-	unsigned long tso_packets;
-	unsigned long xmit_more;
-	unsigned long queue_stopped;
-	unsigned long wake_queue;
-	unsigned long tx_timeout;
-	unsigned long rx_alloc_failed;
-	unsigned long rx_chksum_good;
-	unsigned long rx_chksum_none;
-	unsigned long rx_chksum_complete;
-	unsigned long tx_chksum_offload;
-#define NUM_PORT_STATS		9
-};
-
-struct mlx4_en_perf_stats {
-	u32 tx_poll;
-	u64 tx_pktsz_avg;
-	u32 inflight_avg;
-	u16 tx_coal_avg;
-	u16 rx_coal_avg;
-	u32 napi_quota;
-#define NUM_PERF_COUNTERS		6
-};
-
 enum mlx4_en_mclist_act {
 	MCLIST_NONE,
 	MCLIST_REM,
@@ -514,9 +482,15 @@ enum {
 	MLX4_EN_FLAG_RX_CSUM_NON_TCP_UDP	= (1 << 5),
 };
 
+#define PORT_BEACON_MAX_LIMIT (65535)
 #define MLX4_EN_MAC_HASH_SIZE (1 << BITS_PER_BYTE)
 #define MLX4_EN_MAC_HASH_IDX 5
 
+struct mlx4_en_stats_bitmap {
+	DECLARE_BITMAP(bitmap, NUM_ALL_STATS);
+	struct mutex mutex; /* for mutual access to stats bitmap */
+};
+
 struct mlx4_en_priv {
 	struct mlx4_en_dev *mdev;
 	struct mlx4_en_port_profile *prof;
@@ -592,8 +566,12 @@ struct mlx4_en_priv {
 #endif
 	struct mlx4_en_perf_stats pstats;
 	struct mlx4_en_pkt_stats pkstats;
+	struct mlx4_en_flow_stats_rx rx_priority_flowstats[MLX4_NUM_PRIORITIES];
+	struct mlx4_en_flow_stats_tx tx_priority_flowstats[MLX4_NUM_PRIORITIES];
+	struct mlx4_en_flow_stats_rx rx_flowstats;
+	struct mlx4_en_flow_stats_tx tx_flowstats;
 	struct mlx4_en_port_stats port_stats;
-	u64 stats_bitmap;
+	struct mlx4_en_stats_bitmap stats_bitmap;
 	struct list_head mc_list;
 	struct list_head curr_list;
 	u64 broadcast_id;
@@ -608,6 +586,7 @@ struct mlx4_en_priv {
 #ifdef CONFIG_MLX4_EN_DCB
 	struct ieee_ets ets;
 	u16 maxrate[IEEE_8021QAZ_MAX_TCS];
+	enum dcbnl_cndd_states cndd_state[IEEE_8021QAZ_MAX_TCS];
 #endif
 #ifdef CONFIG_RFS_ACCEL
 	spinlock_t filters_lock;
@@ -761,6 +740,11 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 int mlx4_en_start_port(struct net_device *dev);
 void mlx4_en_stop_port(struct net_device *dev, int detach);
 
+void mlx4_en_set_stats_bitmap(struct mlx4_dev *dev,
+			      struct mlx4_en_stats_bitmap *stats_bitmap,
+			      u8 rx_ppp, u8 rx_pause,
+			      u8 tx_ppp, u8 tx_pause);
+
 void mlx4_en_free_resources(struct mlx4_en_priv *priv);
 int mlx4_en_alloc_resources(struct mlx4_en_priv *priv);
 
@@ -846,7 +830,10 @@ void mlx4_en_ptp_overflow_check(struct mlx4_en_dev *mdev);
 int mlx4_en_reset_config(struct net_device *dev,
 			 struct hwtstamp_config ts_config,
 			 netdev_features_t new_features);
-
+void mlx4_en_update_pfc_stats_bitmap(struct mlx4_dev *dev,
+				     struct mlx4_en_stats_bitmap *stats_bitmap,
+				     u8 rx_ppp, u8 rx_pause,
+				     u8 tx_ppp, u8 tx_pause);
 int mlx4_en_netdev_event(struct notifier_block *this,
 			 unsigned long event, void *ptr);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_stats.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_stats.h
new file mode 100644
index 000000000000..00555832a4ae
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_stats.h
@@ -0,0 +1,107 @@
+#ifndef _MLX4_STATS_
+#define _MLX4_STATS_
+
+#ifdef MLX4_EN_PERF_STAT
+#define NUM_PERF_STATS			NUM_PERF_COUNTERS
+#else
+#define NUM_PERF_STATS			0
+#endif
+
+#define NUM_PRIORITIES	9
+#define NUM_PRIORITY_STATS 2
+
+struct mlx4_en_pkt_stats {
+	unsigned long rx_multicast_packets;
+	unsigned long rx_broadcast_packets;
+	unsigned long rx_jabbers;
+	unsigned long rx_in_range_length_error;
+	unsigned long rx_out_range_length_error;
+	unsigned long tx_multicast_packets;
+	unsigned long tx_broadcast_packets;
+	unsigned long rx_prio[NUM_PRIORITIES][NUM_PRIORITY_STATS];
+	unsigned long tx_prio[NUM_PRIORITIES][NUM_PRIORITY_STATS];
+#define NUM_PKT_STATS		43
+};
+
+struct mlx4_en_port_stats {
+	unsigned long tso_packets;
+	unsigned long xmit_more;
+	unsigned long queue_stopped;
+	unsigned long wake_queue;
+	unsigned long tx_timeout;
+	unsigned long rx_alloc_failed;
+	unsigned long rx_chksum_good;
+	unsigned long rx_chksum_none;
+	unsigned long rx_chksum_complete;
+	unsigned long tx_chksum_offload;
+#define NUM_PORT_STATS		10
+};
+
+struct mlx4_en_perf_stats {
+	u32 tx_poll;
+	u64 tx_pktsz_avg;
+	u32 inflight_avg;
+	u16 tx_coal_avg;
+	u16 rx_coal_avg;
+	u32 napi_quota;
+#define NUM_PERF_COUNTERS		6
+};
+
+#define NUM_MAIN_STATS	21
+
+#define MLX4_NUM_PRIORITIES	8
+
+struct mlx4_en_flow_stats_rx {
+	u64 rx_pause;
+	u64 rx_pause_duration;
+	u64 rx_pause_transition;
+#define NUM_FLOW_STATS_RX	3
+#define NUM_FLOW_PRIORITY_STATS_RX	(NUM_FLOW_STATS_RX * \
+					 MLX4_NUM_PRIORITIES)
+};
+
+struct mlx4_en_flow_stats_tx {
+	u64 tx_pause;
+	u64 tx_pause_duration;
+	u64 tx_pause_transition;
+#define NUM_FLOW_STATS_TX	3
+#define NUM_FLOW_PRIORITY_STATS_TX	(NUM_FLOW_STATS_TX * \
+					 MLX4_NUM_PRIORITIES)
+};
+
+#define NUM_FLOW_STATS (NUM_FLOW_STATS_RX + NUM_FLOW_STATS_TX + \
+			NUM_FLOW_PRIORITY_STATS_TX + \
+			NUM_FLOW_PRIORITY_STATS_RX)
+
+struct mlx4_en_stat_out_flow_control_mbox {
+	/* Total number of PAUSE frames received from the far-end port */
+	__be64 rx_pause;
+	/* Total number of microseconds that far-end port requested to pause
+	* transmission of packets
+	*/
+	__be64 rx_pause_duration;
+	/* Number of received transmission from XOFF state to XON state */
+	__be64 rx_pause_transition;
+	/* Total number of PAUSE frames sent from the far-end port */
+	__be64 tx_pause;
+	/* Total time in microseconds that transmission of packets has been
+	* paused
+	*/
+	__be64 tx_pause_duration;
+	/* Number of transmitter transitions from XOFF state to XON state */
+	__be64 tx_pause_transition;
+	/* Reserverd */
+	__be64 reserved[2];
+};
+
+enum {
+	MLX4_DUMP_ETH_STATS_FLOW_CONTROL = 1 << 12
+};
+
+#define NUM_ALL_STATS	(NUM_MAIN_STATS + NUM_PORT_STATS + NUM_PKT_STATS + \
+			 NUM_FLOW_STATS + NUM_PERF_STATS)
+
+#define MLX4_FIND_NETDEV_STAT(n) (offsetof(struct net_device_stats, n) / \
+				  sizeof(((struct net_device_stats *)0)->n))
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index 9f268f05290a..c2b21313dba7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -38,6 +38,7 @@
 #include <linux/mlx4/cmd.h>
 
 #include "mlx4.h"
+#include "mlx4_stats.h"
 
 #define MLX4_MAC_VALID		(1ull << 63)
 
@@ -49,6 +50,9 @@
 #define MLX4_STATS_ERROR_COUNTERS_MASK		0x1ffc30ULL
 #define MLX4_STATS_PORT_COUNTERS_MASK		0x1fe00000ULL
 
+#define MLX4_FLAG_V_IGNORE_FCS_MASK		0x2
+#define MLX4_IGNORE_FCS_MASK			0x1
+
 void mlx4_init_mac_table(struct mlx4_dev *dev, struct mlx4_mac_table *table)
 {
 	int i;
@@ -127,8 +131,9 @@ static int mlx4_set_port_mac_table(struct mlx4_dev *dev, u8 port,
 
 	in_mod = MLX4_SET_PORT_MAC_TABLE << 8 | port;
 
-	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, MLX4_SET_PORT_ETH_OPCODE,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_NATIVE);
 
 	mlx4_free_cmd_mailbox(dev, mailbox);
 	return err;
@@ -341,8 +346,9 @@ static int mlx4_set_port_vlan_table(struct mlx4_dev *dev, u8 port,
 
 	memcpy(mailbox->buf, entries, MLX4_VLAN_TABLE_SIZE);
 	in_mod = MLX4_SET_PORT_VLAN_TABLE << 8 | port;
-	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, MLX4_SET_PORT_ETH_OPCODE,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_NATIVE);
 
 	mlx4_free_cmd_mailbox(dev, mailbox);
 
@@ -629,9 +635,9 @@ static int mlx4_reset_roce_port_gids(struct mlx4_dev *dev, int slave,
 		       MLX4_ROCE_GID_ENTRY_SIZE);
 
 	err = mlx4_cmd(dev, mailbox->dma,
-		       ((u32)port) | (MLX4_SET_PORT_GID_TABLE << 8), 1,
-		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
-		       MLX4_CMD_NATIVE);
+		       ((u32)port) | (MLX4_SET_PORT_GID_TABLE << 8),
+		       MLX4_SET_PORT_ETH_OPCODE, MLX4_CMD_SET_PORT,
+		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
 	mutex_unlock(&(priv->port[port].gid_table.mutex));
 	return err;
 }
@@ -837,6 +843,12 @@ static int mlx4_common_set_port(struct mlx4_dev *dev, int slave, u32 in_mod,
 				MLX4_CMD_NATIVE);
 	}
 
+	/* Slaves are not allowed to SET_PORT beacon (LED) blink */
+	if (op_mod == MLX4_SET_PORT_BEACON_OPCODE) {
+		mlx4_warn(dev, "denying SET_PORT Beacon slave:%d\n", slave);
+		return -EPERM;
+	}
+
 	/* For IB, we only consider:
 	 * - The capability mask, which is set to the aggregate of all
 	 *   slave function capabilities
@@ -945,8 +957,9 @@ int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port, int pkey_tbl_sz)
 			(pkey_tbl_flag << MLX4_CHANGE_PORT_PKEY_TBL_SZ) |
 			(dev->caps.port_ib_mtu[port] << MLX4_SET_PORT_MTU_CAP) |
 			(vl_cap << MLX4_SET_PORT_VL_CAP));
-		err = mlx4_cmd(dev, mailbox->dma, port, 0, MLX4_CMD_SET_PORT,
-				MLX4_CMD_TIME_CLASS_B, MLX4_CMD_WRAPPED);
+		err = mlx4_cmd(dev, mailbox->dma, port,
+			       MLX4_SET_PORT_IB_OPCODE, MLX4_CMD_SET_PORT,
+			       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_WRAPPED);
 		if (err != -ENOMEM)
 			break;
 	}
@@ -975,8 +988,9 @@ int mlx4_SET_PORT_general(struct mlx4_dev *dev, u8 port, int mtu,
 	context->pfcrx = pfcrx;
 
 	in_mod = MLX4_SET_PORT_GENERAL << 8 | port;
-	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B,  MLX4_CMD_WRAPPED);
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, MLX4_SET_PORT_ETH_OPCODE,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_WRAPPED);
 
 	mlx4_free_cmd_mailbox(dev, mailbox);
 	return err;
@@ -1012,84 +1026,40 @@ int mlx4_SET_PORT_qpn_calc(struct mlx4_dev *dev, u8 port, u32 base_qpn,
 	context->vlan_miss = MLX4_VLAN_MISS_IDX;
 
 	in_mod = MLX4_SET_PORT_RQP_CALC << 8 | port;
-	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B,  MLX4_CMD_WRAPPED);
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, MLX4_SET_PORT_ETH_OPCODE,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_WRAPPED);
 
 	mlx4_free_cmd_mailbox(dev, mailbox);
 	return err;
 }
 EXPORT_SYMBOL(mlx4_SET_PORT_qpn_calc);
 
-int mlx4_SET_PORT_PRIO2TC(struct mlx4_dev *dev, u8 port, u8 *prio2tc)
+int mlx4_SET_PORT_fcs_check(struct mlx4_dev *dev, u8 port, u8 ignore_fcs_value)
 {
 	struct mlx4_cmd_mailbox *mailbox;
-	struct mlx4_set_port_prio2tc_context *context;
-	int err;
+	struct mlx4_set_port_general_context *context;
 	u32 in_mod;
-	int i;
-
-	mailbox = mlx4_alloc_cmd_mailbox(dev);
-	if (IS_ERR(mailbox))
-		return PTR_ERR(mailbox);
-	context = mailbox->buf;
-	for (i = 0; i < MLX4_NUM_UP; i += 2)
-		context->prio2tc[i >> 1] = prio2tc[i] << 4 | prio2tc[i + 1];
-
-	in_mod = MLX4_SET_PORT_PRIO2TC << 8 | port;
-	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
-
-	mlx4_free_cmd_mailbox(dev, mailbox);
-	return err;
-}
-EXPORT_SYMBOL(mlx4_SET_PORT_PRIO2TC);
-
-int mlx4_SET_PORT_SCHEDULER(struct mlx4_dev *dev, u8 port, u8 *tc_tx_bw,
-		u8 *pg, u16 *ratelimit)
-{
-	struct mlx4_cmd_mailbox *mailbox;
-	struct mlx4_set_port_scheduler_context *context;
 	int err;
-	u32 in_mod;
-	int i;
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox))
 		return PTR_ERR(mailbox);
 	context = mailbox->buf;
+	context->v_ignore_fcs |= MLX4_FLAG_V_IGNORE_FCS_MASK;
+	if (ignore_fcs_value)
+		context->ignore_fcs |= MLX4_IGNORE_FCS_MASK;
+	else
+		context->ignore_fcs &= ~MLX4_IGNORE_FCS_MASK;
 
-	for (i = 0; i < MLX4_NUM_TC; i++) {
-		struct mlx4_port_scheduler_tc_cfg_be *tc = &context->tc[i];
-		u16 r;
-
-		if (ratelimit && ratelimit[i]) {
-			if (ratelimit[i] <= MLX4_MAX_100M_UNITS_VAL) {
-				r = ratelimit[i];
-				tc->max_bw_units =
-					htons(MLX4_RATELIMIT_100M_UNITS);
-			} else {
-				r = ratelimit[i]/10;
-				tc->max_bw_units =
-					htons(MLX4_RATELIMIT_1G_UNITS);
-			}
-			tc->max_bw_value = htons(r);
-		} else {
-			tc->max_bw_value = htons(MLX4_RATELIMIT_DEFAULT);
-			tc->max_bw_units = htons(MLX4_RATELIMIT_1G_UNITS);
-		}
-
-		tc->pg = htons(pg[i]);
-		tc->bw_precentage = htons(tc_tx_bw[i]);
-	}
-
-	in_mod = MLX4_SET_PORT_SCHEDULER << 8 | port;
+	in_mod = MLX4_SET_PORT_GENERAL << 8 | port;
 	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
 		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
 
 	mlx4_free_cmd_mailbox(dev, mailbox);
 	return err;
 }
-EXPORT_SYMBOL(mlx4_SET_PORT_SCHEDULER);
+EXPORT_SYMBOL(mlx4_SET_PORT_fcs_check);
 
 enum {
 	VXLAN_ENABLE_MODIFY	= 1 << 7,
@@ -1125,14 +1095,35 @@ int mlx4_SET_PORT_VXLAN(struct mlx4_dev *dev, u8 port, u8 steering, int enable)
 	context->steering  = steering;
 
 	in_mod = MLX4_SET_PORT_VXLAN << 8 | port;
-	err = mlx4_cmd(dev, mailbox->dma, in_mod, 1, MLX4_CMD_SET_PORT,
-		       MLX4_CMD_TIME_CLASS_B, MLX4_CMD_NATIVE);
+	err = mlx4_cmd(dev, mailbox->dma, in_mod, MLX4_SET_PORT_ETH_OPCODE,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_NATIVE);
 
 	mlx4_free_cmd_mailbox(dev, mailbox);
 	return err;
 }
 EXPORT_SYMBOL(mlx4_SET_PORT_VXLAN);
 
+int mlx4_SET_PORT_BEACON(struct mlx4_dev *dev, u8 port, u16 time)
+{
+	int err;
+	struct mlx4_cmd_mailbox *mailbox;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	*((__be32 *)mailbox->buf) = cpu_to_be32(time);
+
+	err = mlx4_cmd(dev, mailbox->dma, port, MLX4_SET_PORT_BEACON_OPCODE,
+		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_SET_PORT_BEACON);
+
 int mlx4_SET_MCAST_FLTR_wrapper(struct mlx4_dev *dev, int slave,
 				struct mlx4_vhcr *vhcr,
 				struct mlx4_cmd_mailbox *inbox,
@@ -1184,22 +1175,6 @@ int mlx4_DUMP_ETH_STATS_wrapper(struct mlx4_dev *dev, int slave,
 					  vhcr->in_modifier, outbox);
 }
 
-void mlx4_set_stats_bitmap(struct mlx4_dev *dev, u64 *stats_bitmap)
-{
-	if (!mlx4_is_mfunc(dev)) {
-		*stats_bitmap = 0;
-		return;
-	}
-
-	*stats_bitmap = (MLX4_STATS_TRAFFIC_COUNTERS_MASK |
-			 MLX4_STATS_TRAFFIC_DROPS_MASK |
-			 MLX4_STATS_PORT_COUNTERS_MASK);
-
-	if (mlx4_is_master(dev))
-		*stats_bitmap |= MLX4_STATS_ERROR_COUNTERS_MASK;
-}
-EXPORT_SYMBOL(mlx4_set_stats_bitmap);
-
 int mlx4_get_slave_from_roce_gid(struct mlx4_dev *dev, int port, u8 *gid,
 				 int *slave_id)
 {
diff --git a/drivers/net/ethernet/mellanox/mlx4/qp.c b/drivers/net/ethernet/mellanox/mlx4/qp.c
index eda29dbbfcd2..b75214a80d0e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx4/qp.c
@@ -442,6 +442,16 @@ int mlx4_update_qp(struct mlx4_dev *dev, u32 qpn,
 			cmd->qp_context.param3 |= cpu_to_be32(MLX4_STRIP_VLAN);
 	}
 
+	if (attr & MLX4_UPDATE_QP_RATE_LIMIT) {
+		qp_mask |= 1ULL << MLX4_UPD_QP_MASK_RATE_LIMIT;
+		cmd->qp_context.rate_limit_params = cpu_to_be16((params->rate_unit << 14) | params->rate_val);
+	}
+
+	if (attr & MLX4_UPDATE_QP_QOS_VPORT) {
+		qp_mask |= 1ULL << MLX4_UPD_QP_MASK_QOS_VPP;
+		cmd->qp_context.qos_vport = params->qos_vport;
+	}
+
 	cmd->primary_addr_path_mask = cpu_to_be64(pri_addr_path_mask);
 	cmd->qp_mask = cpu_to_be64(qp_mask);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index d97ca88c55b5..c7f28bf4b8e2 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -221,11 +221,6 @@ struct res_fs_rule {
 	int			qpn;
 };
 
-static int mlx4_is_eth(struct mlx4_dev *dev, int port)
-{
-	return dev->caps.port_mask[port] == MLX4_PORT_TYPE_IB ? 0 : 1;
-}
-
 static void *res_tracker_lookup(struct rb_root *root, u64 res_id)
 {
 	struct rb_node *node = root->rb_node;
@@ -770,6 +765,7 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 		qpc->pri_path.feup |= MLX4_FEUP_FORCE_ETH_UP | MLX4_FVL_FORCE_ETH_VLAN;
 		qpc->pri_path.sched_queue &= 0xC7;
 		qpc->pri_path.sched_queue |= (vp_oper->state.default_qos) << 3;
+		qpc->qos_vport = vp_oper->state.qos_vport;
 	}
 	if (vp_oper->state.spoofchk) {
 		qpc->pri_path.feup |= MLX4_FSM_FORCE_ETH_SRC_MAC;
@@ -2947,8 +2943,12 @@ static int verify_qp_parameters(struct mlx4_dev *dev,
 	qp_type	= (be32_to_cpu(qp_ctx->flags) >> 16) & 0xff;
 	optpar	= be32_to_cpu(*(__be32 *) inbox->buf);
 
-	if (slave != mlx4_master_func_num(dev))
+	if (slave != mlx4_master_func_num(dev)) {
 		qp_ctx->params2 &= ~MLX4_QP_BIT_FPP;
+		/* setting QP rate-limit is disallowed for VFs */
+		if (qp_ctx->rate_limit_params)
+			return -EPERM;
+	}
 
 	switch (qp_type) {
 	case MLX4_QP_ST_RC:
@@ -3027,7 +3027,7 @@ int mlx4_WRITE_MTT_wrapper(struct mlx4_dev *dev, int slave,
 
 	/* Call the SW implementation of write_mtt:
 	 * - Prepare a dummy mtt struct
-	 * - Translate inbox contents to simple addresses in host endianess */
+	 * - Translate inbox contents to simple addresses in host endianness */
 	mtt.offset = 0;  /* TBD this is broken but I don't handle it since
 			    we don't really use it */
 	mtt.order = 0;
@@ -3095,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 */
@@ -4912,6 +4918,11 @@ void mlx4_vf_immed_vlan_work_handler(struct work_struct *_work)
 					qp->sched_queue & 0xC7;
 				upd_context->qp_context.pri_path.sched_queue |=
 					((work->qos & 0x7) << 3);
+				upd_context->qp_mask |=
+					cpu_to_be64(1ULL <<
+						    MLX4_UPD_QP_MASK_QOS_VPP);
+				upd_context->qp_context.qos_vport =
+					work->qos_vport;
 			}
 
 			err = mlx4_cmd(dev, mailbox->dma,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/alloc.c b/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
index 201ca6d76ce5..ac0f7bf4be95 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -171,6 +171,9 @@ static int mlx5_alloc_db_from_pgdir(struct mlx5_db_pgdir *pgdir,
 	db->db      = pgdir->db_page + offset / sizeof(*pgdir->db_page);
 	db->dma     = pgdir->db_dma  + offset;
 
+	db->db[0] = 0;
+	db->db[1] = 0;
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
index a2853057c779..e3273faf4568 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -125,7 +125,10 @@ static u8 alloc_token(struct mlx5_cmd *cmd)
 	u8 token;
 
 	spin_lock(&cmd->token_lock);
-	token = cmd->token++ % 255 + 1;
+	cmd->token++;
+	if (cmd->token == 0)
+		cmd->token++;
+	token = cmd->token;
 	spin_unlock(&cmd->token_lock);
 
 	return token;
@@ -515,10 +518,11 @@ static void cmd_work_handler(struct work_struct *work)
 	ent->ts1 = ktime_get_ns();
 
 	/* ring doorbell after the descriptor is valid */
+	mlx5_core_dbg(dev, "writing 0x%x to command doorbell\n", 1 << ent->idx);
 	wmb();
 	iowrite32be(1 << ent->idx, &dev->iseg->cmd_dbell);
-	mlx5_core_dbg(dev, "write 0x%x to command doorbell\n", 1 << ent->idx);
 	mmiowb();
+	/* if not in polling don't use ent after this point */
 	if (cmd->mode == CMD_MODE_POLLING) {
 		poll_timeout(ent);
 		/* make sure we read the descriptor after ownership is SW */
@@ -1236,7 +1240,8 @@ static int cmd_exec(struct mlx5_core_dev *dev, void *in, int in_size, void *out,
 		goto out_out;
 	}
 
-	err = mlx5_copy_from_msg(out, outb, out_size);
+	if (!callback)
+		err = mlx5_copy_from_msg(out, outb, out_size);
 
 out_out:
 	if (!callback)
@@ -1319,6 +1324,45 @@ ex_err:
 	return err;
 }
 
+static int alloc_cmd_page(struct mlx5_core_dev *dev, struct mlx5_cmd *cmd)
+{
+	struct device *ddev = &dev->pdev->dev;
+
+	cmd->cmd_alloc_buf = dma_zalloc_coherent(ddev, MLX5_ADAPTER_PAGE_SIZE,
+						 &cmd->alloc_dma, GFP_KERNEL);
+	if (!cmd->cmd_alloc_buf)
+		return -ENOMEM;
+
+	/* make sure it is aligned to 4K */
+	if (!((uintptr_t)cmd->cmd_alloc_buf & (MLX5_ADAPTER_PAGE_SIZE - 1))) {
+		cmd->cmd_buf = cmd->cmd_alloc_buf;
+		cmd->dma = cmd->alloc_dma;
+		cmd->alloc_size = MLX5_ADAPTER_PAGE_SIZE;
+		return 0;
+	}
+
+	dma_free_coherent(ddev, MLX5_ADAPTER_PAGE_SIZE, cmd->cmd_alloc_buf,
+			  cmd->alloc_dma);
+	cmd->cmd_alloc_buf = dma_zalloc_coherent(ddev,
+						 2 * MLX5_ADAPTER_PAGE_SIZE - 1,
+						 &cmd->alloc_dma, GFP_KERNEL);
+	if (!cmd->cmd_alloc_buf)
+		return -ENOMEM;
+
+	cmd->cmd_buf = PTR_ALIGN(cmd->cmd_alloc_buf, MLX5_ADAPTER_PAGE_SIZE);
+	cmd->dma = ALIGN(cmd->alloc_dma, MLX5_ADAPTER_PAGE_SIZE);
+	cmd->alloc_size = 2 * MLX5_ADAPTER_PAGE_SIZE - 1;
+	return 0;
+}
+
+static void free_cmd_page(struct mlx5_core_dev *dev, struct mlx5_cmd *cmd)
+{
+	struct device *ddev = &dev->pdev->dev;
+
+	dma_free_coherent(ddev, cmd->alloc_size, cmd->cmd_alloc_buf,
+			  cmd->alloc_dma);
+}
+
 int mlx5_cmd_init(struct mlx5_core_dev *dev)
 {
 	int size = sizeof(struct mlx5_cmd_prot_block);
@@ -1341,17 +1385,9 @@ int mlx5_cmd_init(struct mlx5_core_dev *dev)
 	if (!cmd->pool)
 		return -ENOMEM;
 
-	cmd->cmd_buf = (void *)__get_free_pages(GFP_ATOMIC, 0);
-	if (!cmd->cmd_buf) {
-		err = -ENOMEM;
+	err = alloc_cmd_page(dev, cmd);
+	if (err)
 		goto err_free_pool;
-	}
-	cmd->dma = dma_map_single(&dev->pdev->dev, cmd->cmd_buf, PAGE_SIZE,
-				  DMA_BIDIRECTIONAL);
-	if (dma_mapping_error(&dev->pdev->dev, cmd->dma)) {
-		err = -ENOMEM;
-		goto err_free;
-	}
 
 	cmd_l = ioread32be(&dev->iseg->cmdq_addr_l_sz) & 0xff;
 	cmd->log_sz = cmd_l >> 4 & 0xf;
@@ -1360,13 +1396,13 @@ int mlx5_cmd_init(struct mlx5_core_dev *dev)
 		dev_err(&dev->pdev->dev, "firmware reports too many outstanding commands %d\n",
 			1 << cmd->log_sz);
 		err = -EINVAL;
-		goto err_map;
+		goto err_free_page;
 	}
 
 	if (cmd->log_sz + cmd->log_stride > MLX5_ADAPTER_PAGE_SHIFT) {
 		dev_err(&dev->pdev->dev, "command queue size overflow\n");
 		err = -EINVAL;
-		goto err_map;
+		goto err_free_page;
 	}
 
 	cmd->checksum_disabled = 1;
@@ -1378,7 +1414,7 @@ int mlx5_cmd_init(struct mlx5_core_dev *dev)
 		dev_err(&dev->pdev->dev, "driver does not support command interface version. driver %d, firmware %d\n",
 			CMD_IF_REV, cmd->cmdif_rev);
 		err = -ENOTSUPP;
-		goto err_map;
+		goto err_free_page;
 	}
 
 	spin_lock_init(&cmd->alloc_lock);
@@ -1394,7 +1430,7 @@ int mlx5_cmd_init(struct mlx5_core_dev *dev)
 	if (cmd_l & 0xfff) {
 		dev_err(&dev->pdev->dev, "invalid command queue address\n");
 		err = -ENOMEM;
-		goto err_map;
+		goto err_free_page;
 	}
 
 	iowrite32be(cmd_h, &dev->iseg->cmdq_addr_h);
@@ -1410,7 +1446,7 @@ int mlx5_cmd_init(struct mlx5_core_dev *dev)
 	err = create_msg_cache(dev);
 	if (err) {
 		dev_err(&dev->pdev->dev, "failed to create command cache\n");
-		goto err_map;
+		goto err_free_page;
 	}
 
 	set_wqname(dev);
@@ -1435,11 +1471,8 @@ err_wq:
 err_cache:
 	destroy_msg_cache(dev);
 
-err_map:
-	dma_unmap_single(&dev->pdev->dev, cmd->dma, PAGE_SIZE,
-			 DMA_BIDIRECTIONAL);
-err_free:
-	free_pages((unsigned long)cmd->cmd_buf, 0);
+err_free_page:
+	free_cmd_page(dev, cmd);
 
 err_free_pool:
 	pci_pool_destroy(cmd->pool);
@@ -1455,9 +1488,7 @@ void mlx5_cmd_cleanup(struct mlx5_core_dev *dev)
 	clean_debug_files(dev);
 	destroy_workqueue(cmd->wq);
 	destroy_msg_cache(dev);
-	dma_unmap_single(&dev->pdev->dev, cmd->dma, PAGE_SIZE,
-			 DMA_BIDIRECTIONAL);
-	free_pages((unsigned long)cmd->cmd_buf, 0);
+	free_cmd_page(dev, cmd);
 	pci_pool_destroy(cmd->pool);
 }
 EXPORT_SYMBOL(mlx5_cmd_cleanup);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cq.c b/drivers/net/ethernet/mellanox/mlx5/core/cq.c
index 43c5f4809526..eb0cf81f5f45 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
index 4878025e231c..5210d92e6bc7 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index da82991239a8..58800e4f3958 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -208,7 +208,7 @@ static int mlx5_eq_int(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 		 * Make sure we read EQ entry contents after we've
 		 * checked the ownership bit.
 		 */
-		rmb();
+		dma_rmb();
 
 		mlx5_core_dbg(eq->dev, "eqn %d, eqe type %s\n",
 			      eq->eqn, eqe_type_str(eqe->type));
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fw.c b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
index 06f9036acd83..4b4cda3bcc5f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/health.c b/drivers/net/ethernet/mellanox/mlx5/core/health.c
index 3e6670c4a7cd..292d76f2a904 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/health.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/health.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mad.c b/drivers/net/ethernet/mellanox/mlx5/core/mad.c
index fd80ecfa7195..ee1b0b965f34 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mad.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mad.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/main.c b/drivers/net/ethernet/mellanox/mlx5/core/main.c
index 5394a8486558..28425e5ea91f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -48,11 +48,11 @@
 #include "mlx5_core.h"
 
 #define DRIVER_NAME "mlx5_core"
-#define DRIVER_VERSION "2.2-1"
-#define DRIVER_RELDATE	"Feb 2014"
+#define DRIVER_VERSION "3.0"
+#define DRIVER_RELDATE  "January 2015"
 
 MODULE_AUTHOR("Eli Cohen <eli@mellanox.com>");
-MODULE_DESCRIPTION("Mellanox ConnectX-IB HCA core library");
+MODULE_DESCRIPTION("Mellanox Connect-IB, ConnectX-4 core driver");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(DRIVER_VERSION);
 
@@ -288,8 +288,6 @@ static void copy_rw_fields(void *to, struct mlx5_caps *from)
 	MLX5_SET(cmd_hca_cap, to, log_max_ra_req_qp, from->gen.log_max_ra_req_qp);
 	MLX5_SET(cmd_hca_cap, to, log_max_ra_res_qp, from->gen.log_max_ra_res_qp);
 	MLX5_SET(cmd_hca_cap, to, pkey_table_size, from->gen.pkey_table_size);
-	MLX5_SET(cmd_hca_cap, to, log_max_ra_req_dc, from->gen.log_max_ra_req_dc);
-	MLX5_SET(cmd_hca_cap, to, log_max_ra_res_dc, from->gen.log_max_ra_res_dc);
 	MLX5_SET(cmd_hca_cap, to, pkey_table_size, to_fw_pkey_sz(from->gen.pkey_table_size));
 	MLX5_SET(cmd_hca_cap, to, log_uar_page_sz, PAGE_SHIFT - 12);
 	v64 = from->gen.flags & MLX5_CAP_BITS_RW_MASK;
@@ -509,6 +507,87 @@ static int mlx5_core_disable_hca(struct mlx5_core_dev *dev)
 	return 0;
 }
 
+int mlx5_vector2eqn(struct mlx5_core_dev *dev, int vector, int *eqn, int *irqn)
+{
+	struct mlx5_eq_table *table = &dev->priv.eq_table;
+	struct mlx5_eq *eq, *n;
+	int err = -ENOENT;
+
+	spin_lock(&table->lock);
+	list_for_each_entry_safe(eq, n, &table->comp_eqs_list, list) {
+		if (eq->index == vector) {
+			*eqn = eq->eqn;
+			*irqn = eq->irqn;
+			err = 0;
+			break;
+		}
+	}
+	spin_unlock(&table->lock);
+
+	return err;
+}
+EXPORT_SYMBOL(mlx5_vector2eqn);
+
+static void free_comp_eqs(struct mlx5_core_dev *dev)
+{
+	struct mlx5_eq_table *table = &dev->priv.eq_table;
+	struct mlx5_eq *eq, *n;
+
+	spin_lock(&table->lock);
+	list_for_each_entry_safe(eq, n, &table->comp_eqs_list, list) {
+		list_del(&eq->list);
+		spin_unlock(&table->lock);
+		if (mlx5_destroy_unmap_eq(dev, eq))
+			mlx5_core_warn(dev, "failed to destroy EQ 0x%x\n",
+				       eq->eqn);
+		kfree(eq);
+		spin_lock(&table->lock);
+	}
+	spin_unlock(&table->lock);
+}
+
+static int alloc_comp_eqs(struct mlx5_core_dev *dev)
+{
+	struct mlx5_eq_table *table = &dev->priv.eq_table;
+	char name[MLX5_MAX_EQ_NAME];
+	struct mlx5_eq *eq;
+	int ncomp_vec;
+	int nent;
+	int err;
+	int i;
+
+	INIT_LIST_HEAD(&table->comp_eqs_list);
+	ncomp_vec = table->num_comp_vectors;
+	nent = MLX5_COMP_EQ_SIZE;
+	for (i = 0; i < ncomp_vec; i++) {
+		eq = kzalloc(sizeof(*eq), GFP_KERNEL);
+		if (!eq) {
+			err = -ENOMEM;
+			goto clean;
+		}
+
+		snprintf(name, MLX5_MAX_EQ_NAME, "mlx5_comp%d", i);
+		err = mlx5_create_map_eq(dev, eq,
+					 i + MLX5_EQ_VEC_COMP_BASE, nent, 0,
+					 name, &dev->priv.uuari.uars[0]);
+		if (err) {
+			kfree(eq);
+			goto clean;
+		}
+		mlx5_core_dbg(dev, "allocated completion EQN %d\n", eq->eqn);
+		eq->index = i;
+		spin_lock(&table->lock);
+		list_add_tail(&eq->list, &table->comp_eqs_list);
+		spin_unlock(&table->lock);
+	}
+
+	return 0;
+
+clean:
+	free_comp_eqs(dev);
+	return err;
+}
+
 static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 {
 	struct mlx5_priv *priv = &dev->priv;
@@ -645,6 +724,12 @@ static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 		goto err_free_uar;
 	}
 
+	err = alloc_comp_eqs(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to alloc completion EQs\n");
+		goto err_stop_eqs;
+	}
+
 	MLX5_INIT_DOORBELL_LOCK(&priv->cq_uar_lock);
 
 	mlx5_init_cq_table(dev);
@@ -654,6 +739,9 @@ static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 
 	return 0;
 
+err_stop_eqs:
+	mlx5_stop_eqs(dev);
+
 err_free_uar:
 	mlx5_free_uuars(dev, &priv->uuari);
 
@@ -697,7 +785,6 @@ err_dbg:
 	debugfs_remove(priv->dbg_root);
 	return err;
 }
-EXPORT_SYMBOL(mlx5_dev_init);
 
 static void mlx5_dev_cleanup(struct mlx5_core_dev *dev)
 {
@@ -706,6 +793,7 @@ static void mlx5_dev_cleanup(struct mlx5_core_dev *dev)
 	mlx5_cleanup_srq_table(dev);
 	mlx5_cleanup_qp_table(dev);
 	mlx5_cleanup_cq_table(dev);
+	free_comp_eqs(dev);
 	mlx5_stop_eqs(dev);
 	mlx5_free_uuars(dev, &priv->uuari);
 	mlx5_eq_cleanup(dev);
@@ -820,6 +908,28 @@ void mlx5_unregister_interface(struct mlx5_interface *intf)
 }
 EXPORT_SYMBOL(mlx5_unregister_interface);
 
+void *mlx5_get_protocol_dev(struct mlx5_core_dev *mdev, int protocol)
+{
+	struct mlx5_priv *priv = &mdev->priv;
+	struct mlx5_device_context *dev_ctx;
+	unsigned long flags;
+	void *result = NULL;
+
+	spin_lock_irqsave(&priv->ctx_lock, flags);
+
+	list_for_each_entry(dev_ctx, &mdev->priv.ctx_list, list)
+		if ((dev_ctx->intf->protocol == protocol) &&
+		    dev_ctx->intf->get_dev) {
+			result = dev_ctx->intf->get_dev(dev_ctx->context);
+			break;
+		}
+
+	spin_unlock_irqrestore(&priv->ctx_lock, flags);
+
+	return result;
+}
+EXPORT_SYMBOL(mlx5_get_protocol_dev);
+
 static void mlx5_core_event(struct mlx5_core_dev *dev, enum mlx5_dev_event event,
 			    unsigned long param)
 {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mcg.c b/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
index 44837640bd7c..d79fd85d1dd5 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
index f0c9f9a7a361..a051b906afdf 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mr.c b/drivers/net/ethernet/mellanox/mlx5/core/mr.c
index 184c3615f479..1adb300dd850 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mr.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mr.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -141,7 +141,7 @@ EXPORT_SYMBOL(mlx5_core_destroy_mkey);
 int mlx5_core_query_mkey(struct mlx5_core_dev *dev, struct mlx5_core_mr *mr,
 			 struct mlx5_query_mkey_mbox_out *out, int outlen)
 {
-	struct mlx5_destroy_mkey_mbox_in in;
+	struct mlx5_query_mkey_mbox_in in;
 	int err;
 
 	memset(&in, 0, sizeof(in));
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
index 4fdaae9b54d9..df2238372ea7 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -243,8 +243,9 @@ static int alloc_system_page(struct mlx5_core_dev *dev, u16 func_id)
 	struct page *page;
 	u64 addr;
 	int err;
+	int nid = dev_to_node(&dev->pdev->dev);
 
-	page = alloc_page(GFP_HIGHUSER);
+	page = alloc_pages_node(nid, GFP_HIGHUSER, 0);
 	if (!page) {
 		mlx5_core_warn(dev, "failed to allocate page\n");
 		return -ENOMEM;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/pd.c b/drivers/net/ethernet/mellanox/mlx5/core/pd.c
index 790da5c4ca4f..f2d3aee909e8 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/pd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/pd.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/port.c b/drivers/net/ethernet/mellanox/mlx5/core/port.c
index 72c2d002c3b8..49e90f2612d8 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/port.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/port.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/qp.c b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
index 575d853dbe05..dc7dbf7e9d98 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/srq.c b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
index 38bce93f8314..f9d25dcd03c1 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/uar.c b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
index 06801d6f595e..5a89bb1d678a 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/uar.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Mellanox Technologies inc.  All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
diff --git a/drivers/net/ethernet/micrel/ksz884x.c b/drivers/net/ethernet/micrel/ksz884x.c
index 10988fbf47eb..6f332ebdf3b5 100644
--- a/drivers/net/ethernet/micrel/ksz884x.c
+++ b/drivers/net/ethernet/micrel/ksz884x.c
@@ -4144,7 +4144,7 @@ static int hw_del_addr(struct ksz_hw *hw, u8 *mac_addr)
 
 	for (i = 0; i < hw->addr_list_size; i++) {
 		if (ether_addr_equal(hw->address[i], mac_addr)) {
-			memset(hw->address[i], 0, ETH_ALEN);
+			eth_zero_addr(hw->address[i]);
 			writel(0, hw->io + ADD_ADDR_INCR * i +
 				KS_ADD_ADDR_0_HI);
 			return 0;
diff --git a/drivers/net/ethernet/moxa/moxart_ether.c b/drivers/net/ethernet/moxa/moxart_ether.c
index 6c72e74fef3e..81d0f1c86d6d 100644
--- a/drivers/net/ethernet/moxa/moxart_ether.c
+++ b/drivers/net/ethernet/moxa/moxart_ether.c
@@ -150,7 +150,7 @@ static void moxart_mac_setup_desc_ring(struct net_device *ndev)
 
 	priv->rx_head = 0;
 
-	/* reset the MAC controler TX/RX desciptor base address */
+	/* reset the MAC controller TX/RX desciptor base address */
 	writel(priv->tx_base, priv->base + REG_TXR_BASE_ADDRESS);
 	writel(priv->rx_base, priv->base + REG_RXR_BASE_ADDRESS);
 }
diff --git a/drivers/net/ethernet/neterion/s2io.c b/drivers/net/ethernet/neterion/s2io.c
index a4cdf2f8041a..1e0f72b65459 100644
--- a/drivers/net/ethernet/neterion/s2io.c
+++ b/drivers/net/ethernet/neterion/s2io.c
@@ -1343,7 +1343,7 @@ static int init_nic(struct s2io_nic *nic)
 		TX_PA_CFG_IGNORE_L2_ERR;
 	writeq(val64, &bar0->tx_pa_cfg);
 
-	/* Rx DMA intialization. */
+	/* Rx DMA initialization. */
 	val64 = 0;
 	for (i = 0; i < config->rx_ring_num; i++) {
 		struct rx_ring_config *rx_cfg = &config->rx_cfg[i];
@@ -2520,7 +2520,7 @@ static int fill_rx_buffers(struct s2io_nic *nic, struct ring_info *ring,
 			DBG_PRINT(INFO_DBG, "%s: Could not allocate skb\n",
 				  ring->dev->name);
 			if (first_rxdp) {
-				wmb();
+				dma_wmb();
 				first_rxdp->Control_1 |= RXD_OWN_XENA;
 			}
 			swstats->mem_alloc_fail_cnt++;
@@ -2634,7 +2634,7 @@ static int fill_rx_buffers(struct s2io_nic *nic, struct ring_info *ring,
 		rxdp->Control_2 |= SET_RXD_MARKER;
 		if (!(alloc_tab & ((1 << rxsync_frequency) - 1))) {
 			if (first_rxdp) {
-				wmb();
+				dma_wmb();
 				first_rxdp->Control_1 |= RXD_OWN_XENA;
 			}
 			first_rxdp = rxdp;
@@ -2649,7 +2649,7 @@ end:
 	 * and other fields are seen by adapter correctly.
 	 */
 	if (first_rxdp) {
-		wmb();
+		dma_wmb();
 		first_rxdp->Control_1 |= RXD_OWN_XENA;
 	}
 
@@ -6950,7 +6950,7 @@ static  int rxd_owner_bit_reset(struct s2io_nic *sp)
 				}
 
 				set_rxd_buffer_size(sp, rxdp, size);
-				wmb();
+				dma_wmb();
 				/* flip the Ownership bit to Hardware */
 				rxdp->Control_1 |= RXD_OWN_XENA;
 			}
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-ethtool.c b/drivers/net/ethernet/neterion/vxge/vxge-ethtool.c
index b07d552a27d4..be916eb2f2e7 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-ethtool.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-ethtool.c
@@ -18,6 +18,25 @@
 
 #include "vxge-ethtool.h"
 
+static const char ethtool_driver_stats_keys[][ETH_GSTRING_LEN] = {
+	{"\n DRIVER STATISTICS"},
+	{"vpaths_opened"},
+	{"vpath_open_fail_cnt"},
+	{"link_up_cnt"},
+	{"link_down_cnt"},
+	{"tx_frms"},
+	{"tx_errors"},
+	{"tx_bytes"},
+	{"txd_not_free"},
+	{"txd_out_of_desc"},
+	{"rx_frms"},
+	{"rx_errors"},
+	{"rx_bytes"},
+	{"rx_mcast"},
+	{"pci_map_fail_cnt"},
+	{"skb_alloc_fail_cnt"}
+};
+
 /**
  * vxge_ethtool_sset - Sets different link parameters.
  * @dev: device pointer.
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-ethtool.h b/drivers/net/ethernet/neterion/vxge/vxge-ethtool.h
index 6cf3044d7f43..065a2c0429a4 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-ethtool.h
+++ b/drivers/net/ethernet/neterion/vxge/vxge-ethtool.h
@@ -19,25 +19,6 @@
 /* Ethtool related variables and Macros. */
 static int vxge_ethtool_get_sset_count(struct net_device *dev, int sset);
 
-static char ethtool_driver_stats_keys[][ETH_GSTRING_LEN] = {
-	{"\n DRIVER STATISTICS"},
-	{"vpaths_opened"},
-	{"vpath_open_fail_cnt"},
-	{"link_up_cnt"},
-	{"link_down_cnt"},
-	{"tx_frms"},
-	{"tx_errors"},
-	{"tx_bytes"},
-	{"txd_not_free"},
-	{"txd_out_of_desc"},
-	{"rx_frms"},
-	{"rx_errors"},
-	{"rx_bytes"},
-	{"rx_mcast"},
-	{"pci_map_fail_cnt"},
-	{"skb_alloc_fail_cnt"}
-};
-
 #define VXGE_TITLE_LEN			5
 #define VXGE_HW_VPATH_STATS_LEN 	27
 #define VXGE_HW_AGGR_STATS_LEN  	13
diff --git a/drivers/net/ethernet/octeon/octeon_mgmt.c b/drivers/net/ethernet/octeon/octeon_mgmt.c
index d36599f47af5..7bf9c028d8d7 100644
--- a/drivers/net/ethernet/octeon/octeon_mgmt.c
+++ b/drivers/net/ethernet/octeon/octeon_mgmt.c
@@ -1557,7 +1557,7 @@ static int octeon_mgmt_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id octeon_mgmt_match[] = {
+static const struct of_device_id octeon_mgmt_match[] = {
 	{
 		.compatible = "cavium,octeon-5750-mix",
 	},
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c
index 4fe8ea96bd25..f6fcf7450352 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c
@@ -394,7 +394,7 @@ static void pch_gbe_get_pauseparam(struct net_device *netdev,
 }
 
 /**
- * pch_gbe_set_pauseparam - Set pause paramters
+ * pch_gbe_set_pauseparam - Set pause parameters
  * @netdev:  Network interface device structure
  * @pause:   Pause parameters structure
  * Returns:
diff --git a/drivers/net/ethernet/packetengines/hamachi.c b/drivers/net/ethernet/packetengines/hamachi.c
index 319d9d40f922..13d88a6025c8 100644
--- a/drivers/net/ethernet/packetengines/hamachi.c
+++ b/drivers/net/ethernet/packetengines/hamachi.c
@@ -350,7 +350,7 @@ V.  Recent Changes
     incorrectly defined and corrected (as per Michel Mueller).
 
 02/23/1999 EPK Corrected the Tx full check to check that at least 4 slots
-    were available before reseting the tbusy and tx_full flags
+    were available before resetting the tbusy and tx_full flags
     (as per Michel Mueller).
 
 03/11/1999 EPK Added Pete Wyckoff's hardware checksumming support.
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
index 716fc37ada5a..db80eb1c6d4f 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
@@ -537,7 +537,7 @@ static void netxen_p2_nic_set_multi(struct net_device *netdev)
 	u8 null_addr[ETH_ALEN];
 	int i;
 
-	memset(null_addr, 0, ETH_ALEN);
+	eth_zero_addr(null_addr);
 
 	if (netdev->flags & IFF_PROMISC) {
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
index f3346a3779d3..69f828eb42cf 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
@@ -205,7 +205,7 @@ struct qlcnic_add_rings_mbx_out {
  * @phys_addr_{low|high}: DMA address of the transmit buffer
  * @cnsmr_index_{low|high}: host consumer index
  * @size: legth of transmit buffer ring
- * @intr_id: interrput id
+ * @intr_id: interrupt id
  * @src: src of interrupt
  */
 struct qlcnic_tx_mbx {
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
index 2bb48d57e7a5..33669c29b341 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
@@ -269,7 +269,7 @@ static int qlcnic_83xx_idc_clear_registers(struct qlcnic_adapter *adapter,
 	}
 
 	QLCWRX(adapter->ahw, QLC_83XX_IDC_DRV_ACK, 0);
-	/* Clear gracefull reset bit */
+	/* Clear graceful reset bit */
 	val = QLCRDX(adapter->ahw, QLC_83XX_IDC_CTRL);
 	val &= ~QLC_83XX_IDC_GRACEFULL_RESET;
 	QLCWRX(adapter->ahw, QLC_83XX_IDC_CTRL, val);
@@ -889,7 +889,7 @@ static int qlcnic_83xx_idc_ready_state(struct qlcnic_adapter *adapter)
  * @adapter: adapter structure
  *
  * Device will remain in this state until:
- *	Reset request ACK's are recieved from all the functions
+ *	Reset request ACK's are received from all the functions
  *	Wait time exceeds max time limit
  *
  * Returns: Error code or Success(0)
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
index 3e0f705a4311..75ee9e4ced51 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
@@ -818,7 +818,7 @@ int qlcnic_82xx_config_intr_coalesce(struct qlcnic_adapter *adapter,
 
 	if (rv)
 		netdev_err(adapter->netdev,
-			   "Failed to set Rx coalescing parametrs\n");
+			   "Failed to set Rx coalescing parameters\n");
 
 	return rv;
 }
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index a430a34a4434..367f3976df56 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -507,6 +507,7 @@ static netdev_features_t qlcnic_features_check(struct sk_buff *skb,
 					       struct net_device *dev,
 					       netdev_features_t features)
 {
+	features = vlan_features_check(skb, features);
 	return vxlan_features_check(skb, features);
 }
 #endif
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index 8011ef3e7707..25800a1dedcb 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -460,7 +460,7 @@ static int ql_set_mac_addr(struct ql_adapter *qdev, int set)
 		netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
 			     "Set Mac addr %pM\n", addr);
 	} else {
-		memset(zero_mac_addr, 0, ETH_ALEN);
+		eth_zero_addr(zero_mac_addr);
 		addr = &zero_mac_addr[0];
 		netif_printk(qdev, ifup, KERN_DEBUG, qdev->ndev,
 			     "Clearing MAC address\n");
diff --git a/drivers/net/ethernet/qualcomm/qca_spi.c b/drivers/net/ethernet/qualcomm/qca_spi.c
index 2c811f66d5ac..4a42e960d331 100644
--- a/drivers/net/ethernet/qualcomm/qca_spi.c
+++ b/drivers/net/ethernet/qualcomm/qca_spi.c
@@ -571,7 +571,7 @@ qcaspi_spi_thread(void *data)
 			}
 
 			/* can only handle other interrupts
-			 * if sync has occured
+			 * if sync has occurred
 			 */
 			if (qca->sync == QCASPI_SYNC_READY) {
 				if (intr_cause & SPI_INT_PKT_AVLBL)
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 736d5d1624a1..7fb244f565b2 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -52,7 +52,12 @@
 		NETIF_MSG_RX_ERR| \
 		NETIF_MSG_TX_ERR)
 
+#define SH_ETH_OFFSET_DEFAULTS			\
+	[0 ... SH_ETH_MAX_REGISTER_OFFSET - 1] = SH_ETH_OFFSET_INVALID
+
 static const u16 sh_eth_offset_gigabit[SH_ETH_MAX_REGISTER_OFFSET] = {
+	SH_ETH_OFFSET_DEFAULTS,
+
 	[EDSR]		= 0x0000,
 	[EDMR]		= 0x0400,
 	[EDTRR]		= 0x0408,
@@ -132,9 +137,6 @@ static const u16 sh_eth_offset_gigabit[SH_ETH_MAX_REGISTER_OFFSET] = {
 	[TSU_POST3]	= 0x0078,
 	[TSU_POST4]	= 0x007c,
 	[TSU_ADRH0]	= 0x0100,
-	[TSU_ADRL0]	= 0x0104,
-	[TSU_ADRH31]	= 0x01f8,
-	[TSU_ADRL31]	= 0x01fc,
 
 	[TXNLCR0]	= 0x0080,
 	[TXALCR0]	= 0x0084,
@@ -151,6 +153,8 @@ static const u16 sh_eth_offset_gigabit[SH_ETH_MAX_REGISTER_OFFSET] = {
 };
 
 static const u16 sh_eth_offset_fast_rz[SH_ETH_MAX_REGISTER_OFFSET] = {
+	SH_ETH_OFFSET_DEFAULTS,
+
 	[EDSR]		= 0x0000,
 	[EDMR]		= 0x0400,
 	[EDTRR]		= 0x0408,
@@ -199,9 +203,6 @@ static const u16 sh_eth_offset_fast_rz[SH_ETH_MAX_REGISTER_OFFSET] = {
 	[TSU_ADSBSY]	= 0x0060,
 	[TSU_TEN]	= 0x0064,
 	[TSU_ADRH0]	= 0x0100,
-	[TSU_ADRL0]	= 0x0104,
-	[TSU_ADRH31]	= 0x01f8,
-	[TSU_ADRL31]	= 0x01fc,
 
 	[TXNLCR0]	= 0x0080,
 	[TXALCR0]	= 0x0084,
@@ -210,6 +211,8 @@ static const u16 sh_eth_offset_fast_rz[SH_ETH_MAX_REGISTER_OFFSET] = {
 };
 
 static const u16 sh_eth_offset_fast_rcar[SH_ETH_MAX_REGISTER_OFFSET] = {
+	SH_ETH_OFFSET_DEFAULTS,
+
 	[ECMR]		= 0x0300,
 	[RFLR]		= 0x0308,
 	[ECSR]		= 0x0310,
@@ -256,6 +259,8 @@ static const u16 sh_eth_offset_fast_rcar[SH_ETH_MAX_REGISTER_OFFSET] = {
 };
 
 static const u16 sh_eth_offset_fast_sh4[SH_ETH_MAX_REGISTER_OFFSET] = {
+	SH_ETH_OFFSET_DEFAULTS,
+
 	[ECMR]		= 0x0100,
 	[RFLR]		= 0x0108,
 	[ECSR]		= 0x0110,
@@ -308,6 +313,8 @@ static const u16 sh_eth_offset_fast_sh4[SH_ETH_MAX_REGISTER_OFFSET] = {
 };
 
 static const u16 sh_eth_offset_fast_sh3_sh2[SH_ETH_MAX_REGISTER_OFFSET] = {
+	SH_ETH_OFFSET_DEFAULTS,
+
 	[EDMR]		= 0x0000,
 	[EDTRR]		= 0x0004,
 	[EDRRR]		= 0x0008,
@@ -392,8 +399,6 @@ static const u16 sh_eth_offset_fast_sh3_sh2[SH_ETH_MAX_REGISTER_OFFSET] = {
 	[FWALCR1]	= 0x00b4,
 
 	[TSU_ADRH0]	= 0x0100,
-	[TSU_ADRL0]	= 0x0104,
-	[TSU_ADRL31]	= 0x01fc,
 };
 
 static void sh_eth_rcv_snd_disable(struct net_device *ndev);
@@ -588,6 +593,7 @@ static struct sh_eth_cpu_data sh7757_data = {
 	.no_ade		= 1,
 	.rpadir		= 1,
 	.rpadir_value   = 2 << 16,
+	.rtrate		= 1,
 };
 
 #define SH_GIGA_ETH_BASE	0xfee00000UL
@@ -1411,6 +1417,9 @@ static int sh_eth_txfree(struct net_device *ndev)
 			break;
 		/* TACT bit must be checked before all the following reads */
 		rmb();
+		netif_info(mdp, tx_done, ndev,
+			   "tx entry %d status 0x%08x\n",
+			   entry, edmac_to_cpu(mdp, txdesc->status));
 		/* Free the original skb. */
 		if (mdp->tx_skbuff[entry]) {
 			dma_unmap_single(&ndev->dev, txdesc->addr,
@@ -1456,6 +1465,10 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 		if (--boguscnt < 0)
 			break;
 
+		netif_info(mdp, rx_status, ndev,
+			   "rx entry %d status 0x%08x len %d\n",
+			   entry, desc_status, pkt_len);
+
 		if (!(desc_status & RDFEND))
 			ndev->stats.rx_length_errors++;
 
@@ -1500,6 +1513,8 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 			netif_receive_skb(skb);
 			ndev->stats.rx_packets++;
 			ndev->stats.rx_bytes += pkt_len;
+			if (desc_status & RD_RFS8)
+				ndev->stats.multicast++;
 		}
 		entry = (++mdp->cur_rx) % mdp->num_rx_ring;
 		rxdesc = &mdp->rx_ring[entry];
@@ -1542,7 +1557,8 @@ 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 && mdp->reg_offset[RDFAR] != 0) {
+		if (intr_status & EESR_RDE &&
+		    mdp->reg_offset[RDFAR] != SH_ETH_OFFSET_INVALID) {
 			u32 count = (sh_eth_read(ndev, RDFAR) -
 				     sh_eth_read(ndev, RDLAR)) >> 4;
 
@@ -1929,6 +1945,192 @@ error_exit:
 	return ret;
 }
 
+/* If it is ever necessary to increase SH_ETH_REG_DUMP_MAX_REGS, the
+ * version must be bumped as well.  Just adding registers up to that
+ * limit is fine, as long as the existing register indices don't
+ * change.
+ */
+#define SH_ETH_REG_DUMP_VERSION		1
+#define SH_ETH_REG_DUMP_MAX_REGS	256
+
+static size_t __sh_eth_get_regs(struct net_device *ndev, u32 *buf)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	struct sh_eth_cpu_data *cd = mdp->cd;
+	u32 *valid_map;
+	size_t len;
+
+	BUILD_BUG_ON(SH_ETH_MAX_REGISTER_OFFSET > SH_ETH_REG_DUMP_MAX_REGS);
+
+	/* Dump starts with a bitmap that tells ethtool which
+	 * registers are defined for this chip.
+	 */
+	len = DIV_ROUND_UP(SH_ETH_REG_DUMP_MAX_REGS, 32);
+	if (buf) {
+		valid_map = buf;
+		buf += len;
+	} else {
+		valid_map = NULL;
+	}
+
+	/* Add a register to the dump, if it has a defined offset.
+	 * This automatically skips most undefined registers, but for
+	 * some it is also necessary to check a capability flag in
+	 * struct sh_eth_cpu_data.
+	 */
+#define mark_reg_valid(reg) valid_map[reg / 32] |= 1U << (reg % 32)
+#define add_reg_from(reg, read_expr) do {				\
+		if (mdp->reg_offset[reg] != SH_ETH_OFFSET_INVALID) {	\
+			if (buf) {					\
+				mark_reg_valid(reg);			\
+				*buf++ = read_expr;			\
+			}						\
+			++len;						\
+		}							\
+	} while (0)
+#define add_reg(reg) add_reg_from(reg, sh_eth_read(ndev, reg))
+#define add_tsu_reg(reg) add_reg_from(reg, sh_eth_tsu_read(mdp, reg))
+
+	add_reg(EDSR);
+	add_reg(EDMR);
+	add_reg(EDTRR);
+	add_reg(EDRRR);
+	add_reg(EESR);
+	add_reg(EESIPR);
+	add_reg(TDLAR);
+	add_reg(TDFAR);
+	add_reg(TDFXR);
+	add_reg(TDFFR);
+	add_reg(RDLAR);
+	add_reg(RDFAR);
+	add_reg(RDFXR);
+	add_reg(RDFFR);
+	add_reg(TRSCER);
+	add_reg(RMFCR);
+	add_reg(TFTR);
+	add_reg(FDR);
+	add_reg(RMCR);
+	add_reg(TFUCR);
+	add_reg(RFOCR);
+	if (cd->rmiimode)
+		add_reg(RMIIMODE);
+	add_reg(FCFTR);
+	if (cd->rpadir)
+		add_reg(RPADIR);
+	if (!cd->no_trimd)
+		add_reg(TRIMD);
+	add_reg(ECMR);
+	add_reg(ECSR);
+	add_reg(ECSIPR);
+	add_reg(PIR);
+	if (!cd->no_psr)
+		add_reg(PSR);
+	add_reg(RDMLR);
+	add_reg(RFLR);
+	add_reg(IPGR);
+	if (cd->apr)
+		add_reg(APR);
+	if (cd->mpr)
+		add_reg(MPR);
+	add_reg(RFCR);
+	add_reg(RFCF);
+	if (cd->tpauser)
+		add_reg(TPAUSER);
+	add_reg(TPAUSECR);
+	add_reg(GECMR);
+	if (cd->bculr)
+		add_reg(BCULR);
+	add_reg(MAHR);
+	add_reg(MALR);
+	add_reg(TROCR);
+	add_reg(CDCR);
+	add_reg(LCCR);
+	add_reg(CNDCR);
+	add_reg(CEFCR);
+	add_reg(FRECR);
+	add_reg(TSFRCR);
+	add_reg(TLFRCR);
+	add_reg(CERCR);
+	add_reg(CEECR);
+	add_reg(MAFCR);
+	if (cd->rtrate)
+		add_reg(RTRATE);
+	if (cd->hw_crc)
+		add_reg(CSMR);
+	if (cd->select_mii)
+		add_reg(RMII_MII);
+	add_reg(ARSTR);
+	if (cd->tsu) {
+		add_tsu_reg(TSU_CTRST);
+		add_tsu_reg(TSU_FWEN0);
+		add_tsu_reg(TSU_FWEN1);
+		add_tsu_reg(TSU_FCM);
+		add_tsu_reg(TSU_BSYSL0);
+		add_tsu_reg(TSU_BSYSL1);
+		add_tsu_reg(TSU_PRISL0);
+		add_tsu_reg(TSU_PRISL1);
+		add_tsu_reg(TSU_FWSL0);
+		add_tsu_reg(TSU_FWSL1);
+		add_tsu_reg(TSU_FWSLC);
+		add_tsu_reg(TSU_QTAG0);
+		add_tsu_reg(TSU_QTAG1);
+		add_tsu_reg(TSU_QTAGM0);
+		add_tsu_reg(TSU_QTAGM1);
+		add_tsu_reg(TSU_FWSR);
+		add_tsu_reg(TSU_FWINMK);
+		add_tsu_reg(TSU_ADQT0);
+		add_tsu_reg(TSU_ADQT1);
+		add_tsu_reg(TSU_VTAG0);
+		add_tsu_reg(TSU_VTAG1);
+		add_tsu_reg(TSU_ADSBSY);
+		add_tsu_reg(TSU_TEN);
+		add_tsu_reg(TSU_POST1);
+		add_tsu_reg(TSU_POST2);
+		add_tsu_reg(TSU_POST3);
+		add_tsu_reg(TSU_POST4);
+		if (mdp->reg_offset[TSU_ADRH0] != SH_ETH_OFFSET_INVALID) {
+			/* This is the start of a table, not just a single
+			 * register.
+			 */
+			if (buf) {
+				unsigned int i;
+
+				mark_reg_valid(TSU_ADRH0);
+				for (i = 0; i < SH_ETH_TSU_CAM_ENTRIES * 2; i++)
+					*buf++ = ioread32(
+						mdp->tsu_addr +
+						mdp->reg_offset[TSU_ADRH0] +
+						i * 4);
+			}
+			len += SH_ETH_TSU_CAM_ENTRIES * 2;
+		}
+	}
+
+#undef mark_reg_valid
+#undef add_reg_from
+#undef add_reg
+#undef add_tsu_reg
+
+	return len * 4;
+}
+
+static int sh_eth_get_regs_len(struct net_device *ndev)
+{
+	return __sh_eth_get_regs(ndev, NULL);
+}
+
+static void sh_eth_get_regs(struct net_device *ndev, struct ethtool_regs *regs,
+			    void *buf)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	regs->version = SH_ETH_REG_DUMP_VERSION;
+
+	pm_runtime_get_sync(&mdp->pdev->dev);
+	__sh_eth_get_regs(ndev, buf);
+	pm_runtime_put_sync(&mdp->pdev->dev);
+}
+
 static int sh_eth_nway_reset(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
@@ -2074,6 +2276,8 @@ static int sh_eth_set_ringparam(struct net_device *ndev,
 static const struct ethtool_ops sh_eth_ethtool_ops = {
 	.get_settings	= sh_eth_get_settings,
 	.set_settings	= sh_eth_set_settings,
+	.get_regs_len	= sh_eth_get_regs_len,
+	.get_regs	= sh_eth_get_regs,
 	.nway_reset	= sh_eth_nway_reset,
 	.get_msglevel	= sh_eth_get_msglevel,
 	.set_msglevel	= sh_eth_set_msglevel,
@@ -2213,6 +2417,22 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	return NETDEV_TX_OK;
 }
 
+/* The statistics registers have write-clear behaviour, which means we
+ * will lose any increment between the read and write.  We mitigate
+ * this by only clearing when we read a non-zero value, so we will
+ * never falsely report a total of zero.
+ */
+static void
+sh_eth_update_stat(struct net_device *ndev, unsigned long *stat, int reg)
+{
+	u32 delta = sh_eth_read(ndev, reg);
+
+	if (delta) {
+		*stat += delta;
+		sh_eth_write(ndev, 0, reg);
+	}
+}
+
 static struct net_device_stats *sh_eth_get_stats(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
@@ -2223,21 +2443,18 @@ static struct net_device_stats *sh_eth_get_stats(struct net_device *ndev)
 	if (!mdp->is_opened)
 		return &ndev->stats;
 
-	ndev->stats.tx_dropped += sh_eth_read(ndev, TROCR);
-	sh_eth_write(ndev, 0, TROCR);	/* (write clear) */
-	ndev->stats.collisions += sh_eth_read(ndev, CDCR);
-	sh_eth_write(ndev, 0, CDCR);	/* (write clear) */
-	ndev->stats.tx_carrier_errors += sh_eth_read(ndev, LCCR);
-	sh_eth_write(ndev, 0, LCCR);	/* (write clear) */
+	sh_eth_update_stat(ndev, &ndev->stats.tx_dropped, TROCR);
+	sh_eth_update_stat(ndev, &ndev->stats.collisions, CDCR);
+	sh_eth_update_stat(ndev, &ndev->stats.tx_carrier_errors, LCCR);
 
 	if (sh_eth_is_gether(mdp)) {
-		ndev->stats.tx_carrier_errors += sh_eth_read(ndev, CERCR);
-		sh_eth_write(ndev, 0, CERCR);	/* (write clear) */
-		ndev->stats.tx_carrier_errors += sh_eth_read(ndev, CEECR);
-		sh_eth_write(ndev, 0, CEECR);	/* (write clear) */
+		sh_eth_update_stat(ndev, &ndev->stats.tx_carrier_errors,
+				   CERCR);
+		sh_eth_update_stat(ndev, &ndev->stats.tx_carrier_errors,
+				   CEECR);
 	} else {
-		ndev->stats.tx_carrier_errors += sh_eth_read(ndev, CNDCR);
-		sh_eth_write(ndev, 0, CNDCR);	/* (write clear) */
+		sh_eth_update_stat(ndev, &ndev->stats.tx_carrier_errors,
+				   CNDCR);
 	}
 
 	return &ndev->stats;
diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index 259d03f353e1..06dbbe5201cb 100644
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -32,6 +32,10 @@
 #define SH_ETH_TSU_CAM_ENTRIES	32
 
 enum {
+	/* IMPORTANT: To keep ethtool register dump working, add new
+	 * register names immediately before SH_ETH_MAX_REGISTER_OFFSET.
+	 */
+
 	/* E-DMAC registers */
 	EDSR = 0,
 	EDMR,
@@ -131,9 +135,7 @@ enum {
 	TSU_POST3,
 	TSU_POST4,
 	TSU_ADRH0,
-	TSU_ADRL0,
-	TSU_ADRH31,
-	TSU_ADRL31,
+	/* TSU_ADR{H,L}{0..31} are assumed to be contiguous */
 
 	TXNLCR0,
 	TXALCR0,
@@ -491,6 +493,7 @@ struct sh_eth_cpu_data {
 	unsigned select_mii:1;	/* EtherC have RMII_MII (MII select register) */
 	unsigned shift_rd0:1;	/* shift Rx descriptor word 0 right by 16 */
 	unsigned rmiimode:1;	/* EtherC has RMIIMODE register */
+	unsigned rtrate:1;	/* EtherC has RTRATE register */
 };
 
 struct sh_eth_private {
@@ -543,19 +546,29 @@ static inline void sh_eth_soft_swap(char *src, int len)
 #endif
 }
 
+#define SH_ETH_OFFSET_INVALID	((u16) ~0)
+
 static inline void sh_eth_write(struct net_device *ndev, u32 data,
 				int enum_index)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
+	u16 offset = mdp->reg_offset[enum_index];
 
-	iowrite32(data, mdp->addr + mdp->reg_offset[enum_index]);
+	if (WARN_ON(offset == SH_ETH_OFFSET_INVALID))
+		return;
+
+	iowrite32(data, mdp->addr + offset);
 }
 
 static inline u32 sh_eth_read(struct net_device *ndev, int enum_index)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
+	u16 offset = mdp->reg_offset[enum_index];
+
+	if (WARN_ON(offset == SH_ETH_OFFSET_INVALID))
+		return ~0U;
 
-	return ioread32(mdp->addr + mdp->reg_offset[enum_index]);
+	return ioread32(mdp->addr + offset);
 }
 
 static inline void *sh_eth_tsu_get_offset(struct sh_eth_private *mdp,
diff --git a/drivers/net/ethernet/rocker/rocker.c b/drivers/net/ethernet/rocker/rocker.c
index 9fb6948e14c6..a87b177bd723 100644
--- a/drivers/net/ethernet/rocker/rocker.c
+++ b/drivers/net/ethernet/rocker/rocker.c
@@ -30,8 +30,12 @@
 #include <linux/if_vlan.h>
 #include <linux/if_bridge.h>
 #include <linux/bitops.h>
+#include <linux/ctype.h>
 #include <net/switchdev.h>
 #include <net/rtnetlink.h>
+#include <net/ip_fib.h>
+#include <net/netevent.h>
+#include <net/arp.h>
 #include <asm-generic/io-64-nonatomic-lo-hi.h>
 #include <generated/utsrelease.h>
 
@@ -49,12 +53,12 @@ struct rocker_flow_tbl_key {
 	enum rocker_of_dpa_table_id tbl_id;
 	union {
 		struct {
-			u32 in_lport;
-			u32 in_lport_mask;
+			u32 in_pport;
+			u32 in_pport_mask;
 			enum rocker_of_dpa_table_id goto_tbl;
 		} ig_port;
 		struct {
-			u32 in_lport;
+			u32 in_pport;
 			__be16 vlan_id;
 			__be16 vlan_id_mask;
 			enum rocker_of_dpa_table_id goto_tbl;
@@ -62,8 +66,8 @@ struct rocker_flow_tbl_key {
 			__be16 new_vlan_id;
 		} vlan;
 		struct {
-			u32 in_lport;
-			u32 in_lport_mask;
+			u32 in_pport;
+			u32 in_pport_mask;
 			__be16 eth_type;
 			u8 eth_dst[ETH_ALEN];
 			u8 eth_dst_mask[ETH_ALEN];
@@ -91,8 +95,8 @@ struct rocker_flow_tbl_key {
 			bool copy_to_cpu;
 		} bridge;
 		struct {
-			u32 in_lport;
-			u32 in_lport_mask;
+			u32 in_pport;
+			u32 in_pport_mask;
 			u8 eth_src[ETH_ALEN];
 			u8 eth_src_mask[ETH_ALEN];
 			u8 eth_dst[ETH_ALEN];
@@ -111,9 +115,10 @@ struct rocker_flow_tbl_key {
 
 struct rocker_flow_tbl_entry {
 	struct hlist_node entry;
-	u32 ref_count;
+	u32 cmd;
 	u64 cookie;
 	struct rocker_flow_tbl_key key;
+	size_t key_len;
 	u32 key_crc32; /* key */
 };
 
@@ -148,7 +153,7 @@ struct rocker_fdb_tbl_entry {
 	u32 key_crc32; /* key */
 	bool learned;
 	struct rocker_fdb_tbl_key {
-		u32 lport;
+		u32 pport;
 		u8 addr[ETH_ALEN];
 		__be16 vlan_id;
 	} key;
@@ -161,6 +166,16 @@ struct rocker_internal_vlan_tbl_entry {
 	__be16 vlan_id;
 };
 
+struct rocker_neigh_tbl_entry {
+	struct hlist_node entry;
+	__be32 ip_addr; /* key */
+	struct net_device *dev;
+	u32 ref_count;
+	u32 index;
+	u8 eth_dst[ETH_ALEN];
+	bool ttl_check;
+};
+
 struct rocker_desc_info {
 	char *data; /* mapped */
 	size_t data_size;
@@ -200,7 +215,7 @@ struct rocker_port {
 	struct net_device *bridge_dev;
 	struct rocker *rocker;
 	unsigned int port_number;
-	u32 lport;
+	u32 pport;
 	__be16 internal_vlan_id;
 	int stp_state;
 	u32 brport_flags;
@@ -234,6 +249,9 @@ struct rocker {
 	unsigned long internal_vlan_bitmap[ROCKER_INTERNAL_VLAN_BITMAP_LEN];
 	DECLARE_HASHTABLE(internal_vlan_tbl, 8);
 	spinlock_t internal_vlan_tbl_lock;
+	DECLARE_HASHTABLE(neigh_tbl, 16);
+	spinlock_t neigh_tbl_lock;
+	u32 neigh_tbl_next_index;
 };
 
 static const u8 zero_mac[ETH_ALEN]   = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
@@ -256,7 +274,6 @@ enum {
 	ROCKER_PRIORITY_VLAN = 1,
 	ROCKER_PRIORITY_TERM_MAC_UCAST = 0,
 	ROCKER_PRIORITY_TERM_MAC_MCAST = 1,
-	ROCKER_PRIORITY_UNICAST_ROUTING = 1,
 	ROCKER_PRIORITY_BRIDGING_VLAN_DFLT_EXACT = 1,
 	ROCKER_PRIORITY_BRIDGING_VLAN_DFLT_WILD = 2,
 	ROCKER_PRIORITY_BRIDGING_VLAN = 3,
@@ -789,7 +806,30 @@ static u32 __pos_inc(u32 pos, size_t limit)
 
 static int rocker_desc_err(struct rocker_desc_info *desc_info)
 {
-	return -(desc_info->desc->comp_err & ~ROCKER_DMA_DESC_COMP_ERR_GEN);
+	int err = desc_info->desc->comp_err & ~ROCKER_DMA_DESC_COMP_ERR_GEN;
+
+	switch (err) {
+	case ROCKER_OK:
+		return 0;
+	case -ROCKER_ENOENT:
+		return -ENOENT;
+	case -ROCKER_ENXIO:
+		return -ENXIO;
+	case -ROCKER_ENOMEM:
+		return -ENOMEM;
+	case -ROCKER_EEXIST:
+		return -EEXIST;
+	case -ROCKER_EINVAL:
+		return -EINVAL;
+	case -ROCKER_EMSGSIZE:
+		return -EMSGSIZE;
+	case -ROCKER_ENOTSUP:
+		return -EOPNOTSUPP;
+	case -ROCKER_ENOBUFS:
+		return -ENOBUFS;
+	}
+
+	return -EINVAL;
 }
 
 static void rocker_desc_gen_clear(struct rocker_desc_info *desc_info)
@@ -1257,9 +1297,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 |= 1ULL << rocker_port->lport;
+		val |= 1ULL << rocker_port->pport;
 	else
-		val &= ~(1ULL << rocker_port->lport);
+		val &= ~(1ULL << rocker_port->pport);
 	rocker_write64(rocker_port->rocker, PORT_PHYS_ENABLE, val);
 }
 
@@ -1312,11 +1352,11 @@ static int rocker_event_link_change(struct rocker *rocker,
 	struct rocker_port *rocker_port;
 
 	rocker_tlv_parse_nested(attrs, ROCKER_TLV_EVENT_LINK_CHANGED_MAX, info);
-	if (!attrs[ROCKER_TLV_EVENT_LINK_CHANGED_LPORT] ||
+	if (!attrs[ROCKER_TLV_EVENT_LINK_CHANGED_PPORT] ||
 	    !attrs[ROCKER_TLV_EVENT_LINK_CHANGED_LINKUP])
 		return -EIO;
 	port_number =
-		rocker_tlv_get_u32(attrs[ROCKER_TLV_EVENT_LINK_CHANGED_LPORT]) - 1;
+		rocker_tlv_get_u32(attrs[ROCKER_TLV_EVENT_LINK_CHANGED_PPORT]) - 1;
 	link_up = rocker_tlv_get_u8(attrs[ROCKER_TLV_EVENT_LINK_CHANGED_LINKUP]);
 
 	if (port_number >= rocker->port_count)
@@ -1353,12 +1393,12 @@ static int rocker_event_mac_vlan_seen(struct rocker *rocker,
 	__be16 vlan_id;
 
 	rocker_tlv_parse_nested(attrs, ROCKER_TLV_EVENT_MAC_VLAN_MAX, info);
-	if (!attrs[ROCKER_TLV_EVENT_MAC_VLAN_LPORT] ||
+	if (!attrs[ROCKER_TLV_EVENT_MAC_VLAN_PPORT] ||
 	    !attrs[ROCKER_TLV_EVENT_MAC_VLAN_MAC] ||
 	    !attrs[ROCKER_TLV_EVENT_MAC_VLAN_VLAN_ID])
 		return -EIO;
 	port_number =
-		rocker_tlv_get_u32(attrs[ROCKER_TLV_EVENT_MAC_VLAN_LPORT]) - 1;
+		rocker_tlv_get_u32(attrs[ROCKER_TLV_EVENT_MAC_VLAN_PPORT]) - 1;
 	addr = rocker_tlv_data(attrs[ROCKER_TLV_EVENT_MAC_VLAN_MAC]);
 	vlan_id = rocker_tlv_get_be16(attrs[ROCKER_TLV_EVENT_MAC_VLAN_VLAN_ID]);
 
@@ -1517,8 +1557,8 @@ rocker_cmd_get_port_settings_prep(struct rocker *rocker,
 	cmd_info = rocker_tlv_nest_start(desc_info, ROCKER_TLV_CMD_INFO);
 	if (!cmd_info)
 		return -EMSGSIZE;
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_LPORT,
-			       rocker_port->lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_PPORT,
+			       rocker_port->pport))
 		return -EMSGSIZE;
 	rocker_tlv_nest_end(desc_info, cmd_info);
 	return 0;
@@ -1591,6 +1631,53 @@ rocker_cmd_get_port_settings_macaddr_proc(struct rocker *rocker,
 	return 0;
 }
 
+struct port_name {
+	char *buf;
+	size_t len;
+};
+
+static int
+rocker_cmd_get_port_settings_phys_name_proc(struct rocker *rocker,
+					    struct rocker_port *rocker_port,
+					    struct rocker_desc_info *desc_info,
+					    void *priv)
+{
+	struct rocker_tlv *info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_MAX + 1];
+	struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
+	struct port_name *name = priv;
+	struct rocker_tlv *attr;
+	size_t i, j, len;
+	char *str;
+
+	rocker_tlv_parse_desc(attrs, ROCKER_TLV_CMD_MAX, desc_info);
+	if (!attrs[ROCKER_TLV_CMD_INFO])
+		return -EIO;
+
+	rocker_tlv_parse_nested(info_attrs, ROCKER_TLV_CMD_PORT_SETTINGS_MAX,
+				attrs[ROCKER_TLV_CMD_INFO]);
+	attr = info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_PHYS_NAME];
+	if (!attr)
+		return -EIO;
+
+	len = min_t(size_t, rocker_tlv_len(attr), name->len);
+	str = rocker_tlv_data(attr);
+
+	/* make sure name only contains alphanumeric characters */
+	for (i = j = 0; i < len; ++i) {
+		if (isalnum(str[i])) {
+			name->buf[j] = str[i];
+			j++;
+		}
+	}
+
+	if (j == 0)
+		return -EIO;
+
+	name->buf[j] = '\0';
+
+	return 0;
+}
+
 static int
 rocker_cmd_set_port_settings_ethtool_prep(struct rocker *rocker,
 					  struct rocker_port *rocker_port,
@@ -1606,8 +1693,8 @@ rocker_cmd_set_port_settings_ethtool_prep(struct rocker *rocker,
 	cmd_info = rocker_tlv_nest_start(desc_info, ROCKER_TLV_CMD_INFO);
 	if (!cmd_info)
 		return -EMSGSIZE;
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_LPORT,
-			       rocker_port->lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_PPORT,
+			       rocker_port->pport))
 		return -EMSGSIZE;
 	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_SPEED,
 			       ethtool_cmd_speed(ecmd)))
@@ -1637,8 +1724,8 @@ rocker_cmd_set_port_settings_macaddr_prep(struct rocker *rocker,
 	cmd_info = rocker_tlv_nest_start(desc_info, ROCKER_TLV_CMD_INFO);
 	if (!cmd_info)
 		return -EMSGSIZE;
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_LPORT,
-			       rocker_port->lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_PPORT,
+			       rocker_port->pport))
 		return -EMSGSIZE;
 	if (rocker_tlv_put(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_MACADDR,
 			   ETH_ALEN, macaddr))
@@ -1661,8 +1748,8 @@ rocker_cmd_set_port_learning_prep(struct rocker *rocker,
 	cmd_info = rocker_tlv_nest_start(desc_info, ROCKER_TLV_CMD_INFO);
 	if (!cmd_info)
 		return -EMSGSIZE;
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_LPORT,
-			       rocker_port->lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_PPORT,
+			       rocker_port->pport))
 		return -EMSGSIZE;
 	if (rocker_tlv_put_u8(desc_info, ROCKER_TLV_CMD_PORT_SETTINGS_LEARNING,
 			      !!(rocker_port->brport_flags & BR_LEARNING)))
@@ -1715,11 +1802,11 @@ static int rocker_port_set_learning(struct rocker_port *rocker_port)
 static int rocker_cmd_flow_tbl_add_ig_port(struct rocker_desc_info *desc_info,
 					   struct rocker_flow_tbl_entry *entry)
 {
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_LPORT,
-			       entry->key.ig_port.in_lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
+			       entry->key.ig_port.in_pport))
 		return -EMSGSIZE;
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_LPORT_MASK,
-			       entry->key.ig_port.in_lport_mask))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT_MASK,
+			       entry->key.ig_port.in_pport_mask))
 		return -EMSGSIZE;
 	if (rocker_tlv_put_u16(desc_info, ROCKER_TLV_OF_DPA_GOTO_TABLE_ID,
 			       entry->key.ig_port.goto_tbl))
@@ -1731,8 +1818,8 @@ static int rocker_cmd_flow_tbl_add_ig_port(struct rocker_desc_info *desc_info,
 static int rocker_cmd_flow_tbl_add_vlan(struct rocker_desc_info *desc_info,
 					struct rocker_flow_tbl_entry *entry)
 {
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_LPORT,
-			       entry->key.vlan.in_lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
+			       entry->key.vlan.in_pport))
 		return -EMSGSIZE;
 	if (rocker_tlv_put_be16(desc_info, ROCKER_TLV_OF_DPA_VLAN_ID,
 				entry->key.vlan.vlan_id))
@@ -1754,11 +1841,11 @@ static int rocker_cmd_flow_tbl_add_vlan(struct rocker_desc_info *desc_info,
 static int rocker_cmd_flow_tbl_add_term_mac(struct rocker_desc_info *desc_info,
 					    struct rocker_flow_tbl_entry *entry)
 {
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_LPORT,
-			       entry->key.term_mac.in_lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
+			       entry->key.term_mac.in_pport))
 		return -EMSGSIZE;
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_LPORT_MASK,
-			       entry->key.term_mac.in_lport_mask))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT_MASK,
+			       entry->key.term_mac.in_pport_mask))
 		return -EMSGSIZE;
 	if (rocker_tlv_put_be16(desc_info, ROCKER_TLV_OF_DPA_ETHERTYPE,
 				entry->key.term_mac.eth_type))
@@ -1845,11 +1932,11 @@ static int rocker_cmd_flow_tbl_add_bridge(struct rocker_desc_info *desc_info,
 static int rocker_cmd_flow_tbl_add_acl(struct rocker_desc_info *desc_info,
 				       struct rocker_flow_tbl_entry *entry)
 {
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_LPORT,
-			       entry->key.acl.in_lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
+			       entry->key.acl.in_pport))
 		return -EMSGSIZE;
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_LPORT_MASK,
-			       entry->key.acl.in_lport_mask))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT_MASK,
+			       entry->key.acl.in_pport_mask))
 		return -EMSGSIZE;
 	if (rocker_tlv_put(desc_info, ROCKER_TLV_OF_DPA_SRC_MAC,
 			   ETH_ALEN, entry->key.acl.eth_src))
@@ -1917,8 +2004,7 @@ static int rocker_cmd_flow_tbl_add(struct rocker *rocker,
 	struct rocker_tlv *cmd_info;
 	int err = 0;
 
-	if (rocker_tlv_put_u16(desc_info, ROCKER_TLV_CMD_TYPE,
-			       ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_ADD))
+	if (rocker_tlv_put_u16(desc_info, ROCKER_TLV_CMD_TYPE, entry->cmd))
 		return -EMSGSIZE;
 	cmd_info = rocker_tlv_nest_start(desc_info, ROCKER_TLV_CMD_INFO);
 	if (!cmd_info)
@@ -1975,8 +2061,7 @@ static int rocker_cmd_flow_tbl_del(struct rocker *rocker,
 	const struct rocker_flow_tbl_entry *entry = priv;
 	struct rocker_tlv *cmd_info;
 
-	if (rocker_tlv_put_u16(desc_info, ROCKER_TLV_CMD_TYPE,
-			       ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_DEL))
+	if (rocker_tlv_put_u16(desc_info, ROCKER_TLV_CMD_TYPE, entry->cmd))
 		return -EMSGSIZE;
 	cmd_info = rocker_tlv_nest_start(desc_info, ROCKER_TLV_CMD_INFO);
 	if (!cmd_info)
@@ -1993,7 +2078,7 @@ static int
 rocker_cmd_group_tbl_add_l2_interface(struct rocker_desc_info *desc_info,
 				      struct rocker_group_tbl_entry *entry)
 {
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_OUT_LPORT,
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_OUT_PPORT,
 			       ROCKER_GROUP_PORT_GET(entry->group_id)))
 		return -EMSGSIZE;
 	if (rocker_tlv_put_u8(desc_info, ROCKER_TLV_OF_DPA_POP_VLAN,
@@ -2145,9 +2230,9 @@ static int rocker_cmd_group_tbl_del(struct rocker *rocker,
 	return 0;
 }
 
-/*****************************************
- * Flow, group, FDB, internal VLAN tables
- *****************************************/
+/***************************************************
+ * Flow, group, FDB, internal VLAN and neigh tables
+ ***************************************************/
 
 static int rocker_init_tbls(struct rocker *rocker)
 {
@@ -2163,6 +2248,9 @@ static int rocker_init_tbls(struct rocker *rocker)
 	hash_init(rocker->internal_vlan_tbl);
 	spin_lock_init(&rocker->internal_vlan_tbl_lock);
 
+	hash_init(rocker->neigh_tbl);
+	spin_lock_init(&rocker->neigh_tbl_lock);
+
 	return 0;
 }
 
@@ -2173,6 +2261,7 @@ static void rocker_free_tbls(struct rocker *rocker)
 	struct rocker_group_tbl_entry *group_entry;
 	struct rocker_fdb_tbl_entry *fdb_entry;
 	struct rocker_internal_vlan_tbl_entry *internal_vlan_entry;
+	struct rocker_neigh_tbl_entry *neigh_entry;
 	struct hlist_node *tmp;
 	int bkt;
 
@@ -2196,16 +2285,22 @@ static void rocker_free_tbls(struct rocker *rocker)
 			   tmp, internal_vlan_entry, entry)
 		hash_del(&internal_vlan_entry->entry);
 	spin_unlock_irqrestore(&rocker->internal_vlan_tbl_lock, flags);
+
+	spin_lock_irqsave(&rocker->neigh_tbl_lock, flags);
+	hash_for_each_safe(rocker->neigh_tbl, bkt, tmp, neigh_entry, entry)
+		hash_del(&neigh_entry->entry);
+	spin_unlock_irqrestore(&rocker->neigh_tbl_lock, flags);
 }
 
 static struct rocker_flow_tbl_entry *
 rocker_flow_tbl_find(struct rocker *rocker, struct rocker_flow_tbl_entry *match)
 {
 	struct rocker_flow_tbl_entry *found;
+	size_t key_len = match->key_len ? match->key_len : sizeof(found->key);
 
 	hash_for_each_possible(rocker->flow_tbl, found,
 			       entry, match->key_crc32) {
-		if (memcmp(&found->key, &match->key, sizeof(found->key)) == 0)
+		if (memcmp(&found->key, &match->key, key_len) == 0)
 			return found;
 	}
 
@@ -2218,42 +2313,34 @@ static int rocker_flow_tbl_add(struct rocker_port *rocker_port,
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_flow_tbl_entry *found;
+	size_t key_len = match->key_len ? match->key_len : sizeof(found->key);
 	unsigned long flags;
-	bool add_to_hw = false;
-	int err = 0;
 
-	match->key_crc32 = crc32(~0, &match->key, sizeof(match->key));
+	match->key_crc32 = crc32(~0, &match->key, key_len);
 
 	spin_lock_irqsave(&rocker->flow_tbl_lock, flags);
 
 	found = rocker_flow_tbl_find(rocker, match);
 
 	if (found) {
-		kfree(match);
+		match->cookie = found->cookie;
+		hash_del(&found->entry);
+		kfree(found);
+		found = match;
+		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_MOD;
 	} else {
 		found = match;
 		found->cookie = rocker->flow_tbl_next_cookie++;
-		hash_add(rocker->flow_tbl, &found->entry, found->key_crc32);
-		add_to_hw = true;
+		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_ADD;
 	}
 
-	found->ref_count++;
+	hash_add(rocker->flow_tbl, &found->entry, found->key_crc32);
 
 	spin_unlock_irqrestore(&rocker->flow_tbl_lock, flags);
 
-	if (add_to_hw) {
-		err = rocker_cmd_exec(rocker, rocker_port,
-				      rocker_cmd_flow_tbl_add,
-				      found, NULL, NULL, nowait);
-		if (err) {
-			spin_lock_irqsave(&rocker->flow_tbl_lock, flags);
-			hash_del(&found->entry);
-			spin_unlock_irqrestore(&rocker->flow_tbl_lock, flags);
-			kfree(found);
-		}
-	}
-
-	return err;
+	return rocker_cmd_exec(rocker, rocker_port,
+			       rocker_cmd_flow_tbl_add,
+			       found, NULL, NULL, nowait);
 }
 
 static int rocker_flow_tbl_del(struct rocker_port *rocker_port,
@@ -2262,29 +2349,26 @@ static int rocker_flow_tbl_del(struct rocker_port *rocker_port,
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_flow_tbl_entry *found;
+	size_t key_len = match->key_len ? match->key_len : sizeof(found->key);
 	unsigned long flags;
-	bool del_from_hw = false;
 	int err = 0;
 
-	match->key_crc32 = crc32(~0, &match->key, sizeof(match->key));
+	match->key_crc32 = crc32(~0, &match->key, key_len);
 
 	spin_lock_irqsave(&rocker->flow_tbl_lock, flags);
 
 	found = rocker_flow_tbl_find(rocker, match);
 
 	if (found) {
-		found->ref_count--;
-		if (found->ref_count == 0) {
-			hash_del(&found->entry);
-			del_from_hw = true;
-		}
+		hash_del(&found->entry);
+		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_DEL;
 	}
 
 	spin_unlock_irqrestore(&rocker->flow_tbl_lock, flags);
 
 	kfree(match);
 
-	if (del_from_hw) {
+	if (found) {
 		err = rocker_cmd_exec(rocker, rocker_port,
 				      rocker_cmd_flow_tbl_del,
 				      found, NULL, NULL, nowait);
@@ -2311,7 +2395,7 @@ static int rocker_flow_tbl_do(struct rocker_port *rocker_port,
 }
 
 static int rocker_flow_tbl_ig_port(struct rocker_port *rocker_port,
-				   int flags, u32 in_lport, u32 in_lport_mask,
+				   int flags, u32 in_pport, u32 in_pport_mask,
 				   enum rocker_of_dpa_table_id goto_tbl)
 {
 	struct rocker_flow_tbl_entry *entry;
@@ -2322,15 +2406,15 @@ static int rocker_flow_tbl_ig_port(struct rocker_port *rocker_port,
 
 	entry->key.priority = ROCKER_PRIORITY_IG_PORT;
 	entry->key.tbl_id = ROCKER_OF_DPA_TABLE_ID_INGRESS_PORT;
-	entry->key.ig_port.in_lport = in_lport;
-	entry->key.ig_port.in_lport_mask = in_lport_mask;
+	entry->key.ig_port.in_pport = in_pport;
+	entry->key.ig_port.in_pport_mask = in_pport_mask;
 	entry->key.ig_port.goto_tbl = goto_tbl;
 
 	return rocker_flow_tbl_do(rocker_port, flags, entry);
 }
 
 static int rocker_flow_tbl_vlan(struct rocker_port *rocker_port,
-				int flags, u32 in_lport,
+				int flags, u32 in_pport,
 				__be16 vlan_id, __be16 vlan_id_mask,
 				enum rocker_of_dpa_table_id goto_tbl,
 				bool untagged, __be16 new_vlan_id)
@@ -2343,7 +2427,7 @@ static int rocker_flow_tbl_vlan(struct rocker_port *rocker_port,
 
 	entry->key.priority = ROCKER_PRIORITY_VLAN;
 	entry->key.tbl_id = ROCKER_OF_DPA_TABLE_ID_VLAN;
-	entry->key.vlan.in_lport = in_lport;
+	entry->key.vlan.in_pport = in_pport;
 	entry->key.vlan.vlan_id = vlan_id;
 	entry->key.vlan.vlan_id_mask = vlan_id_mask;
 	entry->key.vlan.goto_tbl = goto_tbl;
@@ -2355,7 +2439,7 @@ static int rocker_flow_tbl_vlan(struct rocker_port *rocker_port,
 }
 
 static int rocker_flow_tbl_term_mac(struct rocker_port *rocker_port,
-				    u32 in_lport, u32 in_lport_mask,
+				    u32 in_pport, u32 in_pport_mask,
 				    __be16 eth_type, const u8 *eth_dst,
 				    const u8 *eth_dst_mask, __be16 vlan_id,
 				    __be16 vlan_id_mask, bool copy_to_cpu,
@@ -2378,8 +2462,8 @@ static int rocker_flow_tbl_term_mac(struct rocker_port *rocker_port,
 	}
 
 	entry->key.tbl_id = ROCKER_OF_DPA_TABLE_ID_TERMINATION_MAC;
-	entry->key.term_mac.in_lport = in_lport;
-	entry->key.term_mac.in_lport_mask = in_lport_mask;
+	entry->key.term_mac.in_pport = in_pport;
+	entry->key.term_mac.in_pport_mask = in_pport_mask;
 	entry->key.term_mac.eth_type = eth_type;
 	ether_addr_copy(entry->key.term_mac.eth_dst, eth_dst);
 	ether_addr_copy(entry->key.term_mac.eth_dst_mask, eth_dst_mask);
@@ -2444,9 +2528,34 @@ static int rocker_flow_tbl_bridge(struct rocker_port *rocker_port,
 	return rocker_flow_tbl_do(rocker_port, flags, entry);
 }
 
+static int rocker_flow_tbl_ucast4_routing(struct rocker_port *rocker_port,
+					  __be16 eth_type, __be32 dst,
+					  __be32 dst_mask, u32 priority,
+					  enum rocker_of_dpa_table_id goto_tbl,
+					  u32 group_id, int flags)
+{
+	struct rocker_flow_tbl_entry *entry;
+
+	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	if (!entry)
+		return -ENOMEM;
+
+	entry->key.tbl_id = ROCKER_OF_DPA_TABLE_ID_UNICAST_ROUTING;
+	entry->key.priority = priority;
+	entry->key.ucast_routing.eth_type = eth_type;
+	entry->key.ucast_routing.dst4 = dst;
+	entry->key.ucast_routing.dst4_mask = dst_mask;
+	entry->key.ucast_routing.goto_tbl = goto_tbl;
+	entry->key.ucast_routing.group_id = group_id;
+	entry->key_len = offsetof(struct rocker_flow_tbl_key,
+				  ucast_routing.group_id);
+
+	return rocker_flow_tbl_do(rocker_port, flags, entry);
+}
+
 static int rocker_flow_tbl_acl(struct rocker_port *rocker_port,
-			       int flags, u32 in_lport,
-			       u32 in_lport_mask,
+			       int flags, u32 in_pport,
+			       u32 in_pport_mask,
 			       const u8 *eth_src, const u8 *eth_src_mask,
 			       const u8 *eth_dst, const u8 *eth_dst_mask,
 			       __be16 eth_type,
@@ -2472,8 +2581,8 @@ static int rocker_flow_tbl_acl(struct rocker_port *rocker_port,
 
 	entry->key.priority = priority;
 	entry->key.tbl_id = ROCKER_OF_DPA_TABLE_ID_ACL_POLICY;
-	entry->key.acl.in_lport = in_lport;
-	entry->key.acl.in_lport_mask = in_lport_mask;
+	entry->key.acl.in_pport = in_pport;
+	entry->key.acl.in_pport_mask = in_pport_mask;
 
 	if (eth_src)
 		ether_addr_copy(entry->key.acl.eth_src, eth_src);
@@ -2531,7 +2640,6 @@ static int rocker_group_tbl_add(struct rocker_port *rocker_port,
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_group_tbl_entry *found;
 	unsigned long flags;
-	int err = 0;
 
 	spin_lock_irqsave(&rocker->group_tbl_lock, flags);
 
@@ -2551,12 +2659,9 @@ static int rocker_group_tbl_add(struct rocker_port *rocker_port,
 
 	spin_unlock_irqrestore(&rocker->group_tbl_lock, flags);
 
-	if (found->cmd)
-		err = rocker_cmd_exec(rocker, rocker_port,
-				      rocker_cmd_group_tbl_add,
-				      found, NULL, NULL, nowait);
-
-	return err;
+	return rocker_cmd_exec(rocker, rocker_port,
+			       rocker_cmd_group_tbl_add,
+			       found, NULL, NULL, nowait);
 }
 
 static int rocker_group_tbl_del(struct rocker_port *rocker_port,
@@ -2604,7 +2709,7 @@ static int rocker_group_tbl_do(struct rocker_port *rocker_port,
 
 static int rocker_group_l2_interface(struct rocker_port *rocker_port,
 				     int flags, __be16 vlan_id,
-				     u32 out_lport, int pop_vlan)
+				     u32 out_pport, int pop_vlan)
 {
 	struct rocker_group_tbl_entry *entry;
 
@@ -2612,7 +2717,7 @@ static int rocker_group_l2_interface(struct rocker_port *rocker_port,
 	if (!entry)
 		return -ENOMEM;
 
-	entry->group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_lport);
+	entry->group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 	entry->l2_interface.pop_vlan = pop_vlan;
 
 	return rocker_group_tbl_do(rocker_port, flags, entry);
@@ -2652,17 +2757,262 @@ static int rocker_group_l2_flood(struct rocker_port *rocker_port,
 				       group_id);
 }
 
+static int rocker_group_l3_unicast(struct rocker_port *rocker_port,
+				   int flags, u32 index, u8 *src_mac,
+				   u8 *dst_mac, __be16 vlan_id,
+				   bool ttl_check, u32 pport)
+{
+	struct rocker_group_tbl_entry *entry;
+
+	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	if (!entry)
+		return -ENOMEM;
+
+	entry->group_id = ROCKER_GROUP_L3_UNICAST(index);
+	if (src_mac)
+		ether_addr_copy(entry->l3_unicast.eth_src, src_mac);
+	if (dst_mac)
+		ether_addr_copy(entry->l3_unicast.eth_dst, dst_mac);
+	entry->l3_unicast.vlan_id = vlan_id;
+	entry->l3_unicast.ttl_check = ttl_check;
+	entry->l3_unicast.group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, pport);
+
+	return rocker_group_tbl_do(rocker_port, flags, entry);
+}
+
+static struct rocker_neigh_tbl_entry *
+	rocker_neigh_tbl_find(struct rocker *rocker, __be32 ip_addr)
+{
+	struct rocker_neigh_tbl_entry *found;
+
+	hash_for_each_possible(rocker->neigh_tbl, found,
+			       entry, be32_to_cpu(ip_addr))
+		if (found->ip_addr == ip_addr)
+			return found;
+
+	return NULL;
+}
+
+static void _rocker_neigh_add(struct rocker *rocker,
+			      struct rocker_neigh_tbl_entry *entry)
+{
+	entry->index = rocker->neigh_tbl_next_index++;
+	entry->ref_count++;
+	hash_add(rocker->neigh_tbl, &entry->entry,
+		 be32_to_cpu(entry->ip_addr));
+}
+
+static void _rocker_neigh_del(struct rocker *rocker,
+			      struct rocker_neigh_tbl_entry *entry)
+{
+	if (--entry->ref_count == 0) {
+		hash_del(&entry->entry);
+		kfree(entry);
+	}
+}
+
+static void _rocker_neigh_update(struct rocker *rocker,
+				 struct rocker_neigh_tbl_entry *entry,
+				 u8 *eth_dst, bool ttl_check)
+{
+	if (eth_dst) {
+		ether_addr_copy(entry->eth_dst, eth_dst);
+		entry->ttl_check = ttl_check;
+	} else {
+		entry->ref_count++;
+	}
+}
+
+static int rocker_port_ipv4_neigh(struct rocker_port *rocker_port,
+				  int flags, __be32 ip_addr, u8 *eth_dst)
+{
+	struct rocker *rocker = rocker_port->rocker;
+	struct rocker_neigh_tbl_entry *entry;
+	struct rocker_neigh_tbl_entry *found;
+	unsigned long lock_flags;
+	__be16 eth_type = htons(ETH_P_IP);
+	enum rocker_of_dpa_table_id goto_tbl =
+		ROCKER_OF_DPA_TABLE_ID_ACL_POLICY;
+	u32 group_id;
+	u32 priority = 0;
+	bool adding = !(flags & ROCKER_OP_FLAG_REMOVE);
+	bool updating;
+	bool removing;
+	int err = 0;
+
+	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	if (!entry)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&rocker->neigh_tbl_lock, lock_flags);
+
+	found = rocker_neigh_tbl_find(rocker, ip_addr);
+
+	updating = found && adding;
+	removing = found && !adding;
+	adding = !found && adding;
+
+	if (adding) {
+		entry->ip_addr = ip_addr;
+		entry->dev = rocker_port->dev;
+		ether_addr_copy(entry->eth_dst, eth_dst);
+		entry->ttl_check = true;
+		_rocker_neigh_add(rocker, entry);
+	} else if (removing) {
+		memcpy(entry, found, sizeof(*entry));
+		_rocker_neigh_del(rocker, found);
+	} else if (updating) {
+		_rocker_neigh_update(rocker, found, eth_dst, true);
+		memcpy(entry, found, sizeof(*entry));
+	} else {
+		err = -ENOENT;
+	}
+
+	spin_unlock_irqrestore(&rocker->neigh_tbl_lock, lock_flags);
+
+	if (err)
+		goto err_out;
+
+	/* For each active neighbor, we have an L3 unicast group and
+	 * a /32 route to the neighbor, which uses the L3 unicast
+	 * group.  The L3 unicast group can also be referred to by
+	 * other routes' nexthops.
+	 */
+
+	err = rocker_group_l3_unicast(rocker_port, flags,
+				      entry->index,
+				      rocker_port->dev->dev_addr,
+				      entry->eth_dst,
+				      rocker_port->internal_vlan_id,
+				      entry->ttl_check,
+				      rocker_port->pport);
+	if (err) {
+		netdev_err(rocker_port->dev,
+			   "Error (%d) L3 unicast group index %d\n",
+			   err, entry->index);
+		goto err_out;
+	}
+
+	if (adding || removing) {
+		group_id = ROCKER_GROUP_L3_UNICAST(entry->index);
+		err = rocker_flow_tbl_ucast4_routing(rocker_port,
+						     eth_type, ip_addr,
+						     inet_make_mask(32),
+						     priority, goto_tbl,
+						     group_id, flags);
+
+		if (err)
+			netdev_err(rocker_port->dev,
+				   "Error (%d) /32 unicast route %pI4 group 0x%08x\n",
+				   err, &entry->ip_addr, group_id);
+	}
+
+err_out:
+	if (!adding)
+		kfree(entry);
+
+	return err;
+}
+
+static int rocker_port_ipv4_resolve(struct rocker_port *rocker_port,
+				    __be32 ip_addr)
+{
+	struct net_device *dev = rocker_port->dev;
+	struct neighbour *n = __ipv4_neigh_lookup(dev, (__force u32)ip_addr);
+	int err = 0;
+
+	if (!n)
+		n = neigh_create(&arp_tbl, &ip_addr, dev);
+	if (!n)
+		return -ENOMEM;
+
+	/* If the neigh is already resolved, then go ahead and
+	 * install the entry, otherwise start the ARP process to
+	 * resolve the neigh.
+	 */
+
+	if (n->nud_state & NUD_VALID)
+		err = rocker_port_ipv4_neigh(rocker_port, 0, ip_addr, n->ha);
+	else
+		neigh_event_send(n, NULL);
+
+	return err;
+}
+
+static int rocker_port_ipv4_nh(struct rocker_port *rocker_port, int flags,
+			       __be32 ip_addr, u32 *index)
+{
+	struct rocker *rocker = rocker_port->rocker;
+	struct rocker_neigh_tbl_entry *entry;
+	struct rocker_neigh_tbl_entry *found;
+	unsigned long lock_flags;
+	bool adding = !(flags & ROCKER_OP_FLAG_REMOVE);
+	bool updating;
+	bool removing;
+	bool resolved = true;
+	int err = 0;
+
+	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	if (!entry)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&rocker->neigh_tbl_lock, lock_flags);
+
+	found = rocker_neigh_tbl_find(rocker, ip_addr);
+	if (found)
+		*index = found->index;
+
+	updating = found && adding;
+	removing = found && !adding;
+	adding = !found && adding;
+
+	if (adding) {
+		entry->ip_addr = ip_addr;
+		entry->dev = rocker_port->dev;
+		_rocker_neigh_add(rocker, entry);
+		*index = entry->index;
+		resolved = false;
+	} else if (removing) {
+		_rocker_neigh_del(rocker, found);
+	} else if (updating) {
+		_rocker_neigh_update(rocker, found, NULL, false);
+		resolved = !is_zero_ether_addr(found->eth_dst);
+	} else {
+		err = -ENOENT;
+	}
+
+	spin_unlock_irqrestore(&rocker->neigh_tbl_lock, lock_flags);
+
+	if (!adding)
+		kfree(entry);
+
+	if (err)
+		return err;
+
+	/* Resolved means neigh ip_addr is resolved to neigh mac. */
+
+	if (!resolved)
+		err = rocker_port_ipv4_resolve(rocker_port, ip_addr);
+
+	return err;
+}
+
 static int rocker_port_vlan_flood_group(struct rocker_port *rocker_port,
 					int flags, __be16 vlan_id)
 {
 	struct rocker_port *p;
 	struct rocker *rocker = rocker_port->rocker;
 	u32 group_id = ROCKER_GROUP_L2_FLOOD(vlan_id, 0);
-	u32 group_ids[rocker->port_count];
+	u32 *group_ids;
 	u8 group_count = 0;
-	int err;
+	int err = 0;
 	int i;
 
+	group_ids = kcalloc(rocker->port_count, sizeof(u32),
+			    rocker_op_flags_gfp(flags));
+	if (!group_ids)
+		return -ENOMEM;
+
 	/* Adjust the flood group for this VLAN.  The flood group
 	 * references an L2 interface group for each port in this
 	 * VLAN.
@@ -2674,14 +3024,13 @@ static int rocker_port_vlan_flood_group(struct rocker_port *rocker_port,
 			continue;
 		if (test_bit(ntohs(vlan_id), p->vlan_bitmap)) {
 			group_ids[group_count++] =
-				ROCKER_GROUP_L2_INTERFACE(vlan_id,
-							  p->lport);
+				ROCKER_GROUP_L2_INTERFACE(vlan_id, p->pport);
 		}
 	}
 
 	/* If there are no bridged ports in this VLAN, we're done */
 	if (group_count == 0)
-		return 0;
+		goto no_ports_in_vlan;
 
 	err = rocker_group_l2_flood(rocker_port, flags, vlan_id,
 				    group_count, group_ids,
@@ -2690,6 +3039,8 @@ static int rocker_port_vlan_flood_group(struct rocker_port *rocker_port,
 		netdev_err(rocker_port->dev,
 			   "Error (%d) port VLAN l2 flood group\n", err);
 
+no_ports_in_vlan:
+	kfree(group_ids);
 	return err;
 }
 
@@ -2700,7 +3051,7 @@ static int rocker_port_vlan_l2_groups(struct rocker_port *rocker_port,
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_port *p;
 	bool adding = !(flags & ROCKER_OP_FLAG_REMOVE);
-	u32 out_lport;
+	u32 out_pport;
 	int ref = 0;
 	int err;
 	int i;
@@ -2711,14 +3062,14 @@ static int rocker_port_vlan_l2_groups(struct rocker_port *rocker_port,
 
 	if (rocker_port->stp_state == BR_STATE_LEARNING ||
 	    rocker_port->stp_state == BR_STATE_FORWARDING) {
-		out_lport = rocker_port->lport;
+		out_pport = rocker_port->pport;
 		err = rocker_group_l2_interface(rocker_port, flags,
-						vlan_id, out_lport,
+						vlan_id, out_pport,
 						pop_vlan);
 		if (err) {
 			netdev_err(rocker_port->dev,
-				   "Error (%d) port VLAN l2 group for lport %d\n",
-				   err, out_lport);
+				   "Error (%d) port VLAN l2 group for pport %d\n",
+				   err, out_pport);
 			return err;
 		}
 	}
@@ -2737,9 +3088,9 @@ static int rocker_port_vlan_l2_groups(struct rocker_port *rocker_port,
 	if ((!adding || ref != 1) && (adding || ref != 0))
 		return 0;
 
-	out_lport = 0;
+	out_pport = 0;
 	err = rocker_group_l2_interface(rocker_port, flags,
-					vlan_id, out_lport,
+					vlan_id, out_pport,
 					pop_vlan);
 	if (err) {
 		netdev_err(rocker_port->dev,
@@ -2799,9 +3150,9 @@ static int rocker_port_ctrl_vlan_acl(struct rocker_port *rocker_port,
 				     int flags, struct rocker_ctrl *ctrl,
 				     __be16 vlan_id)
 {
-	u32 in_lport = rocker_port->lport;
-	u32 in_lport_mask = 0xffffffff;
-	u32 out_lport = 0;
+	u32 in_pport = rocker_port->pport;
+	u32 in_pport_mask = 0xffffffff;
+	u32 out_pport = 0;
 	u8 *eth_src = NULL;
 	u8 *eth_src_mask = NULL;
 	__be16 vlan_id_mask = htons(0xffff);
@@ -2809,11 +3160,11 @@ static int rocker_port_ctrl_vlan_acl(struct rocker_port *rocker_port,
 	u8 ip_proto_mask = 0;
 	u8 ip_tos = 0;
 	u8 ip_tos_mask = 0;
-	u32 group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_lport);
+	u32 group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 	int err;
 
 	err = rocker_flow_tbl_acl(rocker_port, flags,
-				  in_lport, in_lport_mask,
+				  in_pport, in_pport_mask,
 				  eth_src, eth_src_mask,
 				  ctrl->eth_dst, ctrl->eth_dst_mask,
 				  ctrl->eth_type,
@@ -2856,7 +3207,7 @@ static int rocker_port_ctrl_vlan_term(struct rocker_port *rocker_port,
 				      int flags, struct rocker_ctrl *ctrl,
 				      __be16 vlan_id)
 {
-	u32 in_lport_mask = 0xffffffff;
+	u32 in_pport_mask = 0xffffffff;
 	__be16 vlan_id_mask = htons(0xffff);
 	int err;
 
@@ -2864,7 +3215,7 @@ static int rocker_port_ctrl_vlan_term(struct rocker_port *rocker_port,
 		vlan_id = rocker_port->internal_vlan_id;
 
 	err = rocker_flow_tbl_term_mac(rocker_port,
-				       rocker_port->lport, in_lport_mask,
+				       rocker_port->pport, in_pport_mask,
 				       ctrl->eth_type, ctrl->eth_dst,
 				       ctrl->eth_dst_mask, vlan_id,
 				       vlan_id_mask, ctrl->copy_to_cpu,
@@ -2934,7 +3285,7 @@ static int rocker_port_vlan(struct rocker_port *rocker_port, int flags,
 {
 	enum rocker_of_dpa_table_id goto_tbl =
 		ROCKER_OF_DPA_TABLE_ID_TERMINATION_MAC;
-	u32 in_lport = rocker_port->lport;
+	u32 in_pport = rocker_port->pport;
 	__be16 vlan_id = htons(vid);
 	__be16 vlan_id_mask = htons(0xffff);
 	__be16 internal_vlan_id;
@@ -2978,7 +3329,7 @@ static int rocker_port_vlan(struct rocker_port *rocker_port, int flags,
 	}
 
 	err = rocker_flow_tbl_vlan(rocker_port, flags,
-				   in_lport, vlan_id, vlan_id_mask,
+				   in_pport, vlan_id, vlan_id_mask,
 				   goto_tbl, untagged, internal_vlan_id);
 	if (err)
 		netdev_err(rocker_port->dev,
@@ -2990,20 +3341,20 @@ static int rocker_port_vlan(struct rocker_port *rocker_port, int flags,
 static int rocker_port_ig_tbl(struct rocker_port *rocker_port, int flags)
 {
 	enum rocker_of_dpa_table_id goto_tbl;
-	u32 in_lport;
-	u32 in_lport_mask;
+	u32 in_pport;
+	u32 in_pport_mask;
 	int err;
 
 	/* Normal Ethernet Frames.  Matches pkts from any local physical
 	 * ports.  Goto VLAN tbl.
 	 */
 
-	in_lport = 0;
-	in_lport_mask = 0xffff0000;
+	in_pport = 0;
+	in_pport_mask = 0xffff0000;
 	goto_tbl = ROCKER_OF_DPA_TABLE_ID_VLAN;
 
 	err = rocker_flow_tbl_ig_port(rocker_port, flags,
-				      in_lport, in_lport_mask,
+				      in_pport, in_pport_mask,
 				      goto_tbl);
 	if (err)
 		netdev_err(rocker_port->dev,
@@ -3047,7 +3398,7 @@ static int rocker_port_fdb_learn(struct rocker_port *rocker_port,
 	struct rocker_fdb_learn_work *lw;
 	enum rocker_of_dpa_table_id goto_tbl =
 		ROCKER_OF_DPA_TABLE_ID_ACL_POLICY;
-	u32 out_lport = rocker_port->lport;
+	u32 out_pport = rocker_port->pport;
 	u32 tunnel_id = 0;
 	u32 group_id = ROCKER_GROUP_NONE;
 	bool syncing = !!(rocker_port->brport_flags & BR_LEARNING_SYNC);
@@ -3055,7 +3406,7 @@ static int rocker_port_fdb_learn(struct rocker_port *rocker_port,
 	int err;
 
 	if (rocker_port_is_bridged(rocker_port))
-		group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_lport);
+		group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 
 	if (!(flags & ROCKER_OP_FLAG_REFRESH)) {
 		err = rocker_flow_tbl_bridge(rocker_port, flags, addr, NULL,
@@ -3114,7 +3465,7 @@ static int rocker_port_fdb(struct rocker_port *rocker_port,
 		return -ENOMEM;
 
 	fdb->learned = (flags & ROCKER_OP_FLAG_LEARNED);
-	fdb->key.lport = rocker_port->lport;
+	fdb->key.pport = rocker_port->pport;
 	ether_addr_copy(fdb->key.addr, addr);
 	fdb->key.vlan_id = vlan_id;
 	fdb->key_crc32 = crc32(~0, &fdb->key, sizeof(fdb->key));
@@ -3161,7 +3512,7 @@ static int rocker_port_fdb_flush(struct rocker_port *rocker_port)
 	spin_lock_irqsave(&rocker->fdb_tbl_lock, lock_flags);
 
 	hash_for_each_safe(rocker->fdb_tbl, bkt, tmp, found, entry) {
-		if (found->key.lport != rocker_port->lport)
+		if (found->key.pport != rocker_port->pport)
 			continue;
 		if (!found->learned)
 			continue;
@@ -3182,7 +3533,7 @@ err_out:
 static int rocker_port_router_mac(struct rocker_port *rocker_port,
 				  int flags, __be16 vlan_id)
 {
-	u32 in_lport_mask = 0xffffffff;
+	u32 in_pport_mask = 0xffffffff;
 	__be16 eth_type;
 	const u8 *dst_mac_mask = ff_mac;
 	__be16 vlan_id_mask = htons(0xffff);
@@ -3194,7 +3545,7 @@ static int rocker_port_router_mac(struct rocker_port *rocker_port,
 
 	eth_type = htons(ETH_P_IP);
 	err = rocker_flow_tbl_term_mac(rocker_port,
-				       rocker_port->lport, in_lport_mask,
+				       rocker_port->pport, in_pport_mask,
 				       eth_type, rocker_port->dev->dev_addr,
 				       dst_mac_mask, vlan_id, vlan_id_mask,
 				       copy_to_cpu, flags);
@@ -3203,7 +3554,7 @@ static int rocker_port_router_mac(struct rocker_port *rocker_port,
 
 	eth_type = htons(ETH_P_IPV6);
 	err = rocker_flow_tbl_term_mac(rocker_port,
-				       rocker_port->lport, in_lport_mask,
+				       rocker_port->pport, in_pport_mask,
 				       eth_type, rocker_port->dev->dev_addr,
 				       dst_mac_mask, vlan_id, vlan_id_mask,
 				       copy_to_cpu, flags);
@@ -3214,7 +3565,7 @@ static int rocker_port_router_mac(struct rocker_port *rocker_port,
 static int rocker_port_fwding(struct rocker_port *rocker_port)
 {
 	bool pop_vlan;
-	u32 out_lport;
+	u32 out_pport;
 	__be16 vlan_id;
 	u16 vid;
 	int flags = ROCKER_OP_FLAG_NOWAIT;
@@ -3231,19 +3582,19 @@ static int rocker_port_fwding(struct rocker_port *rocker_port)
 	    rocker_port->stp_state != BR_STATE_FORWARDING)
 		flags |= ROCKER_OP_FLAG_REMOVE;
 
-	out_lport = rocker_port->lport;
+	out_pport = rocker_port->pport;
 	for (vid = 1; vid < VLAN_N_VID; vid++) {
 		if (!test_bit(vid, rocker_port->vlan_bitmap))
 			continue;
 		vlan_id = htons(vid);
 		pop_vlan = rocker_vlan_id_is_internal(vlan_id);
 		err = rocker_group_l2_interface(rocker_port, flags,
-						vlan_id, out_lport,
+						vlan_id, out_pport,
 						pop_vlan);
 		if (err) {
 			netdev_err(rocker_port->dev,
-				   "Error (%d) port VLAN l2 group for lport %d\n",
-				   err, out_lport);
+				   "Error (%d) port VLAN l2 group for pport %d\n",
+				   err, out_pport);
 			return err;
 		}
 	}
@@ -3302,6 +3653,26 @@ static int rocker_port_stp_update(struct rocker_port *rocker_port, u8 state)
 	return rocker_port_fwding(rocker_port);
 }
 
+static int rocker_port_fwd_enable(struct rocker_port *rocker_port)
+{
+	if (rocker_port_is_bridged(rocker_port))
+		/* bridge STP will enable port */
+		return 0;
+
+	/* port is not bridged, so simulate going to FORWARDING state */
+	return rocker_port_stp_update(rocker_port, BR_STATE_FORWARDING);
+}
+
+static int rocker_port_fwd_disable(struct rocker_port *rocker_port)
+{
+	if (rocker_port_is_bridged(rocker_port))
+		/* bridge STP will disable port */
+		return 0;
+
+	/* port is not bridged, so simulate going to DISABLED state */
+	return rocker_port_stp_update(rocker_port, BR_STATE_DISABLED);
+}
+
 static struct rocker_internal_vlan_tbl_entry *
 rocker_internal_vlan_tbl_find(struct rocker *rocker, int ifindex)
 {
@@ -3387,6 +3758,51 @@ not_found:
 	spin_unlock_irqrestore(&rocker->internal_vlan_tbl_lock, lock_flags);
 }
 
+static int rocker_port_fib_ipv4(struct rocker_port *rocker_port, __be32 dst,
+				int dst_len, struct fib_info *fi, u32 tb_id,
+				int flags)
+{
+	struct fib_nh *nh;
+	__be16 eth_type = htons(ETH_P_IP);
+	__be32 dst_mask = inet_make_mask(dst_len);
+	__be16 internal_vlan_id = rocker_port->internal_vlan_id;
+	u32 priority = fi->fib_priority;
+	enum rocker_of_dpa_table_id goto_tbl =
+		ROCKER_OF_DPA_TABLE_ID_ACL_POLICY;
+	u32 group_id;
+	bool nh_on_port;
+	bool has_gw;
+	u32 index;
+	int err;
+
+	/* XXX support ECMP */
+
+	nh = fi->fib_nh;
+	nh_on_port = (fi->fib_dev == rocker_port->dev);
+	has_gw = !!nh->nh_gw;
+
+	if (has_gw && nh_on_port) {
+		err = rocker_port_ipv4_nh(rocker_port, flags,
+					  nh->nh_gw, &index);
+		if (err)
+			return err;
+
+		group_id = ROCKER_GROUP_L3_UNICAST(index);
+	} else {
+		/* Send to CPU for processing */
+		group_id = ROCKER_GROUP_L2_INTERFACE(internal_vlan_id, 0);
+	}
+
+	err = rocker_flow_tbl_ucast4_routing(rocker_port, eth_type, dst,
+					     dst_mask, priority, goto_tbl,
+					     group_id, flags);
+	if (err)
+		netdev_err(rocker_port->dev, "Error (%d) IPv4 route %pI4\n",
+			   err, &dst);
+
+	return err;
+}
+
 /*****************
  * Net device ops
  *****************/
@@ -3394,8 +3810,6 @@ not_found:
 static int rocker_port_open(struct net_device *dev)
 {
 	struct rocker_port *rocker_port = netdev_priv(dev);
-	u8 stp_state = rocker_port_is_bridged(rocker_port) ?
-		BR_STATE_BLOCKING : BR_STATE_FORWARDING;
 	int err;
 
 	err = rocker_port_dma_rings_init(rocker_port);
@@ -3418,9 +3832,9 @@ static int rocker_port_open(struct net_device *dev)
 		goto err_request_rx_irq;
 	}
 
-	err = rocker_port_stp_update(rocker_port, stp_state);
+	err = rocker_port_fwd_enable(rocker_port);
 	if (err)
-		goto err_stp_update;
+		goto err_fwd_enable;
 
 	napi_enable(&rocker_port->napi_tx);
 	napi_enable(&rocker_port->napi_rx);
@@ -3428,7 +3842,7 @@ static int rocker_port_open(struct net_device *dev)
 	netif_start_queue(dev);
 	return 0;
 
-err_stp_update:
+err_fwd_enable:
 	free_irq(rocker_msix_rx_vector(rocker_port), rocker_port);
 err_request_rx_irq:
 	free_irq(rocker_msix_tx_vector(rocker_port), rocker_port);
@@ -3445,7 +3859,7 @@ static int rocker_port_stop(struct net_device *dev)
 	rocker_port_set_enable(rocker_port, false);
 	napi_disable(&rocker_port->napi_rx);
 	napi_disable(&rocker_port->napi_tx);
-	rocker_port_stp_update(rocker_port, BR_STATE_DISABLED);
+	rocker_port_fwd_disable(rocker_port);
 	free_irq(rocker_msix_rx_vector(rocker_port), rocker_port);
 	free_irq(rocker_msix_tx_vector(rocker_port), rocker_port);
 	rocker_port_dma_rings_fini(rocker_port);
@@ -3702,7 +4116,7 @@ static int rocker_port_fdb_dump(struct sk_buff *skb,
 
 	spin_lock_irqsave(&rocker->fdb_tbl_lock, lock_flags);
 	hash_for_each_safe(rocker->fdb_tbl, bkt, tmp, found, entry) {
-		if (found->key.lport != rocker_port->lport)
+		if (found->key.pport != rocker_port->pport)
 			continue;
 		if (idx < cb->args[0])
 			goto skip;
@@ -3772,22 +4186,19 @@ static int rocker_port_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 				       rocker_port->brport_flags, mask);
 }
 
-static int rocker_port_switch_parent_id_get(struct net_device *dev,
-					    struct netdev_phys_item_id *psid)
+static int rocker_port_get_phys_port_name(struct net_device *dev,
+					  char *buf, size_t len)
 {
 	struct rocker_port *rocker_port = netdev_priv(dev);
-	struct rocker *rocker = rocker_port->rocker;
-
-	psid->id_len = sizeof(rocker->hw.id);
-	memcpy(&psid->id, &rocker->hw.id, psid->id_len);
-	return 0;
-}
+	struct port_name name = { .buf = buf, .len = len };
+	int err;
 
-static int rocker_port_switch_port_stp_update(struct net_device *dev, u8 state)
-{
-	struct rocker_port *rocker_port = netdev_priv(dev);
+	err = rocker_cmd_exec(rocker_port->rocker, rocker_port,
+			      rocker_cmd_get_port_settings_prep, NULL,
+			      rocker_cmd_get_port_settings_phys_name_proc,
+			      &name, false);
 
-	return rocker_port_stp_update(rocker_port, state);
+	return err ? -EOPNOTSUPP : 0;
 }
 
 static const struct net_device_ops rocker_port_netdev_ops = {
@@ -3802,8 +4213,61 @@ static const struct net_device_ops rocker_port_netdev_ops = {
 	.ndo_fdb_dump			= rocker_port_fdb_dump,
 	.ndo_bridge_setlink		= rocker_port_bridge_setlink,
 	.ndo_bridge_getlink		= rocker_port_bridge_getlink,
-	.ndo_switch_parent_id_get	= rocker_port_switch_parent_id_get,
-	.ndo_switch_port_stp_update	= rocker_port_switch_port_stp_update,
+	.ndo_get_phys_port_name		= rocker_port_get_phys_port_name,
+};
+
+/********************
+ * swdev interface
+ ********************/
+
+static int rocker_port_swdev_parent_id_get(struct net_device *dev,
+					   struct netdev_phys_item_id *psid)
+{
+	struct rocker_port *rocker_port = netdev_priv(dev);
+	struct rocker *rocker = rocker_port->rocker;
+
+	psid->id_len = sizeof(rocker->hw.id);
+	memcpy(&psid->id, &rocker->hw.id, psid->id_len);
+	return 0;
+}
+
+static int rocker_port_swdev_port_stp_update(struct net_device *dev, u8 state)
+{
+	struct rocker_port *rocker_port = netdev_priv(dev);
+
+	return rocker_port_stp_update(rocker_port, state);
+}
+
+static int rocker_port_swdev_fib_ipv4_add(struct net_device *dev,
+					  __be32 dst, int dst_len,
+					  struct fib_info *fi,
+					  u8 tos, u8 type,
+					  u32 nlflags, u32 tb_id)
+{
+	struct rocker_port *rocker_port = netdev_priv(dev);
+	int flags = 0;
+
+	return rocker_port_fib_ipv4(rocker_port, dst, dst_len,
+				    fi, tb_id, flags);
+}
+
+static int rocker_port_swdev_fib_ipv4_del(struct net_device *dev,
+					  __be32 dst, int dst_len,
+					  struct fib_info *fi,
+					  u8 tos, u8 type, u32 tb_id)
+{
+	struct rocker_port *rocker_port = netdev_priv(dev);
+	int flags = ROCKER_OP_FLAG_REMOVE;
+
+	return rocker_port_fib_ipv4(rocker_port, dst, dst_len,
+				    fi, tb_id, flags);
+}
+
+static const struct swdev_ops rocker_port_swdev_ops = {
+	.swdev_parent_id_get		= rocker_port_swdev_parent_id_get,
+	.swdev_port_stp_update		= rocker_port_swdev_port_stp_update,
+	.swdev_fib_ipv4_add		= rocker_port_swdev_fib_ipv4_add,
+	.swdev_fib_ipv4_del		= rocker_port_swdev_fib_ipv4_del,
 };
 
 /********************
@@ -3882,8 +4346,8 @@ rocker_cmd_get_port_stats_prep(struct rocker *rocker,
 	if (!cmd_stats)
 		return -EMSGSIZE;
 
-	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_STATS_LPORT,
-			       rocker_port->lport))
+	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_CMD_PORT_STATS_PPORT,
+			       rocker_port->pport))
 		return -EMSGSIZE;
 
 	rocker_tlv_nest_end(desc_info, cmd_stats);
@@ -3900,7 +4364,7 @@ rocker_cmd_get_port_stats_ethtool_proc(struct rocker *rocker,
 	struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
 	struct rocker_tlv *stats_attrs[ROCKER_TLV_CMD_PORT_STATS_MAX + 1];
 	struct rocker_tlv *pattr;
-	u32 lport;
+	u32 pport;
 	u64 *data = priv;
 	int i;
 
@@ -3912,11 +4376,11 @@ rocker_cmd_get_port_stats_ethtool_proc(struct rocker *rocker,
 	rocker_tlv_parse_nested(stats_attrs, ROCKER_TLV_CMD_PORT_STATS_MAX,
 				attrs[ROCKER_TLV_CMD_INFO]);
 
-	if (!stats_attrs[ROCKER_TLV_CMD_PORT_STATS_LPORT])
+	if (!stats_attrs[ROCKER_TLV_CMD_PORT_STATS_PPORT])
 		return -EIO;
 
-	lport = rocker_tlv_get_u32(stats_attrs[ROCKER_TLV_CMD_PORT_STATS_LPORT]);
-	if (lport != rocker_port->lport)
+	pport = rocker_tlv_get_u32(stats_attrs[ROCKER_TLV_CMD_PORT_STATS_PPORT]);
+	if (pport != rocker_port->pport)
 		return -EIO;
 
 	for (i = 0; i < ARRAY_SIZE(rocker_port_stats); i++) {
@@ -4104,7 +4568,7 @@ static void rocker_carrier_init(struct rocker_port *rocker_port)
 	u64 link_status = rocker_read64(rocker, PORT_PHYS_LINK_STATUS);
 	bool link_up;
 
-	link_up = link_status & (1 << rocker_port->lport);
+	link_up = link_status & (1 << rocker_port->pport);
 	if (link_up)
 		netif_carrier_on(rocker_port->dev);
 	else
@@ -4152,20 +4616,22 @@ static int rocker_probe_port(struct rocker *rocker, unsigned int port_number)
 	rocker_port->dev = dev;
 	rocker_port->rocker = rocker;
 	rocker_port->port_number = port_number;
-	rocker_port->lport = port_number + 1;
+	rocker_port->pport = port_number + 1;
 	rocker_port->brport_flags = BR_LEARNING | BR_LEARNING_SYNC;
 
 	rocker_port_dev_addr_init(rocker, rocker_port);
 	dev->netdev_ops = &rocker_port_netdev_ops;
 	dev->ethtool_ops = &rocker_port_ethtool_ops;
+	dev->swdev_ops = &rocker_port_swdev_ops;
 	netif_napi_add(dev, &rocker_port->napi_tx, rocker_port_poll_tx,
 		       NAPI_POLL_WEIGHT);
 	netif_napi_add(dev, &rocker_port->napi_rx, rocker_port_poll_rx,
 		       NAPI_POLL_WEIGHT);
 	rocker_carrier_init(rocker_port);
 
-	dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER |
-				NETIF_F_HW_SWITCH_OFFLOAD;
+	dev->features |= NETIF_F_NETNS_LOCAL |
+			 NETIF_F_HW_VLAN_CTAG_FILTER |
+			 NETIF_F_HW_SWITCH_OFFLOAD;
 
 	err = register_netdev(dev);
 	if (err) {
@@ -4436,9 +4902,7 @@ static int rocker_port_bridge_join(struct rocker_port *rocker_port,
 	rocker_port->internal_vlan_id =
 		rocker_port_internal_vlan_id_get(rocker_port,
 						 bridge->ifindex);
-	err = rocker_port_vlan(rocker_port, 0, 0);
-
-	return err;
+	return rocker_port_vlan(rocker_port, 0, 0);
 }
 
 static int rocker_port_bridge_leave(struct rocker_port *rocker_port)
@@ -4458,6 +4922,11 @@ static int rocker_port_bridge_leave(struct rocker_port *rocker_port)
 		rocker_port_internal_vlan_id_get(rocker_port,
 						 rocker_port->dev->ifindex);
 	err = rocker_port_vlan(rocker_port, 0, 0);
+	if (err)
+		return err;
+
+	if (rocker_port->dev->flags & IFF_UP)
+		err = rocker_port_fwd_enable(rocker_port);
 
 	return err;
 }
@@ -4468,10 +4937,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;
@@ -4503,6 +4978,48 @@ static struct notifier_block rocker_netdevice_nb __read_mostly = {
 	.notifier_call = rocker_netdevice_event,
 };
 
+/************************************
+ * Net event notifier event handler
+ ************************************/
+
+static int rocker_neigh_update(struct net_device *dev, struct neighbour *n)
+{
+	struct rocker_port *rocker_port = netdev_priv(dev);
+	int flags = (n->nud_state & NUD_VALID) ? 0 : ROCKER_OP_FLAG_REMOVE;
+	__be32 ip_addr = *(__be32 *)n->primary_key;
+
+	return rocker_port_ipv4_neigh(rocker_port, flags, ip_addr, n->ha);
+}
+
+static int rocker_netevent_event(struct notifier_block *unused,
+				 unsigned long event, void *ptr)
+{
+	struct net_device *dev;
+	struct neighbour *n = ptr;
+	int err;
+
+	switch (event) {
+	case NETEVENT_NEIGH_UPDATE:
+		if (n->tbl != &arp_tbl)
+			return NOTIFY_DONE;
+		dev = n->dev;
+		if (!rocker_port_dev_check(dev))
+			return NOTIFY_DONE;
+		err = rocker_neigh_update(dev, n);
+		if (err)
+			netdev_warn(dev,
+				    "failed to handle neigh update (err %d)\n",
+				    err);
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block rocker_netevent_nb __read_mostly = {
+	.notifier_call = rocker_netevent_event,
+};
+
 /***********************
  * Module init and exit
  ***********************/
@@ -4512,18 +5029,21 @@ static int __init rocker_module_init(void)
 	int err;
 
 	register_netdevice_notifier(&rocker_netdevice_nb);
+	register_netevent_notifier(&rocker_netevent_nb);
 	err = pci_register_driver(&rocker_pci_driver);
 	if (err)
 		goto err_pci_register_driver;
 	return 0;
 
 err_pci_register_driver:
+	unregister_netdevice_notifier(&rocker_netevent_nb);
 	unregister_netdevice_notifier(&rocker_netdevice_nb);
 	return err;
 }
 
 static void __exit rocker_module_exit(void)
 {
+	unregister_netevent_notifier(&rocker_netevent_nb);
 	unregister_netdevice_notifier(&rocker_netdevice_nb);
 	pci_unregister_driver(&rocker_pci_driver);
 }
diff --git a/drivers/net/ethernet/rocker/rocker.h b/drivers/net/ethernet/rocker/rocker.h
index a5bc432feada..a4e9591d7457 100644
--- a/drivers/net/ethernet/rocker/rocker.h
+++ b/drivers/net/ethernet/rocker/rocker.h
@@ -14,6 +14,21 @@
 
 #include <linux/types.h>
 
+/* Return codes */
+enum {
+	ROCKER_OK = 0,
+	ROCKER_ENOENT = 2,
+	ROCKER_ENXIO = 6,
+	ROCKER_ENOMEM = 12,
+	ROCKER_EEXIST = 17,
+	ROCKER_EINVAL = 22,
+	ROCKER_EMSGSIZE = 90,
+	ROCKER_ENOTSUP = 95,
+	ROCKER_ENOBUFS = 105,
+};
+
+#define ROCKER_FP_PORTS_MAX 62
+
 #define PCI_VENDOR_ID_REDHAT		0x1b36
 #define PCI_DEVICE_ID_REDHAT_ROCKER	0x0006
 
@@ -136,13 +151,14 @@ enum {
 
 enum {
 	ROCKER_TLV_CMD_PORT_SETTINGS_UNSPEC,
-	ROCKER_TLV_CMD_PORT_SETTINGS_LPORT,		/* u32 */
+	ROCKER_TLV_CMD_PORT_SETTINGS_PPORT,		/* u32 */
 	ROCKER_TLV_CMD_PORT_SETTINGS_SPEED,		/* u32 */
 	ROCKER_TLV_CMD_PORT_SETTINGS_DUPLEX,		/* u8 */
 	ROCKER_TLV_CMD_PORT_SETTINGS_AUTONEG,		/* u8 */
 	ROCKER_TLV_CMD_PORT_SETTINGS_MACADDR,		/* binary */
 	ROCKER_TLV_CMD_PORT_SETTINGS_MODE,		/* u8 */
 	ROCKER_TLV_CMD_PORT_SETTINGS_LEARNING,		/* u8 */
+	ROCKER_TLV_CMD_PORT_SETTINGS_PHYS_NAME,		/* binary */
 
 	__ROCKER_TLV_CMD_PORT_SETTINGS_MAX,
 	ROCKER_TLV_CMD_PORT_SETTINGS_MAX =
@@ -151,7 +167,7 @@ enum {
 
 enum {
 	ROCKER_TLV_CMD_PORT_STATS_UNSPEC,
-	ROCKER_TLV_CMD_PORT_STATS_LPORT,            /* u32 */
+	ROCKER_TLV_CMD_PORT_STATS_PPORT,            /* u32 */
 
 	ROCKER_TLV_CMD_PORT_STATS_RX_PKTS,          /* u64 */
 	ROCKER_TLV_CMD_PORT_STATS_RX_BYTES,         /* u64 */
@@ -191,7 +207,7 @@ enum {
 
 enum {
 	ROCKER_TLV_EVENT_LINK_CHANGED_UNSPEC,
-	ROCKER_TLV_EVENT_LINK_CHANGED_LPORT,	/* u32 */
+	ROCKER_TLV_EVENT_LINK_CHANGED_PPORT,	/* u32 */
 	ROCKER_TLV_EVENT_LINK_CHANGED_LINKUP,	/* u8 */
 
 	__ROCKER_TLV_EVENT_LINK_CHANGED_MAX,
@@ -201,7 +217,7 @@ enum {
 
 enum {
 	ROCKER_TLV_EVENT_MAC_VLAN_UNSPEC,
-	ROCKER_TLV_EVENT_MAC_VLAN_LPORT,	/* u32 */
+	ROCKER_TLV_EVENT_MAC_VLAN_PPORT,	/* u32 */
 	ROCKER_TLV_EVENT_MAC_VLAN_MAC,		/* binary */
 	ROCKER_TLV_EVENT_MAC_VLAN_VLAN_ID,	/* __be16 */
 
@@ -275,9 +291,9 @@ enum {
 	ROCKER_TLV_OF_DPA_HARDTIME,		/* u32 */
 	ROCKER_TLV_OF_DPA_IDLETIME,		/* u32 */
 	ROCKER_TLV_OF_DPA_COOKIE,		/* u64 */
-	ROCKER_TLV_OF_DPA_IN_LPORT,		/* u32 */
-	ROCKER_TLV_OF_DPA_IN_LPORT_MASK,	/* u32 */
-	ROCKER_TLV_OF_DPA_OUT_LPORT,		/* u32 */
+	ROCKER_TLV_OF_DPA_IN_PPORT,		/* u32 */
+	ROCKER_TLV_OF_DPA_IN_PPORT_MASK,	/* u32 */
+	ROCKER_TLV_OF_DPA_OUT_PPORT,		/* u32 */
 	ROCKER_TLV_OF_DPA_GOTO_TABLE_ID,	/* u16 */
 	ROCKER_TLV_OF_DPA_GROUP_ID,		/* u32 */
 	ROCKER_TLV_OF_DPA_GROUP_ID_LOWER,	/* u32 */
@@ -291,7 +307,7 @@ enum {
 	ROCKER_TLV_OF_DPA_NEW_VLAN_ID,		/* __be16 */
 	ROCKER_TLV_OF_DPA_NEW_VLAN_PCP,		/* u8 */
 	ROCKER_TLV_OF_DPA_TUNNEL_ID,		/* u32 */
-	ROCKER_TLV_OF_DPA_TUN_LOG_LPORT,	/* u32 */
+	ROCKER_TLV_OF_DPA_TUNNEL_LPORT,		/* u32 */
 	ROCKER_TLV_OF_DPA_ETHERTYPE,		/* __be16 */
 	ROCKER_TLV_OF_DPA_DST_MAC,		/* binary */
 	ROCKER_TLV_OF_DPA_DST_MAC_MASK,		/* binary */
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
index c8a01ee4d25e..413ea14ab91f 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
@@ -422,11 +422,11 @@ static int init_tx_ring(struct device *dev, u8 queue_no,
 	/* assign queue number */
 	tx_ring->queue_no = queue_no;
 
-	/* initalise counters */
+	/* initialise counters */
 	tx_ring->dirty_tx = 0;
 	tx_ring->cur_tx = 0;
 
-	/* initalise TX queue lock */
+	/* initialise TX queue lock */
 	spin_lock_init(&tx_ring->tx_lock);
 
 	return 0;
@@ -515,7 +515,7 @@ static int init_rx_ring(struct net_device *dev, u8 queue_no,
 			goto err_free_rx_buffers;
 	}
 
-	/* initalise counters */
+	/* initialise counters */
 	rx_ring->cur_rx = 0;
 	rx_ring->dirty_rx = (unsigned int)(desc_index - rx_rsize);
 	priv->dma_buf_sz = bfsize;
@@ -837,7 +837,7 @@ static void sxgbe_restart_tx_queue(struct sxgbe_priv_data *priv, int queue_num)
 	/* free the skbuffs of the ring */
 	tx_free_ring_skbufs(tx_ring);
 
-	/* initalise counters */
+	/* initialise counters */
 	tx_ring->cur_tx = 0;
 	tx_ring->dirty_tx = 0;
 
@@ -1176,7 +1176,7 @@ static int sxgbe_open(struct net_device *dev)
 	if (priv->phydev)
 		phy_start(priv->phydev);
 
-	/* initalise TX coalesce parameters */
+	/* initialise TX coalesce parameters */
 	sxgbe_tx_init_coalesce(priv);
 
 	if ((priv->use_riwt) && (priv->hw->dma->rx_watchdog)) {
@@ -1721,7 +1721,7 @@ static inline u64 sxgbe_get_stat64(void __iomem *ioaddr, int reg_lo, int reg_hi)
  *  Description:
  *  This function is a driver entry point whenever ifconfig command gets
  *  executed to see device statistics. Statistics are number of
- *  bytes sent or received, errors occured etc.
+ *  bytes sent or received, errors occurred etc.
  *  Return value:
  *  This function returns various statistical information of device.
  */
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index 77d6453f6a3e..4b00545a3ace 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -3213,7 +3213,7 @@ static pci_ers_result_t efx_io_error_detected(struct pci_dev *pdev,
 	return status;
 }
 
-/* Fake a successfull reset, which will be performed later in efx_io_resume. */
+/* Fake a successful reset, which will be performed later in efx_io_resume. */
 static pci_ers_result_t efx_io_slot_reset(struct pci_dev *pdev)
 {
 	struct efx_nic *efx = pci_get_drvdata(pdev);
diff --git a/drivers/net/ethernet/sfc/farch.c b/drivers/net/ethernet/sfc/farch.c
index 75975328e020..bb89e96a125e 100644
--- a/drivers/net/ethernet/sfc/farch.c
+++ b/drivers/net/ethernet/sfc/farch.c
@@ -645,7 +645,7 @@ static bool efx_check_tx_flush_complete(struct efx_nic *efx)
 }
 
 /* Flush all the transmit queues, and continue flushing receive queues until
- * they're all flushed. Wait for the DRAIN events to be recieved so that there
+ * they're all flushed. Wait for the DRAIN events to be received so that there
  * are no more RX and TX events left on any channel. */
 static int efx_farch_do_flush(struct efx_nic *efx)
 {
@@ -1108,7 +1108,7 @@ efx_farch_handle_tx_flush_done(struct efx_nic *efx, efx_qword_t *event)
 }
 
 /* If this flush done event corresponds to a &struct efx_rx_queue: If the flush
- * was succesful then send an %EFX_CHANNEL_MAGIC_RX_DRAIN, otherwise add
+ * was successful then send an %EFX_CHANNEL_MAGIC_RX_DRAIN, otherwise add
  * the RX queue back to the mask of RX queues in need of flushing.
  */
 static void
diff --git a/drivers/net/ethernet/sfc/mcdi_pcol.h b/drivers/net/ethernet/sfc/mcdi_pcol.h
index a707fb5ef14c..e028de10e1b7 100644
--- a/drivers/net/ethernet/sfc/mcdi_pcol.h
+++ b/drivers/net/ethernet/sfc/mcdi_pcol.h
@@ -6497,7 +6497,7 @@
 #define    MC_CMD_DUMP_BUFTBL_ENTRIES_OUT_LENMIN 12
 #define    MC_CMD_DUMP_BUFTBL_ENTRIES_OUT_LENMAX 252
 #define    MC_CMD_DUMP_BUFTBL_ENTRIES_OUT_LEN(num) (0+12*(num))
-/* Raw buffer table entries, layed out as BUFTBL_ENTRY. */
+/* Raw buffer table entries, laid out as BUFTBL_ENTRY. */
 #define       MC_CMD_DUMP_BUFTBL_ENTRIES_OUT_ENTRY_OFST 0
 #define       MC_CMD_DUMP_BUFTBL_ENTRIES_OUT_ENTRY_LEN 12
 #define       MC_CMD_DUMP_BUFTBL_ENTRIES_OUT_ENTRY_MINNUM 1
diff --git a/drivers/net/ethernet/sfc/ptp.c b/drivers/net/ethernet/sfc/ptp.c
index 6b861e3de4b0..a2e9aee05cdd 100644
--- a/drivers/net/ethernet/sfc/ptp.c
+++ b/drivers/net/ethernet/sfc/ptp.c
@@ -323,9 +323,9 @@ struct efx_ptp_data {
 
 static int efx_phc_adjfreq(struct ptp_clock_info *ptp, s32 delta);
 static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta);
-static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts);
+static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts);
 static int efx_phc_settime(struct ptp_clock_info *ptp,
-			   const struct timespec *e_ts);
+			   const struct timespec64 *e_ts);
 static int efx_phc_enable(struct ptp_clock_info *ptp,
 			  struct ptp_clock_request *request, int on);
 
@@ -1198,8 +1198,8 @@ static const struct ptp_clock_info efx_phc_clock_info = {
 	.pps		= 1,
 	.adjfreq	= efx_phc_adjfreq,
 	.adjtime	= efx_phc_adjtime,
-	.gettime	= efx_phc_gettime,
-	.settime	= efx_phc_settime,
+	.gettime64	= efx_phc_gettime,
+	.settime64	= efx_phc_settime,
 	.enable		= efx_phc_enable,
 };
 
@@ -1837,7 +1837,7 @@ static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)
 			    NULL, 0, NULL);
 }
 
-static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct efx_ptp_data *ptp_data = container_of(ptp,
 						     struct efx_ptp_data,
@@ -1859,28 +1859,28 @@ static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 	kt = ptp_data->nic_to_kernel_time(
 		MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_MAJOR),
 		MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_MINOR), 0);
-	*ts = ktime_to_timespec(kt);
+	*ts = ktime_to_timespec64(kt);
 	return 0;
 }
 
 static int efx_phc_settime(struct ptp_clock_info *ptp,
-			   const struct timespec *e_ts)
+			   const struct timespec64 *e_ts)
 {
 	/* Get the current NIC time, efx_phc_gettime.
 	 * Subtract from the desired time to get the offset
 	 * call efx_phc_adjtime with the offset
 	 */
 	int rc;
-	struct timespec time_now;
-	struct timespec delta;
+	struct timespec64 time_now;
+	struct timespec64 delta;
 
 	rc = efx_phc_gettime(ptp, &time_now);
 	if (rc != 0)
 		return rc;
 
-	delta = timespec_sub(*e_ts, time_now);
+	delta = timespec64_sub(*e_ts, time_now);
 
-	rc = efx_phc_adjtime(ptp, timespec_to_ns(&delta));
+	rc = efx_phc_adjtime(ptp, timespec64_to_ns(&delta));
 	if (rc != 0)
 		return rc;
 
diff --git a/drivers/net/ethernet/sfc/siena_sriov.c b/drivers/net/ethernet/sfc/siena_sriov.c
index a8bbbad68a88..fe83430796fd 100644
--- a/drivers/net/ethernet/sfc/siena_sriov.c
+++ b/drivers/net/ethernet/sfc/siena_sriov.c
@@ -1067,7 +1067,7 @@ void efx_siena_sriov_probe(struct efx_nic *efx)
 }
 
 /* Copy the list of individual addresses into the vfdi_status.peers
- * array and auxillary pages, protected by %local_lock. Drop that lock
+ * array and auxiliary pages, protected by %local_lock. Drop that lock
  * and then broadcast the address list to every VF.
  */
 static void efx_siena_sriov_peer_work(struct work_struct *data)
diff --git a/drivers/net/ethernet/sfc/vfdi.h b/drivers/net/ethernet/sfc/vfdi.h
index ae044f44936a..f62901d4cae0 100644
--- a/drivers/net/ethernet/sfc/vfdi.h
+++ b/drivers/net/ethernet/sfc/vfdi.h
@@ -98,7 +98,7 @@ struct vfdi_endpoint {
  * @VFDI_OP_INIT_TXQ: Initialize SRAM entries and initialize a TXQ.
  * @VFDI_OP_FINI_ALL_QUEUES: Flush all queues, finalize all queues, then
  *	finalize the SRAM entries.
- * @VFDI_OP_INSERT_FILTER: Insert a MAC filter targetting the given RXQ.
+ * @VFDI_OP_INSERT_FILTER: Insert a MAC filter targeting the given RXQ.
  * @VFDI_OP_REMOVE_ALL_FILTERS: Remove all filters.
  * @VFDI_OP_SET_STATUS_PAGE: Set the DMA page(s) used for status updates
  *	from PF and write the initial status.
@@ -148,7 +148,7 @@ enum vfdi_op {
  * @u.init_txq.flags: Checksum offload flags.
  * @u.init_txq.addr: Array of length %u.init_txq.buf_count containing DMA
  *	address of each page backing the transmit queue.
- * @u.mac_filter.rxq: Insert MAC filter at VF local address/VLAN targetting
+ * @u.mac_filter.rxq: Insert MAC filter at VF local address/VLAN targeting
  *	all traffic at this receive queue.
  * @u.mac_filter.flags: MAC filter flags.
  * @u.set_status_page.dma_addr: Base address for the &struct vfdi_status.
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index 5d093dc0f5f5..14b363a25c02 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -2204,27 +2204,17 @@ static int try_toggle_control_gpio(struct device *dev,
 				   int value, unsigned int nsdelay)
 {
 	struct gpio_desc *gpio = *desc;
-	int res;
-
-	gpio = devm_gpiod_get_index(dev, name, index);
-	if (IS_ERR(gpio)) {
-		if (PTR_ERR(gpio) == -ENOENT) {
-			*desc = NULL;
-			return 0;
-		}
+	enum gpiod_flags flags = value ? GPIOD_OUT_LOW : GPIOD_OUT_HIGH;
 
+	gpio = devm_gpiod_get_index_optional(dev, name, index, flags);
+	if (IS_ERR(gpio))
 		return PTR_ERR(gpio);
+
+	if (gpio) {
+		if (nsdelay)
+			usleep_range(nsdelay, 2 * nsdelay);
+		gpiod_set_value_cansleep(gpio, value);
 	}
-	res = gpiod_direction_output(gpio, !value);
-	if (res) {
-		dev_err(dev, "unable to toggle gpio %s: %i\n", name, res);
-		devm_gpiod_put(dev, gpio);
-		gpio = NULL;
-		return res;
-	}
-	if (nsdelay)
-		usleep_range(nsdelay, 2 * nsdelay);
-	gpiod_set_value_cansleep(gpio, value);
 	*desc = gpio;
 
 	return 0;
@@ -2248,10 +2238,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));
@@ -2343,19 +2332,16 @@ 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)
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index 2965c6ae7d6e..41047c9143d0 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -843,7 +843,7 @@ static int smsc911x_phy_loopbacktest(struct net_device *dev)
 	unsigned long flags;
 
 	/* Initialise tx packet using broadcast destination address */
-	memset(pdata->loopback_tx_pkt, 0xff, ETH_ALEN);
+	eth_broadcast_addr(pdata->loopback_tx_pkt);
 
 	/* Use incrementing source address */
 	for (i = 6; i < 12; i++)
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
index e97074cd5800..5a36bd2c7837 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
@@ -91,7 +91,9 @@ static int socfpga_dwmac_parse_data(struct socfpga_dwmac *dwmac, struct device *
 						  STMMAC_RESOURCE_NAME);
 	if (IS_ERR(dwmac->stmmac_rst)) {
 		dev_info(dev, "Could not get reset control!\n");
-		return -EINVAL;
+		if (PTR_ERR(dwmac->stmmac_rst) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		dwmac->stmmac_rst = NULL;
 	}
 
 	dwmac->interface = of_get_phy_mode(np);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index c0a391983372..2ac9552d1fa3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -97,6 +97,7 @@ struct stmmac_priv {
 	int wolopts;
 	int wol_irq;
 	struct clk *stmmac_clk;
+	struct clk *pclk;
 	struct reset_control *stmmac_rst;
 	int clk_csr;
 	struct timer_list eee_ctrl_timer;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index a0ea84fe6519..06103cad7c77 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -609,7 +609,7 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 		 * where, freq_div_ratio = clk_ptp_ref_i/50MHz
 		 * hence, addend = ((2^32) * 50MHz)/clk_ptp_ref_i;
 		 * NOTE: clk_ptp_ref_i should be >= 50MHz to
-		 *       achive 20ns accuracy.
+		 *       achieve 20ns accuracy.
 		 *
 		 * 2^x * y == (y << x), hence
 		 * 2^32 * 50000000 ==> (50000000 << 32)
@@ -2849,6 +2849,16 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	}
 	clk_prepare_enable(priv->stmmac_clk);
 
+	priv->pclk = devm_clk_get(priv->device, "pclk");
+	if (IS_ERR(priv->pclk)) {
+		if (PTR_ERR(priv->pclk) == -EPROBE_DEFER) {
+			ret = -EPROBE_DEFER;
+			goto error_pclk_get;
+		}
+		priv->pclk = NULL;
+	}
+	clk_prepare_enable(priv->pclk);
+
 	priv->stmmac_rst = devm_reset_control_get(priv->device,
 						  STMMAC_RESOURCE_NAME);
 	if (IS_ERR(priv->stmmac_rst)) {
@@ -2934,6 +2944,8 @@ error_mdio_register:
 error_netdev_register:
 	netif_napi_del(&priv->napi);
 error_hw_init:
+	clk_disable_unprepare(priv->pclk);
+error_pclk_get:
 	clk_disable_unprepare(priv->stmmac_clk);
 error_clk_get:
 	free_netdev(ndev);
@@ -2965,6 +2977,7 @@ int stmmac_dvr_remove(struct net_device *ndev)
 	unregister_netdev(ndev);
 	if (priv->stmmac_rst)
 		reset_control_assert(priv->stmmac_rst);
+	clk_disable_unprepare(priv->pclk);
 	clk_disable_unprepare(priv->stmmac_clk);
 	free_netdev(ndev);
 
@@ -3011,6 +3024,7 @@ int stmmac_suspend(struct net_device *ndev)
 		stmmac_set_mac(priv->ioaddr, false);
 		pinctrl_pm_select_sleep_state(priv->device);
 		/* Disable clock in case of PWM is off */
+		clk_disable(priv->pclk);
 		clk_disable(priv->stmmac_clk);
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -3051,6 +3065,7 @@ int stmmac_resume(struct net_device *ndev)
 		pinctrl_pm_select_default_state(priv->device);
 		/* enable the clk prevously disabled */
 		clk_enable(priv->stmmac_clk);
+		clk_enable(priv->pclk);
 		/* reset the phy so that it's ready */
 		if (priv->mii)
 			stmmac_mdio_reset(priv->mii);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
index c5ee79d8a8c5..170a18b61281 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
@@ -105,13 +105,12 @@ static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
  * Description: this function will read the current time from the
  * hardware clock and store it in @ts.
  */
-static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec *ts)
+static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct stmmac_priv *priv =
 	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 	unsigned long flags;
 	u64 ns;
-	u32 reminder;
 
 	spin_lock_irqsave(&priv->ptp_lock, flags);
 
@@ -119,8 +118,7 @@ static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec *ts)
 
 	spin_unlock_irqrestore(&priv->ptp_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &reminder);
-	ts->tv_nsec = reminder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
@@ -135,7 +133,7 @@ static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec *ts)
  * hardware clock.
  */
 static int stmmac_set_time(struct ptp_clock_info *ptp,
-			   const struct timespec *ts)
+			   const struct timespec64 *ts)
 {
 	struct stmmac_priv *priv =
 	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
@@ -168,8 +166,8 @@ static struct ptp_clock_info stmmac_ptp_clock_ops = {
 	.pps = 0,
 	.adjfreq = stmmac_adjust_freq,
 	.adjtime = stmmac_adjust_time,
-	.gettime = stmmac_get_time,
-	.settime = stmmac_set_time,
+	.gettime64 = stmmac_get_time,
+	.settime64 = stmmac_set_time,
 	.enable = stmmac_enable,
 };
 
diff --git a/drivers/net/ethernet/sun/sungem.c b/drivers/net/ethernet/sun/sungem.c
index fef5dec2cffe..e23a642357e7 100644
--- a/drivers/net/ethernet/sun/sungem.c
+++ b/drivers/net/ethernet/sun/sungem.c
@@ -718,7 +718,7 @@ static __inline__ void gem_post_rxds(struct gem *gp, int limit)
 	cluster_start = curr = (gp->rx_new & ~(4 - 1));
 	count = 0;
 	kick = -1;
-	wmb();
+	dma_wmb();
 	while (curr != limit) {
 		curr = NEXT_RX(curr);
 		if (++count == 4) {
@@ -1038,7 +1038,7 @@ static netdev_tx_t gem_start_xmit(struct sk_buff *skb,
 		if (gem_intme(entry))
 			ctrl |= TXDCTRL_INTME;
 		txd->buffer = cpu_to_le64(mapping);
-		wmb();
+		dma_wmb();
 		txd->control_word = cpu_to_le64(ctrl);
 		entry = NEXT_TX(entry);
 	} else {
@@ -1076,7 +1076,7 @@ static netdev_tx_t gem_start_xmit(struct sk_buff *skb,
 
 			txd = &gp->init_block->txd[entry];
 			txd->buffer = cpu_to_le64(mapping);
-			wmb();
+			dma_wmb();
 			txd->control_word = cpu_to_le64(this_ctrl | len);
 
 			if (gem_intme(entry))
@@ -1086,7 +1086,7 @@ static netdev_tx_t gem_start_xmit(struct sk_buff *skb,
 		}
 		txd = &gp->init_block->txd[first_entry];
 		txd->buffer = cpu_to_le64(first_mapping);
-		wmb();
+		dma_wmb();
 		txd->control_word =
 			cpu_to_le64(ctrl | TXDCTRL_SOF | intme | first_len);
 	}
@@ -1585,7 +1585,7 @@ static void gem_clean_rings(struct gem *gp)
 			gp->rx_skbs[i] = NULL;
 		}
 		rxd->status_word = 0;
-		wmb();
+		dma_wmb();
 		rxd->buffer = 0;
 	}
 
@@ -1647,7 +1647,7 @@ static void gem_init_rings(struct gem *gp)
 					RX_BUF_ALLOC_SIZE(gp),
 					PCI_DMA_FROMDEVICE);
 		rxd->buffer = cpu_to_le64(dma_addr);
-		wmb();
+		dma_wmb();
 		rxd->status_word = cpu_to_le64(RXDCTRL_FRESH(gp));
 		skb_reserve(skb, RX_OFFSET);
 	}
@@ -1656,7 +1656,7 @@ static void gem_init_rings(struct gem *gp)
 		struct gem_txd *txd = &gb->txd[i];
 
 		txd->control_word = 0;
-		wmb();
+		dma_wmb();
 		txd->buffer = 0;
 	}
 	wmb();
@@ -2175,7 +2175,7 @@ static int gem_do_start(struct net_device *dev)
 	}
 
 	/* Mark us as attached again if we come from resume(), this has
-	 * no effect if we weren't detatched and needs to be done now.
+	 * no effect if we weren't detached and needs to be done now.
 	 */
 	netif_device_attach(dev);
 
@@ -2794,7 +2794,7 @@ static void gem_remove_one(struct pci_dev *pdev)
 
 		unregister_netdev(dev);
 
-		/* Ensure reset task is truely gone */
+		/* Ensure reset task is truly gone */
 		cancel_work_sync(&gp->reset_task);
 
 		/* Free resources */
diff --git a/drivers/net/ethernet/sun/sunhme.c b/drivers/net/ethernet/sun/sunhme.c
index 7a8ca2c7b7df..cf4dcff051d5 100644
--- a/drivers/net/ethernet/sun/sunhme.c
+++ b/drivers/net/ethernet/sun/sunhme.c
@@ -196,14 +196,14 @@ static u32 sbus_hme_read32(void __iomem *reg)
 static void sbus_hme_write_rxd(struct happy_meal_rxd *rxd, u32 flags, u32 addr)
 {
 	rxd->rx_addr = (__force hme32)addr;
-	wmb();
+	dma_wmb();
 	rxd->rx_flags = (__force hme32)flags;
 }
 
 static void sbus_hme_write_txd(struct happy_meal_txd *txd, u32 flags, u32 addr)
 {
 	txd->tx_addr = (__force hme32)addr;
-	wmb();
+	dma_wmb();
 	txd->tx_flags = (__force hme32)flags;
 }
 
@@ -225,14 +225,14 @@ static u32 pci_hme_read32(void __iomem *reg)
 static void pci_hme_write_rxd(struct happy_meal_rxd *rxd, u32 flags, u32 addr)
 {
 	rxd->rx_addr = (__force hme32)cpu_to_le32(addr);
-	wmb();
+	dma_wmb();
 	rxd->rx_flags = (__force hme32)cpu_to_le32(flags);
 }
 
 static void pci_hme_write_txd(struct happy_meal_txd *txd, u32 flags, u32 addr)
 {
 	txd->tx_addr = (__force hme32)cpu_to_le32(addr);
-	wmb();
+	dma_wmb();
 	txd->tx_flags = (__force hme32)cpu_to_le32(flags);
 }
 
@@ -268,12 +268,12 @@ static u32 pci_hme_read_desc32(hme32 *p)
 	sbus_readl(__reg)
 #define hme_write_rxd(__hp, __rxd, __flags, __addr) \
 do {	(__rxd)->rx_addr = (__force hme32)(u32)(__addr); \
-	wmb(); \
+	dma_wmb(); \
 	(__rxd)->rx_flags = (__force hme32)(u32)(__flags); \
 } while(0)
 #define hme_write_txd(__hp, __txd, __flags, __addr) \
 do {	(__txd)->tx_addr = (__force hme32)(u32)(__addr); \
-	wmb(); \
+	dma_wmb(); \
 	(__txd)->tx_flags = (__force hme32)(u32)(__flags); \
 } while(0)
 #define hme_read_desc32(__hp, __p)	((__force u32)(hme32)*(__p))
@@ -293,12 +293,12 @@ do {	(__txd)->tx_addr = (__force hme32)(u32)(__addr); \
 	readl(__reg)
 #define hme_write_rxd(__hp, __rxd, __flags, __addr) \
 do {	(__rxd)->rx_addr = (__force hme32)cpu_to_le32(__addr); \
-	wmb(); \
+	dma_wmb(); \
 	(__rxd)->rx_flags = (__force hme32)cpu_to_le32(__flags); \
 } while(0)
 #define hme_write_txd(__hp, __txd, __flags, __addr) \
 do {	(__txd)->tx_addr = (__force hme32)cpu_to_le32(__addr); \
-	wmb(); \
+	dma_wmb(); \
 	(__txd)->tx_flags = (__force hme32)cpu_to_le32(__flags); \
 } while(0)
 static inline u32 hme_read_desc32(struct happy_meal *hp, hme32 *p)
diff --git a/drivers/net/ethernet/sun/sunvnet.c b/drivers/net/ethernet/sun/sunvnet.c
index 22e0cad1b4b5..53fe200e0b79 100644
--- a/drivers/net/ethernet/sun/sunvnet.c
+++ b/drivers/net/ethernet/sun/sunvnet.c
@@ -519,7 +519,7 @@ static int vnet_walk_rx_one(struct vnet_port *port,
 	if (desc->hdr.state != VIO_DESC_READY)
 		return 1;
 
-	rmb();
+	dma_rmb();
 
 	viodbg(DATA, "vio_walk_rx_one desc[%02x:%02x:%08x:%08x:%llx:%llx]\n",
 	       desc->hdr.state, desc->hdr.ack,
@@ -1380,7 +1380,7 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* This has to be a non-SMP write barrier because we are writing
 	 * to memory which is shared with the peer LDOM.
 	 */
-	wmb();
+	dma_wmb();
 
 	d->hdr.state = VIO_DESC_READY;
 
@@ -1395,7 +1395,7 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 * is marked READY, but start_cons was false.
 	 * If so, vnet_ack() should send out the missed "start" trigger.
 	 *
-	 * Note that the wmb() above makes sure the cookies et al. are
+	 * Note that the dma_wmb() above makes sure the cookies et al. are
 	 * not globally visible before the VIO_DESC_READY, and that the
 	 * stores are ordered correctly by the compiler. The consumer will
 	 * not proceed until the VIO_DESC_READY is visible assuring that
@@ -1411,6 +1411,8 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (unlikely(err < 0)) {
 		netdev_info(dev, "TX trigger error %d\n", err);
 		d->hdr.state = VIO_DESC_FREE;
+		skb = port->tx_bufs[txi].skb;
+		port->tx_bufs[txi].skb = NULL;
 		dev->stats.tx_carrier_errors++;
 		goto out_dropped;
 	}
diff --git a/drivers/net/ethernet/ti/Kconfig b/drivers/net/ethernet/ti/Kconfig
index f6a71092e135..631e0afd07d2 100644
--- a/drivers/net/ethernet/ti/Kconfig
+++ b/drivers/net/ethernet/ti/Kconfig
@@ -88,6 +88,7 @@ config TI_CPTS
 config TI_KEYSTONE_NETCP
 	tristate "TI Keystone NETCP Core Support"
 	select TI_CPSW_ALE
+	select TI_DAVINCI_MDIO
 	depends on OF
 	depends on KEYSTONE_NAVIGATOR_DMA && KEYSTONE_NAVIGATOR_QMSS
 	---help---
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index a1bbaf6352ba..b536b4c82752 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -726,7 +726,7 @@ static void cpsw_rx_handler(void *token, int len, int status)
 		if (ndev_status && (status >= 0)) {
 			/* The packet received is for the interface which
 			 * is already down and the other interface is up
-			 * and running, intead of freeing which results
+			 * and running, instead of freeing which results
 			 * in reducing of the number of rx descriptor in
 			 * DMA engine, requeue skb back to cpdma.
 			 */
diff --git a/drivers/net/ethernet/ti/cpts.c b/drivers/net/ethernet/ti/cpts.c
index fbe42cb107ec..85a55b4ff8c0 100644
--- a/drivers/net/ethernet/ti/cpts.c
+++ b/drivers/net/ethernet/ti/cpts.c
@@ -167,10 +167,9 @@ static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	u64 ns;
-	u32 remainder;
 	unsigned long flags;
 	struct cpts *cpts = container_of(ptp, struct cpts, info);
 
@@ -178,21 +177,19 @@ static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 	ns = timecounter_read(&cpts->tc);
 	spin_unlock_irqrestore(&cpts->lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
 
 static int cpts_ptp_settime(struct ptp_clock_info *ptp,
-			    const struct timespec *ts)
+			    const struct timespec64 *ts)
 {
 	u64 ns;
 	unsigned long flags;
 	struct cpts *cpts = container_of(ptp, struct cpts, info);
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 
 	spin_lock_irqsave(&cpts->lock, flags);
 	timecounter_init(&cpts->tc, &cpts->cc, ns);
@@ -216,20 +213,20 @@ static struct ptp_clock_info cpts_info = {
 	.pps		= 0,
 	.adjfreq	= cpts_ptp_adjfreq,
 	.adjtime	= cpts_ptp_adjtime,
-	.gettime	= cpts_ptp_gettime,
-	.settime	= cpts_ptp_settime,
+	.gettime64	= cpts_ptp_gettime,
+	.settime64	= cpts_ptp_settime,
 	.enable		= cpts_ptp_enable,
 };
 
 static void cpts_overflow_check(struct work_struct *work)
 {
-	struct timespec ts;
+	struct timespec64 ts;
 	struct cpts *cpts = container_of(work, struct cpts, overflow_work.work);
 
 	cpts_write32(cpts, CPTS_EN, control);
 	cpts_write32(cpts, TS_PEND_EN, int_enable);
 	cpts_ptp_gettime(&cpts->info, &ts);
-	pr_debug("cpts overflow check at %ld.%09lu\n", ts.tv_sec, ts.tv_nsec);
+	pr_debug("cpts overflow check at %lld.%09lu\n", ts.tv_sec, ts.tv_nsec);
 	schedule_delayed_work(&cpts->overflow_work, CPTS_OVERFLOW_PERIOD);
 }
 
diff --git a/drivers/net/ethernet/ti/netcp.h b/drivers/net/ethernet/ti/netcp.h
index 906e9bc412f5..bbacf5cccec2 100644
--- a/drivers/net/ethernet/ti/netcp.h
+++ b/drivers/net/ethernet/ti/netcp.h
@@ -41,7 +41,10 @@ struct netcp_tx_pipe {
 	struct netcp_device	*netcp_device;
 	void			*dma_queue;
 	unsigned int		dma_queue_id;
-	u8			dma_psflags;
+	/* To port for packet forwarded to switch. Used only by ethss */
+	u8			switch_to_port;
+#define	SWITCH_TO_PORT_IN_TAGINFO	BIT(0)
+	u8			flags;
 	void			*dma_channel;
 	const char		*dma_chan_name;
 };
diff --git a/drivers/net/ethernet/ti/netcp_core.c b/drivers/net/ethernet/ti/netcp_core.c
index a31a8c3c8e7c..43efc3a0cda5 100644
--- a/drivers/net/ethernet/ti/netcp_core.c
+++ b/drivers/net/ethernet/ti/netcp_core.c
@@ -1098,9 +1098,9 @@ static int netcp_tx_submit_skb(struct netcp_intf *netcp,
 	struct netcp_tx_pipe *tx_pipe = NULL;
 	struct netcp_hook_list *tx_hook;
 	struct netcp_packet p_info;
-	u32 packet_info = 0;
 	unsigned int dma_sz;
 	dma_addr_t dma;
+	u32 tmp = 0;
 	int ret = 0;
 
 	p_info.netcp = netcp;
@@ -1140,20 +1140,27 @@ static int netcp_tx_submit_skb(struct netcp_intf *netcp,
 		memmove(p_info.psdata, p_info.psdata + p_info.psdata_len,
 			p_info.psdata_len);
 		set_words(psdata, p_info.psdata_len, psdata);
-		packet_info |=
-			(p_info.psdata_len & KNAV_DMA_DESC_PSLEN_MASK) <<
+		tmp |= (p_info.psdata_len & KNAV_DMA_DESC_PSLEN_MASK) <<
 			KNAV_DMA_DESC_PSLEN_SHIFT;
 	}
 
-	packet_info |= KNAV_DMA_DESC_HAS_EPIB |
+	tmp |= KNAV_DMA_DESC_HAS_EPIB |
 		((netcp->tx_compl_qid & KNAV_DMA_DESC_RETQ_MASK) <<
-		KNAV_DMA_DESC_RETQ_SHIFT) |
-		((tx_pipe->dma_psflags & KNAV_DMA_DESC_PSFLAG_MASK) <<
-		KNAV_DMA_DESC_PSFLAG_SHIFT);
+		KNAV_DMA_DESC_RETQ_SHIFT);
 
-	set_words(&packet_info, 1, &desc->packet_info);
+	if (!(tx_pipe->flags & SWITCH_TO_PORT_IN_TAGINFO)) {
+		tmp |= ((tx_pipe->switch_to_port & KNAV_DMA_DESC_PSFLAG_MASK) <<
+			KNAV_DMA_DESC_PSFLAG_SHIFT);
+	}
+
+	set_words(&tmp, 1, &desc->packet_info);
 	set_words((u32 *)&skb, 1, &desc->pad[0]);
 
+	if (tx_pipe->flags & SWITCH_TO_PORT_IN_TAGINFO) {
+		tmp = tx_pipe->switch_to_port;
+		set_words((u32 *)&tmp, 1, &desc->tag_info);
+	}
+
 	/* submit packet descriptor */
 	ret = knav_pool_desc_map(netcp->tx_pool, desc, sizeof(*desc), &dma,
 				 &dma_sz);
@@ -1320,7 +1327,7 @@ static struct netcp_addr *netcp_addr_add(struct netcp_intf *netcp,
 	if (addr)
 		ether_addr_copy(naddr->addr, addr);
 	else
-		memset(naddr->addr, 0, ETH_ALEN);
+		eth_zero_addr(naddr->addr);
 	list_add_tail(&naddr->node, &netcp->addr_list);
 
 	return naddr;
@@ -2127,7 +2134,7 @@ static int netcp_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id of_match[] = {
+static const struct of_device_id of_match[] = {
 	{ .compatible = "ti,netcp-1.0", },
 	{},
 };
diff --git a/drivers/net/ethernet/ti/netcp_ethss.c b/drivers/net/ethernet/ti/netcp_ethss.c
index 84f5ce525750..2bef655279f3 100644
--- a/drivers/net/ethernet/ti/netcp_ethss.c
+++ b/drivers/net/ethernet/ti/netcp_ethss.c
@@ -40,35 +40,61 @@
 #define GBE_MODULE_NAME			"netcp-gbe"
 #define GBE_SS_VERSION_14		0x4ed21104
 
+#define GBE_SS_REG_INDEX		0
+#define GBE_SGMII34_REG_INDEX		1
+#define GBE_SM_REG_INDEX		2
+/* offset relative to base of GBE_SS_REG_INDEX */
 #define GBE13_SGMII_MODULE_OFFSET	0x100
-#define GBE13_SGMII34_MODULE_OFFSET	0x400
-#define GBE13_SWITCH_MODULE_OFFSET	0x800
-#define GBE13_HOST_PORT_OFFSET		0x834
-#define GBE13_SLAVE_PORT_OFFSET		0x860
-#define GBE13_EMAC_OFFSET		0x900
-#define GBE13_SLAVE_PORT2_OFFSET	0xa00
-#define GBE13_HW_STATS_OFFSET		0xb00
-#define GBE13_ALE_OFFSET		0xe00
+/* offset relative to base of GBE_SM_REG_INDEX */
+#define GBE13_HOST_PORT_OFFSET		0x34
+#define GBE13_SLAVE_PORT_OFFSET		0x60
+#define GBE13_EMAC_OFFSET		0x100
+#define GBE13_SLAVE_PORT2_OFFSET	0x200
+#define GBE13_HW_STATS_OFFSET		0x300
+#define GBE13_ALE_OFFSET		0x600
 #define GBE13_HOST_PORT_NUM		0
-#define GBE13_NUM_SLAVES		4
-#define GBE13_NUM_ALE_PORTS		(GBE13_NUM_SLAVES + 1)
 #define GBE13_NUM_ALE_ENTRIES		1024
 
+/* 1G Ethernet NU SS defines */
+#define GBENU_MODULE_NAME		"netcp-gbenu"
+#define GBE_SS_ID_NU			0x4ee6
+#define GBE_SS_ID_2U			0x4ee8
+
+#define IS_SS_ID_MU(d) \
+	((GBE_IDENT((d)->ss_version) == GBE_SS_ID_NU) || \
+	 (GBE_IDENT((d)->ss_version) == GBE_SS_ID_2U))
+
+#define IS_SS_ID_NU(d) \
+	(GBE_IDENT((d)->ss_version) == GBE_SS_ID_NU)
+
+#define GBENU_SS_REG_INDEX		0
+#define GBENU_SM_REG_INDEX		1
+#define GBENU_SGMII_MODULE_OFFSET	0x100
+#define GBENU_HOST_PORT_OFFSET		0x1000
+#define GBENU_SLAVE_PORT_OFFSET		0x2000
+#define GBENU_EMAC_OFFSET		0x2330
+#define GBENU_HW_STATS_OFFSET		0x1a000
+#define GBENU_ALE_OFFSET		0x1e000
+#define GBENU_HOST_PORT_NUM		0
+#define GBENU_NUM_ALE_ENTRIES		1024
+
 /* 10G Ethernet SS defines */
 #define XGBE_MODULE_NAME		"netcp-xgbe"
 #define XGBE_SS_VERSION_10		0x4ee42100
 
-#define XGBE_SERDES_REG_INDEX		1
+#define XGBE_SS_REG_INDEX		0
+#define XGBE_SM_REG_INDEX		1
+#define XGBE_SERDES_REG_INDEX		2
+
+/* offset relative to base of XGBE_SS_REG_INDEX */
 #define XGBE10_SGMII_MODULE_OFFSET	0x100
-#define XGBE10_SWITCH_MODULE_OFFSET	0x1000
-#define XGBE10_HOST_PORT_OFFSET		0x1034
-#define XGBE10_SLAVE_PORT_OFFSET	0x1064
-#define XGBE10_EMAC_OFFSET		0x1400
-#define XGBE10_ALE_OFFSET		0x1700
-#define XGBE10_HW_STATS_OFFSET		0x1800
+/* offset relative to base of XGBE_SM_REG_INDEX */
+#define XGBE10_HOST_PORT_OFFSET		0x34
+#define XGBE10_SLAVE_PORT_OFFSET	0x64
+#define XGBE10_EMAC_OFFSET		0x400
+#define XGBE10_ALE_OFFSET		0x700
+#define XGBE10_HW_STATS_OFFSET		0x800
 #define XGBE10_HOST_PORT_NUM		0
-#define XGBE10_NUM_SLAVES		2
-#define XGBE10_NUM_ALE_PORTS		(XGBE10_NUM_SLAVES + 1)
 #define XGBE10_NUM_ALE_ENTRIES		1024
 
 #define	GBE_TIMER_INTERVAL			(HZ / 2)
@@ -88,7 +114,7 @@
 #define MACSL_FULLDUPLEX			BIT(0)
 
 #define GBE_CTL_P0_ENABLE			BIT(2)
-#define GBE_REG_VAL_STAT_ENABLE_ALL		0xff
+#define GBE13_REG_VAL_STAT_ENABLE_ALL		0xff
 #define XGBE_REG_VAL_STAT_ENABLE_ALL		0xf
 #define GBE_STATS_CD_SEL			BIT(28)
 
@@ -108,11 +134,20 @@
 #define GBE_STATSC_MODULE			2
 #define GBE_STATSD_MODULE			3
 
+#define GBENU_STATS0_MODULE			0
+#define GBENU_STATS1_MODULE			1
+#define GBENU_STATS2_MODULE			2
+#define GBENU_STATS3_MODULE			3
+#define GBENU_STATS4_MODULE			4
+#define GBENU_STATS5_MODULE			5
+#define GBENU_STATS6_MODULE			6
+#define GBENU_STATS7_MODULE			7
+#define GBENU_STATS8_MODULE			8
+
 #define XGBE_STATS0_MODULE			0
 #define XGBE_STATS1_MODULE			1
 #define XGBE_STATS2_MODULE			2
 
-#define MAX_SLAVES				GBE13_NUM_SLAVES
 /* s: 0-based slave_port */
 #define SGMII_BASE(s) \
 	(((s) < 2) ? gbe_dev->sgmii_port_regs : gbe_dev->sgmii_port34_regs)
@@ -125,10 +160,14 @@
 
 #define GBE_SET_REG_OFS(p, rb, rn) p->rb##_ofs.rn = \
 		offsetof(struct gbe##_##rb, rn)
+#define GBENU_SET_REG_OFS(p, rb, rn) p->rb##_ofs.rn = \
+		offsetof(struct gbenu##_##rb, rn)
 #define XGBE_SET_REG_OFS(p, rb, rn) p->rb##_ofs.rn = \
 		offsetof(struct xgbe##_##rb, rn)
 #define GBE_REG_ADDR(p, rb, rn) (p->rb + p->rb##_ofs.rn)
 
+#define HOST_TX_PRI_MAP_DEFAULT			0x00000000
+
 struct xgbe_ss_regs {
 	u32	id_ver;
 	u32	synce_count;
@@ -258,6 +297,192 @@ struct xgbe_hw_stats {
 
 #define XGBE10_NUM_STAT_ENTRIES (sizeof(struct xgbe_hw_stats)/sizeof(u32))
 
+struct gbenu_ss_regs {
+	u32	id_ver;
+	u32	synce_count;		/* NU */
+	u32	synce_mux;		/* NU */
+	u32	control;		/* 2U */
+	u32	__rsvd_0[2];		/* 2U */
+	u32	rgmii_status;		/* 2U */
+	u32	ss_status;		/* 2U */
+};
+
+struct gbenu_switch_regs {
+	u32	id_ver;
+	u32	control;
+	u32	__rsvd_0[2];
+	u32	emcontrol;
+	u32	stat_port_en;
+	u32	ptype;			/* NU */
+	u32	soft_idle;
+	u32	thru_rate;		/* NU */
+	u32	gap_thresh;		/* NU */
+	u32	tx_start_wds;		/* NU */
+	u32	eee_prescale;		/* 2U */
+	u32	tx_g_oflow_thresh_set;	/* NU */
+	u32	tx_g_oflow_thresh_clr;	/* NU */
+	u32	tx_g_buf_thresh_set_l;	/* NU */
+	u32	tx_g_buf_thresh_set_h;	/* NU */
+	u32	tx_g_buf_thresh_clr_l;	/* NU */
+	u32	tx_g_buf_thresh_clr_h;	/* NU */
+};
+
+struct gbenu_port_regs {
+	u32	__rsvd_0;
+	u32	control;
+	u32	max_blks;		/* 2U */
+	u32	mem_align1;
+	u32	blk_cnt;
+	u32	port_vlan;
+	u32	tx_pri_map;		/* NU */
+	u32	pri_ctl;		/* 2U */
+	u32	rx_pri_map;
+	u32	rx_maxlen;
+	u32	tx_blks_pri;		/* NU */
+	u32	__rsvd_1;
+	u32	idle2lpi;		/* 2U */
+	u32	lpi2idle;		/* 2U */
+	u32	eee_status;		/* 2U */
+	u32	__rsvd_2;
+	u32	__rsvd_3[176];		/* NU: more to add */
+	u32	__rsvd_4[2];
+	u32	sa_lo;
+	u32	sa_hi;
+	u32	ts_ctl;
+	u32	ts_seq_ltype;
+	u32	ts_vlan;
+	u32	ts_ctl_ltype2;
+	u32	ts_ctl2;
+};
+
+struct gbenu_host_port_regs {
+	u32	__rsvd_0;
+	u32	control;
+	u32	flow_id_offset;		/* 2U */
+	u32	__rsvd_1;
+	u32	blk_cnt;
+	u32	port_vlan;
+	u32	tx_pri_map;		/* NU */
+	u32	pri_ctl;
+	u32	rx_pri_map;
+	u32	rx_maxlen;
+	u32	tx_blks_pri;		/* NU */
+	u32	__rsvd_2;
+	u32	idle2lpi;		/* 2U */
+	u32	lpi2wake;		/* 2U */
+	u32	eee_status;		/* 2U */
+	u32	__rsvd_3;
+	u32	__rsvd_4[184];		/* NU */
+	u32	host_blks_pri;		/* NU */
+};
+
+struct gbenu_emac_regs {
+	u32	mac_control;
+	u32	mac_status;
+	u32	soft_reset;
+	u32	boff_test;
+	u32	rx_pause;
+	u32	__rsvd_0[11];		/* NU */
+	u32	tx_pause;
+	u32	__rsvd_1[11];		/* NU */
+	u32	em_control;
+	u32	tx_gap;
+};
+
+/* Some hw stat regs are applicable to slave port only.
+ * This is handled by gbenu_et_stats struct.  Also some
+ * are for SS version NU and some are for 2U.
+ */
+struct gbenu_hw_stats {
+	u32	rx_good_frames;
+	u32	rx_broadcast_frames;
+	u32	rx_multicast_frames;
+	u32	rx_pause_frames;		/* slave */
+	u32	rx_crc_errors;
+	u32	rx_align_code_errors;		/* slave */
+	u32	rx_oversized_frames;
+	u32	rx_jabber_frames;		/* slave */
+	u32	rx_undersized_frames;
+	u32	rx_fragments;			/* slave */
+	u32	ale_drop;
+	u32	ale_overrun_drop;
+	u32	rx_bytes;
+	u32	tx_good_frames;
+	u32	tx_broadcast_frames;
+	u32	tx_multicast_frames;
+	u32	tx_pause_frames;		/* slave */
+	u32	tx_deferred_frames;		/* slave */
+	u32	tx_collision_frames;		/* slave */
+	u32	tx_single_coll_frames;		/* slave */
+	u32	tx_mult_coll_frames;		/* slave */
+	u32	tx_excessive_collisions;	/* slave */
+	u32	tx_late_collisions;		/* slave */
+	u32	rx_ipg_error;			/* slave 10G only */
+	u32	tx_carrier_sense_errors;	/* slave */
+	u32	tx_bytes;
+	u32	tx_64B_frames;
+	u32	tx_65_to_127B_frames;
+	u32	tx_128_to_255B_frames;
+	u32	tx_256_to_511B_frames;
+	u32	tx_512_to_1023B_frames;
+	u32	tx_1024B_frames;
+	u32	net_bytes;
+	u32	rx_bottom_fifo_drop;
+	u32	rx_port_mask_drop;
+	u32	rx_top_fifo_drop;
+	u32	ale_rate_limit_drop;
+	u32	ale_vid_ingress_drop;
+	u32	ale_da_eq_sa_drop;
+	u32	__rsvd_0[3];
+	u32	ale_unknown_ucast;
+	u32	ale_unknown_ucast_bytes;
+	u32	ale_unknown_mcast;
+	u32	ale_unknown_mcast_bytes;
+	u32	ale_unknown_bcast;
+	u32	ale_unknown_bcast_bytes;
+	u32	ale_pol_match;
+	u32	ale_pol_match_red;		/* NU */
+	u32	ale_pol_match_yellow;		/* NU */
+	u32	__rsvd_1[44];
+	u32	tx_mem_protect_err;
+	/* following NU only */
+	u32	tx_pri0;
+	u32	tx_pri1;
+	u32	tx_pri2;
+	u32	tx_pri3;
+	u32	tx_pri4;
+	u32	tx_pri5;
+	u32	tx_pri6;
+	u32	tx_pri7;
+	u32	tx_pri0_bcnt;
+	u32	tx_pri1_bcnt;
+	u32	tx_pri2_bcnt;
+	u32	tx_pri3_bcnt;
+	u32	tx_pri4_bcnt;
+	u32	tx_pri5_bcnt;
+	u32	tx_pri6_bcnt;
+	u32	tx_pri7_bcnt;
+	u32	tx_pri0_drop;
+	u32	tx_pri1_drop;
+	u32	tx_pri2_drop;
+	u32	tx_pri3_drop;
+	u32	tx_pri4_drop;
+	u32	tx_pri5_drop;
+	u32	tx_pri6_drop;
+	u32	tx_pri7_drop;
+	u32	tx_pri0_drop_bcnt;
+	u32	tx_pri1_drop_bcnt;
+	u32	tx_pri2_drop_bcnt;
+	u32	tx_pri3_drop_bcnt;
+	u32	tx_pri4_drop_bcnt;
+	u32	tx_pri5_drop_bcnt;
+	u32	tx_pri6_drop_bcnt;
+	u32	tx_pri7_drop_bcnt;
+};
+
+#define GBENU_NUM_HW_STAT_ENTRIES (sizeof(struct gbenu_hw_stats) / sizeof(u32))
+#define GBENU_HW_STATS_REG_MAP_SZ	0x200
+
 struct gbe_ss_regs {
 	u32	id_ver;
 	u32	synce_count;
@@ -316,6 +541,7 @@ struct gbe_port_regs_ofs {
 	u16	ts_vlan;
 	u16	ts_ctl_ltype2;
 	u16	ts_ctl2;
+	u16	rx_maxlen;	/* 2U, NU */
 };
 
 struct gbe_host_port_regs {
@@ -390,9 +616,7 @@ struct gbe_hw_stats {
 };
 
 #define GBE13_NUM_HW_STAT_ENTRIES (sizeof(struct gbe_hw_stats)/sizeof(u32))
-#define GBE13_NUM_HW_STATS_MOD			2
-#define XGBE10_NUM_HW_STATS_MOD			3
-#define GBE_MAX_HW_STAT_MODS			3
+#define GBE_MAX_HW_STAT_MODS			9
 #define GBE_HW_STATS_REG_MAP_SZ			0x100
 
 struct gbe_slave {
@@ -420,11 +644,14 @@ struct gbe_priv {
 	u32				ale_entries;
 	u32				ale_ports;
 	bool				enable_ale;
+	u8				max_num_slaves;
+	u8				max_num_ports; /* max_num_slaves + 1 */
 	struct netcp_tx_pipe		tx_pipe;
 
 	int				host_port;
 	u32				rx_packet_max;
 	u32				ss_version;
+	u32				stats_en_mask;
 
 	void __iomem			*ss_regs;
 	void __iomem			*switch_regs;
@@ -475,275 +702,778 @@ struct netcp_ethtool_stat {
 	int offset;
 };
 
-#define GBE_STATSA_INFO(field)		"GBE_A:"#field, GBE_STATSA_MODULE,\
-				FIELD_SIZEOF(struct gbe_hw_stats, field), \
-				offsetof(struct gbe_hw_stats, field)
+#define GBE_STATSA_INFO(field)						\
+{									\
+	"GBE_A:"#field, GBE_STATSA_MODULE,				\
+	FIELD_SIZEOF(struct gbe_hw_stats, field),			\
+	offsetof(struct gbe_hw_stats, field)				\
+}
 
-#define GBE_STATSB_INFO(field)		"GBE_B:"#field, GBE_STATSB_MODULE,\
-				FIELD_SIZEOF(struct gbe_hw_stats, field), \
-				offsetof(struct gbe_hw_stats, field)
+#define GBE_STATSB_INFO(field)						\
+{									\
+	"GBE_B:"#field, GBE_STATSB_MODULE,				\
+	FIELD_SIZEOF(struct gbe_hw_stats, field),			\
+	offsetof(struct gbe_hw_stats, field)				\
+}
 
-#define GBE_STATSC_INFO(field)		"GBE_C:"#field, GBE_STATSC_MODULE,\
-				FIELD_SIZEOF(struct gbe_hw_stats, field), \
-				offsetof(struct gbe_hw_stats, field)
+#define GBE_STATSC_INFO(field)						\
+{									\
+	"GBE_C:"#field, GBE_STATSC_MODULE,				\
+	FIELD_SIZEOF(struct gbe_hw_stats, field),			\
+	offsetof(struct gbe_hw_stats, field)				\
+}
 
-#define GBE_STATSD_INFO(field)		"GBE_D:"#field, GBE_STATSD_MODULE,\
-				FIELD_SIZEOF(struct gbe_hw_stats, field), \
-				offsetof(struct gbe_hw_stats, field)
+#define GBE_STATSD_INFO(field)						\
+{									\
+	"GBE_D:"#field, GBE_STATSD_MODULE,				\
+	FIELD_SIZEOF(struct gbe_hw_stats, field),			\
+	offsetof(struct gbe_hw_stats, field)				\
+}
 
 static const struct netcp_ethtool_stat gbe13_et_stats[] = {
 	/* GBE module A */
-	{GBE_STATSA_INFO(rx_good_frames)},
-	{GBE_STATSA_INFO(rx_broadcast_frames)},
-	{GBE_STATSA_INFO(rx_multicast_frames)},
-	{GBE_STATSA_INFO(rx_pause_frames)},
-	{GBE_STATSA_INFO(rx_crc_errors)},
-	{GBE_STATSA_INFO(rx_align_code_errors)},
-	{GBE_STATSA_INFO(rx_oversized_frames)},
-	{GBE_STATSA_INFO(rx_jabber_frames)},
-	{GBE_STATSA_INFO(rx_undersized_frames)},
-	{GBE_STATSA_INFO(rx_fragments)},
-	{GBE_STATSA_INFO(rx_bytes)},
-	{GBE_STATSA_INFO(tx_good_frames)},
-	{GBE_STATSA_INFO(tx_broadcast_frames)},
-	{GBE_STATSA_INFO(tx_multicast_frames)},
-	{GBE_STATSA_INFO(tx_pause_frames)},
-	{GBE_STATSA_INFO(tx_deferred_frames)},
-	{GBE_STATSA_INFO(tx_collision_frames)},
-	{GBE_STATSA_INFO(tx_single_coll_frames)},
-	{GBE_STATSA_INFO(tx_mult_coll_frames)},
-	{GBE_STATSA_INFO(tx_excessive_collisions)},
-	{GBE_STATSA_INFO(tx_late_collisions)},
-	{GBE_STATSA_INFO(tx_underrun)},
-	{GBE_STATSA_INFO(tx_carrier_sense_errors)},
-	{GBE_STATSA_INFO(tx_bytes)},
-	{GBE_STATSA_INFO(tx_64byte_frames)},
-	{GBE_STATSA_INFO(tx_65_to_127byte_frames)},
-	{GBE_STATSA_INFO(tx_128_to_255byte_frames)},
-	{GBE_STATSA_INFO(tx_256_to_511byte_frames)},
-	{GBE_STATSA_INFO(tx_512_to_1023byte_frames)},
-	{GBE_STATSA_INFO(tx_1024byte_frames)},
-	{GBE_STATSA_INFO(net_bytes)},
-	{GBE_STATSA_INFO(rx_sof_overruns)},
-	{GBE_STATSA_INFO(rx_mof_overruns)},
-	{GBE_STATSA_INFO(rx_dma_overruns)},
+	GBE_STATSA_INFO(rx_good_frames),
+	GBE_STATSA_INFO(rx_broadcast_frames),
+	GBE_STATSA_INFO(rx_multicast_frames),
+	GBE_STATSA_INFO(rx_pause_frames),
+	GBE_STATSA_INFO(rx_crc_errors),
+	GBE_STATSA_INFO(rx_align_code_errors),
+	GBE_STATSA_INFO(rx_oversized_frames),
+	GBE_STATSA_INFO(rx_jabber_frames),
+	GBE_STATSA_INFO(rx_undersized_frames),
+	GBE_STATSA_INFO(rx_fragments),
+	GBE_STATSA_INFO(rx_bytes),
+	GBE_STATSA_INFO(tx_good_frames),
+	GBE_STATSA_INFO(tx_broadcast_frames),
+	GBE_STATSA_INFO(tx_multicast_frames),
+	GBE_STATSA_INFO(tx_pause_frames),
+	GBE_STATSA_INFO(tx_deferred_frames),
+	GBE_STATSA_INFO(tx_collision_frames),
+	GBE_STATSA_INFO(tx_single_coll_frames),
+	GBE_STATSA_INFO(tx_mult_coll_frames),
+	GBE_STATSA_INFO(tx_excessive_collisions),
+	GBE_STATSA_INFO(tx_late_collisions),
+	GBE_STATSA_INFO(tx_underrun),
+	GBE_STATSA_INFO(tx_carrier_sense_errors),
+	GBE_STATSA_INFO(tx_bytes),
+	GBE_STATSA_INFO(tx_64byte_frames),
+	GBE_STATSA_INFO(tx_65_to_127byte_frames),
+	GBE_STATSA_INFO(tx_128_to_255byte_frames),
+	GBE_STATSA_INFO(tx_256_to_511byte_frames),
+	GBE_STATSA_INFO(tx_512_to_1023byte_frames),
+	GBE_STATSA_INFO(tx_1024byte_frames),
+	GBE_STATSA_INFO(net_bytes),
+	GBE_STATSA_INFO(rx_sof_overruns),
+	GBE_STATSA_INFO(rx_mof_overruns),
+	GBE_STATSA_INFO(rx_dma_overruns),
 	/* GBE module B */
-	{GBE_STATSB_INFO(rx_good_frames)},
-	{GBE_STATSB_INFO(rx_broadcast_frames)},
-	{GBE_STATSB_INFO(rx_multicast_frames)},
-	{GBE_STATSB_INFO(rx_pause_frames)},
-	{GBE_STATSB_INFO(rx_crc_errors)},
-	{GBE_STATSB_INFO(rx_align_code_errors)},
-	{GBE_STATSB_INFO(rx_oversized_frames)},
-	{GBE_STATSB_INFO(rx_jabber_frames)},
-	{GBE_STATSB_INFO(rx_undersized_frames)},
-	{GBE_STATSB_INFO(rx_fragments)},
-	{GBE_STATSB_INFO(rx_bytes)},
-	{GBE_STATSB_INFO(tx_good_frames)},
-	{GBE_STATSB_INFO(tx_broadcast_frames)},
-	{GBE_STATSB_INFO(tx_multicast_frames)},
-	{GBE_STATSB_INFO(tx_pause_frames)},
-	{GBE_STATSB_INFO(tx_deferred_frames)},
-	{GBE_STATSB_INFO(tx_collision_frames)},
-	{GBE_STATSB_INFO(tx_single_coll_frames)},
-	{GBE_STATSB_INFO(tx_mult_coll_frames)},
-	{GBE_STATSB_INFO(tx_excessive_collisions)},
-	{GBE_STATSB_INFO(tx_late_collisions)},
-	{GBE_STATSB_INFO(tx_underrun)},
-	{GBE_STATSB_INFO(tx_carrier_sense_errors)},
-	{GBE_STATSB_INFO(tx_bytes)},
-	{GBE_STATSB_INFO(tx_64byte_frames)},
-	{GBE_STATSB_INFO(tx_65_to_127byte_frames)},
-	{GBE_STATSB_INFO(tx_128_to_255byte_frames)},
-	{GBE_STATSB_INFO(tx_256_to_511byte_frames)},
-	{GBE_STATSB_INFO(tx_512_to_1023byte_frames)},
-	{GBE_STATSB_INFO(tx_1024byte_frames)},
-	{GBE_STATSB_INFO(net_bytes)},
-	{GBE_STATSB_INFO(rx_sof_overruns)},
-	{GBE_STATSB_INFO(rx_mof_overruns)},
-	{GBE_STATSB_INFO(rx_dma_overruns)},
+	GBE_STATSB_INFO(rx_good_frames),
+	GBE_STATSB_INFO(rx_broadcast_frames),
+	GBE_STATSB_INFO(rx_multicast_frames),
+	GBE_STATSB_INFO(rx_pause_frames),
+	GBE_STATSB_INFO(rx_crc_errors),
+	GBE_STATSB_INFO(rx_align_code_errors),
+	GBE_STATSB_INFO(rx_oversized_frames),
+	GBE_STATSB_INFO(rx_jabber_frames),
+	GBE_STATSB_INFO(rx_undersized_frames),
+	GBE_STATSB_INFO(rx_fragments),
+	GBE_STATSB_INFO(rx_bytes),
+	GBE_STATSB_INFO(tx_good_frames),
+	GBE_STATSB_INFO(tx_broadcast_frames),
+	GBE_STATSB_INFO(tx_multicast_frames),
+	GBE_STATSB_INFO(tx_pause_frames),
+	GBE_STATSB_INFO(tx_deferred_frames),
+	GBE_STATSB_INFO(tx_collision_frames),
+	GBE_STATSB_INFO(tx_single_coll_frames),
+	GBE_STATSB_INFO(tx_mult_coll_frames),
+	GBE_STATSB_INFO(tx_excessive_collisions),
+	GBE_STATSB_INFO(tx_late_collisions),
+	GBE_STATSB_INFO(tx_underrun),
+	GBE_STATSB_INFO(tx_carrier_sense_errors),
+	GBE_STATSB_INFO(tx_bytes),
+	GBE_STATSB_INFO(tx_64byte_frames),
+	GBE_STATSB_INFO(tx_65_to_127byte_frames),
+	GBE_STATSB_INFO(tx_128_to_255byte_frames),
+	GBE_STATSB_INFO(tx_256_to_511byte_frames),
+	GBE_STATSB_INFO(tx_512_to_1023byte_frames),
+	GBE_STATSB_INFO(tx_1024byte_frames),
+	GBE_STATSB_INFO(net_bytes),
+	GBE_STATSB_INFO(rx_sof_overruns),
+	GBE_STATSB_INFO(rx_mof_overruns),
+	GBE_STATSB_INFO(rx_dma_overruns),
 	/* GBE module C */
-	{GBE_STATSC_INFO(rx_good_frames)},
-	{GBE_STATSC_INFO(rx_broadcast_frames)},
-	{GBE_STATSC_INFO(rx_multicast_frames)},
-	{GBE_STATSC_INFO(rx_pause_frames)},
-	{GBE_STATSC_INFO(rx_crc_errors)},
-	{GBE_STATSC_INFO(rx_align_code_errors)},
-	{GBE_STATSC_INFO(rx_oversized_frames)},
-	{GBE_STATSC_INFO(rx_jabber_frames)},
-	{GBE_STATSC_INFO(rx_undersized_frames)},
-	{GBE_STATSC_INFO(rx_fragments)},
-	{GBE_STATSC_INFO(rx_bytes)},
-	{GBE_STATSC_INFO(tx_good_frames)},
-	{GBE_STATSC_INFO(tx_broadcast_frames)},
-	{GBE_STATSC_INFO(tx_multicast_frames)},
-	{GBE_STATSC_INFO(tx_pause_frames)},
-	{GBE_STATSC_INFO(tx_deferred_frames)},
-	{GBE_STATSC_INFO(tx_collision_frames)},
-	{GBE_STATSC_INFO(tx_single_coll_frames)},
-	{GBE_STATSC_INFO(tx_mult_coll_frames)},
-	{GBE_STATSC_INFO(tx_excessive_collisions)},
-	{GBE_STATSC_INFO(tx_late_collisions)},
-	{GBE_STATSC_INFO(tx_underrun)},
-	{GBE_STATSC_INFO(tx_carrier_sense_errors)},
-	{GBE_STATSC_INFO(tx_bytes)},
-	{GBE_STATSC_INFO(tx_64byte_frames)},
-	{GBE_STATSC_INFO(tx_65_to_127byte_frames)},
-	{GBE_STATSC_INFO(tx_128_to_255byte_frames)},
-	{GBE_STATSC_INFO(tx_256_to_511byte_frames)},
-	{GBE_STATSC_INFO(tx_512_to_1023byte_frames)},
-	{GBE_STATSC_INFO(tx_1024byte_frames)},
-	{GBE_STATSC_INFO(net_bytes)},
-	{GBE_STATSC_INFO(rx_sof_overruns)},
-	{GBE_STATSC_INFO(rx_mof_overruns)},
-	{GBE_STATSC_INFO(rx_dma_overruns)},
+	GBE_STATSC_INFO(rx_good_frames),
+	GBE_STATSC_INFO(rx_broadcast_frames),
+	GBE_STATSC_INFO(rx_multicast_frames),
+	GBE_STATSC_INFO(rx_pause_frames),
+	GBE_STATSC_INFO(rx_crc_errors),
+	GBE_STATSC_INFO(rx_align_code_errors),
+	GBE_STATSC_INFO(rx_oversized_frames),
+	GBE_STATSC_INFO(rx_jabber_frames),
+	GBE_STATSC_INFO(rx_undersized_frames),
+	GBE_STATSC_INFO(rx_fragments),
+	GBE_STATSC_INFO(rx_bytes),
+	GBE_STATSC_INFO(tx_good_frames),
+	GBE_STATSC_INFO(tx_broadcast_frames),
+	GBE_STATSC_INFO(tx_multicast_frames),
+	GBE_STATSC_INFO(tx_pause_frames),
+	GBE_STATSC_INFO(tx_deferred_frames),
+	GBE_STATSC_INFO(tx_collision_frames),
+	GBE_STATSC_INFO(tx_single_coll_frames),
+	GBE_STATSC_INFO(tx_mult_coll_frames),
+	GBE_STATSC_INFO(tx_excessive_collisions),
+	GBE_STATSC_INFO(tx_late_collisions),
+	GBE_STATSC_INFO(tx_underrun),
+	GBE_STATSC_INFO(tx_carrier_sense_errors),
+	GBE_STATSC_INFO(tx_bytes),
+	GBE_STATSC_INFO(tx_64byte_frames),
+	GBE_STATSC_INFO(tx_65_to_127byte_frames),
+	GBE_STATSC_INFO(tx_128_to_255byte_frames),
+	GBE_STATSC_INFO(tx_256_to_511byte_frames),
+	GBE_STATSC_INFO(tx_512_to_1023byte_frames),
+	GBE_STATSC_INFO(tx_1024byte_frames),
+	GBE_STATSC_INFO(net_bytes),
+	GBE_STATSC_INFO(rx_sof_overruns),
+	GBE_STATSC_INFO(rx_mof_overruns),
+	GBE_STATSC_INFO(rx_dma_overruns),
 	/* GBE module D */
-	{GBE_STATSD_INFO(rx_good_frames)},
-	{GBE_STATSD_INFO(rx_broadcast_frames)},
-	{GBE_STATSD_INFO(rx_multicast_frames)},
-	{GBE_STATSD_INFO(rx_pause_frames)},
-	{GBE_STATSD_INFO(rx_crc_errors)},
-	{GBE_STATSD_INFO(rx_align_code_errors)},
-	{GBE_STATSD_INFO(rx_oversized_frames)},
-	{GBE_STATSD_INFO(rx_jabber_frames)},
-	{GBE_STATSD_INFO(rx_undersized_frames)},
-	{GBE_STATSD_INFO(rx_fragments)},
-	{GBE_STATSD_INFO(rx_bytes)},
-	{GBE_STATSD_INFO(tx_good_frames)},
-	{GBE_STATSD_INFO(tx_broadcast_frames)},
-	{GBE_STATSD_INFO(tx_multicast_frames)},
-	{GBE_STATSD_INFO(tx_pause_frames)},
-	{GBE_STATSD_INFO(tx_deferred_frames)},
-	{GBE_STATSD_INFO(tx_collision_frames)},
-	{GBE_STATSD_INFO(tx_single_coll_frames)},
-	{GBE_STATSD_INFO(tx_mult_coll_frames)},
-	{GBE_STATSD_INFO(tx_excessive_collisions)},
-	{GBE_STATSD_INFO(tx_late_collisions)},
-	{GBE_STATSD_INFO(tx_underrun)},
-	{GBE_STATSD_INFO(tx_carrier_sense_errors)},
-	{GBE_STATSD_INFO(tx_bytes)},
-	{GBE_STATSD_INFO(tx_64byte_frames)},
-	{GBE_STATSD_INFO(tx_65_to_127byte_frames)},
-	{GBE_STATSD_INFO(tx_128_to_255byte_frames)},
-	{GBE_STATSD_INFO(tx_256_to_511byte_frames)},
-	{GBE_STATSD_INFO(tx_512_to_1023byte_frames)},
-	{GBE_STATSD_INFO(tx_1024byte_frames)},
-	{GBE_STATSD_INFO(net_bytes)},
-	{GBE_STATSD_INFO(rx_sof_overruns)},
-	{GBE_STATSD_INFO(rx_mof_overruns)},
-	{GBE_STATSD_INFO(rx_dma_overruns)},
+	GBE_STATSD_INFO(rx_good_frames),
+	GBE_STATSD_INFO(rx_broadcast_frames),
+	GBE_STATSD_INFO(rx_multicast_frames),
+	GBE_STATSD_INFO(rx_pause_frames),
+	GBE_STATSD_INFO(rx_crc_errors),
+	GBE_STATSD_INFO(rx_align_code_errors),
+	GBE_STATSD_INFO(rx_oversized_frames),
+	GBE_STATSD_INFO(rx_jabber_frames),
+	GBE_STATSD_INFO(rx_undersized_frames),
+	GBE_STATSD_INFO(rx_fragments),
+	GBE_STATSD_INFO(rx_bytes),
+	GBE_STATSD_INFO(tx_good_frames),
+	GBE_STATSD_INFO(tx_broadcast_frames),
+	GBE_STATSD_INFO(tx_multicast_frames),
+	GBE_STATSD_INFO(tx_pause_frames),
+	GBE_STATSD_INFO(tx_deferred_frames),
+	GBE_STATSD_INFO(tx_collision_frames),
+	GBE_STATSD_INFO(tx_single_coll_frames),
+	GBE_STATSD_INFO(tx_mult_coll_frames),
+	GBE_STATSD_INFO(tx_excessive_collisions),
+	GBE_STATSD_INFO(tx_late_collisions),
+	GBE_STATSD_INFO(tx_underrun),
+	GBE_STATSD_INFO(tx_carrier_sense_errors),
+	GBE_STATSD_INFO(tx_bytes),
+	GBE_STATSD_INFO(tx_64byte_frames),
+	GBE_STATSD_INFO(tx_65_to_127byte_frames),
+	GBE_STATSD_INFO(tx_128_to_255byte_frames),
+	GBE_STATSD_INFO(tx_256_to_511byte_frames),
+	GBE_STATSD_INFO(tx_512_to_1023byte_frames),
+	GBE_STATSD_INFO(tx_1024byte_frames),
+	GBE_STATSD_INFO(net_bytes),
+	GBE_STATSD_INFO(rx_sof_overruns),
+	GBE_STATSD_INFO(rx_mof_overruns),
+	GBE_STATSD_INFO(rx_dma_overruns),
 };
 
-#define XGBE_STATS0_INFO(field)	"GBE_0:"#field, XGBE_STATS0_MODULE, \
-				FIELD_SIZEOF(struct xgbe_hw_stats, field), \
-				offsetof(struct xgbe_hw_stats, field)
+/* This is the size of entries in GBENU_STATS_HOST */
+#define GBENU_ET_STATS_HOST_SIZE	33
+
+#define GBENU_STATS_HOST(field)					\
+{								\
+	"GBE_HOST:"#field, GBENU_STATS0_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
 
-#define XGBE_STATS1_INFO(field)	"GBE_1:"#field, XGBE_STATS1_MODULE, \
-				FIELD_SIZEOF(struct xgbe_hw_stats, field), \
-				offsetof(struct xgbe_hw_stats, field)
+/* This is the size of entries in GBENU_STATS_HOST */
+#define GBENU_ET_STATS_PORT_SIZE	46
 
-#define XGBE_STATS2_INFO(field)	"GBE_2:"#field, XGBE_STATS2_MODULE, \
-				FIELD_SIZEOF(struct xgbe_hw_stats, field), \
-				offsetof(struct xgbe_hw_stats, field)
+#define GBENU_STATS_P1(field)					\
+{								\
+	"GBE_P1:"#field, GBENU_STATS1_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+#define GBENU_STATS_P2(field)					\
+{								\
+	"GBE_P2:"#field, GBENU_STATS2_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+#define GBENU_STATS_P3(field)					\
+{								\
+	"GBE_P3:"#field, GBENU_STATS3_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+#define GBENU_STATS_P4(field)					\
+{								\
+	"GBE_P4:"#field, GBENU_STATS4_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+#define GBENU_STATS_P5(field)					\
+{								\
+	"GBE_P5:"#field, GBENU_STATS5_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+#define GBENU_STATS_P6(field)					\
+{								\
+	"GBE_P6:"#field, GBENU_STATS6_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+#define GBENU_STATS_P7(field)					\
+{								\
+	"GBE_P7:"#field, GBENU_STATS7_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+#define GBENU_STATS_P8(field)					\
+{								\
+	"GBE_P8:"#field, GBENU_STATS8_MODULE,			\
+	FIELD_SIZEOF(struct gbenu_hw_stats, field),		\
+	offsetof(struct gbenu_hw_stats, field)			\
+}
+
+static const struct netcp_ethtool_stat gbenu_et_stats[] = {
+	/* GBENU Host Module */
+	GBENU_STATS_HOST(rx_good_frames),
+	GBENU_STATS_HOST(rx_broadcast_frames),
+	GBENU_STATS_HOST(rx_multicast_frames),
+	GBENU_STATS_HOST(rx_crc_errors),
+	GBENU_STATS_HOST(rx_oversized_frames),
+	GBENU_STATS_HOST(rx_undersized_frames),
+	GBENU_STATS_HOST(ale_drop),
+	GBENU_STATS_HOST(ale_overrun_drop),
+	GBENU_STATS_HOST(rx_bytes),
+	GBENU_STATS_HOST(tx_good_frames),
+	GBENU_STATS_HOST(tx_broadcast_frames),
+	GBENU_STATS_HOST(tx_multicast_frames),
+	GBENU_STATS_HOST(tx_bytes),
+	GBENU_STATS_HOST(tx_64B_frames),
+	GBENU_STATS_HOST(tx_65_to_127B_frames),
+	GBENU_STATS_HOST(tx_128_to_255B_frames),
+	GBENU_STATS_HOST(tx_256_to_511B_frames),
+	GBENU_STATS_HOST(tx_512_to_1023B_frames),
+	GBENU_STATS_HOST(tx_1024B_frames),
+	GBENU_STATS_HOST(net_bytes),
+	GBENU_STATS_HOST(rx_bottom_fifo_drop),
+	GBENU_STATS_HOST(rx_port_mask_drop),
+	GBENU_STATS_HOST(rx_top_fifo_drop),
+	GBENU_STATS_HOST(ale_rate_limit_drop),
+	GBENU_STATS_HOST(ale_vid_ingress_drop),
+	GBENU_STATS_HOST(ale_da_eq_sa_drop),
+	GBENU_STATS_HOST(ale_unknown_ucast),
+	GBENU_STATS_HOST(ale_unknown_ucast_bytes),
+	GBENU_STATS_HOST(ale_unknown_mcast),
+	GBENU_STATS_HOST(ale_unknown_mcast_bytes),
+	GBENU_STATS_HOST(ale_unknown_bcast),
+	GBENU_STATS_HOST(ale_unknown_bcast_bytes),
+	GBENU_STATS_HOST(tx_mem_protect_err),
+	/* GBENU Module 1 */
+	GBENU_STATS_P1(rx_good_frames),
+	GBENU_STATS_P1(rx_broadcast_frames),
+	GBENU_STATS_P1(rx_multicast_frames),
+	GBENU_STATS_P1(rx_pause_frames),
+	GBENU_STATS_P1(rx_crc_errors),
+	GBENU_STATS_P1(rx_align_code_errors),
+	GBENU_STATS_P1(rx_oversized_frames),
+	GBENU_STATS_P1(rx_jabber_frames),
+	GBENU_STATS_P1(rx_undersized_frames),
+	GBENU_STATS_P1(rx_fragments),
+	GBENU_STATS_P1(ale_drop),
+	GBENU_STATS_P1(ale_overrun_drop),
+	GBENU_STATS_P1(rx_bytes),
+	GBENU_STATS_P1(tx_good_frames),
+	GBENU_STATS_P1(tx_broadcast_frames),
+	GBENU_STATS_P1(tx_multicast_frames),
+	GBENU_STATS_P1(tx_pause_frames),
+	GBENU_STATS_P1(tx_deferred_frames),
+	GBENU_STATS_P1(tx_collision_frames),
+	GBENU_STATS_P1(tx_single_coll_frames),
+	GBENU_STATS_P1(tx_mult_coll_frames),
+	GBENU_STATS_P1(tx_excessive_collisions),
+	GBENU_STATS_P1(tx_late_collisions),
+	GBENU_STATS_P1(rx_ipg_error),
+	GBENU_STATS_P1(tx_carrier_sense_errors),
+	GBENU_STATS_P1(tx_bytes),
+	GBENU_STATS_P1(tx_64B_frames),
+	GBENU_STATS_P1(tx_65_to_127B_frames),
+	GBENU_STATS_P1(tx_128_to_255B_frames),
+	GBENU_STATS_P1(tx_256_to_511B_frames),
+	GBENU_STATS_P1(tx_512_to_1023B_frames),
+	GBENU_STATS_P1(tx_1024B_frames),
+	GBENU_STATS_P1(net_bytes),
+	GBENU_STATS_P1(rx_bottom_fifo_drop),
+	GBENU_STATS_P1(rx_port_mask_drop),
+	GBENU_STATS_P1(rx_top_fifo_drop),
+	GBENU_STATS_P1(ale_rate_limit_drop),
+	GBENU_STATS_P1(ale_vid_ingress_drop),
+	GBENU_STATS_P1(ale_da_eq_sa_drop),
+	GBENU_STATS_P1(ale_unknown_ucast),
+	GBENU_STATS_P1(ale_unknown_ucast_bytes),
+	GBENU_STATS_P1(ale_unknown_mcast),
+	GBENU_STATS_P1(ale_unknown_mcast_bytes),
+	GBENU_STATS_P1(ale_unknown_bcast),
+	GBENU_STATS_P1(ale_unknown_bcast_bytes),
+	GBENU_STATS_P1(tx_mem_protect_err),
+	/* GBENU Module 2 */
+	GBENU_STATS_P2(rx_good_frames),
+	GBENU_STATS_P2(rx_broadcast_frames),
+	GBENU_STATS_P2(rx_multicast_frames),
+	GBENU_STATS_P2(rx_pause_frames),
+	GBENU_STATS_P2(rx_crc_errors),
+	GBENU_STATS_P2(rx_align_code_errors),
+	GBENU_STATS_P2(rx_oversized_frames),
+	GBENU_STATS_P2(rx_jabber_frames),
+	GBENU_STATS_P2(rx_undersized_frames),
+	GBENU_STATS_P2(rx_fragments),
+	GBENU_STATS_P2(ale_drop),
+	GBENU_STATS_P2(ale_overrun_drop),
+	GBENU_STATS_P2(rx_bytes),
+	GBENU_STATS_P2(tx_good_frames),
+	GBENU_STATS_P2(tx_broadcast_frames),
+	GBENU_STATS_P2(tx_multicast_frames),
+	GBENU_STATS_P2(tx_pause_frames),
+	GBENU_STATS_P2(tx_deferred_frames),
+	GBENU_STATS_P2(tx_collision_frames),
+	GBENU_STATS_P2(tx_single_coll_frames),
+	GBENU_STATS_P2(tx_mult_coll_frames),
+	GBENU_STATS_P2(tx_excessive_collisions),
+	GBENU_STATS_P2(tx_late_collisions),
+	GBENU_STATS_P2(rx_ipg_error),
+	GBENU_STATS_P2(tx_carrier_sense_errors),
+	GBENU_STATS_P2(tx_bytes),
+	GBENU_STATS_P2(tx_64B_frames),
+	GBENU_STATS_P2(tx_65_to_127B_frames),
+	GBENU_STATS_P2(tx_128_to_255B_frames),
+	GBENU_STATS_P2(tx_256_to_511B_frames),
+	GBENU_STATS_P2(tx_512_to_1023B_frames),
+	GBENU_STATS_P2(tx_1024B_frames),
+	GBENU_STATS_P2(net_bytes),
+	GBENU_STATS_P2(rx_bottom_fifo_drop),
+	GBENU_STATS_P2(rx_port_mask_drop),
+	GBENU_STATS_P2(rx_top_fifo_drop),
+	GBENU_STATS_P2(ale_rate_limit_drop),
+	GBENU_STATS_P2(ale_vid_ingress_drop),
+	GBENU_STATS_P2(ale_da_eq_sa_drop),
+	GBENU_STATS_P2(ale_unknown_ucast),
+	GBENU_STATS_P2(ale_unknown_ucast_bytes),
+	GBENU_STATS_P2(ale_unknown_mcast),
+	GBENU_STATS_P2(ale_unknown_mcast_bytes),
+	GBENU_STATS_P2(ale_unknown_bcast),
+	GBENU_STATS_P2(ale_unknown_bcast_bytes),
+	GBENU_STATS_P2(tx_mem_protect_err),
+	/* GBENU Module 3 */
+	GBENU_STATS_P3(rx_good_frames),
+	GBENU_STATS_P3(rx_broadcast_frames),
+	GBENU_STATS_P3(rx_multicast_frames),
+	GBENU_STATS_P3(rx_pause_frames),
+	GBENU_STATS_P3(rx_crc_errors),
+	GBENU_STATS_P3(rx_align_code_errors),
+	GBENU_STATS_P3(rx_oversized_frames),
+	GBENU_STATS_P3(rx_jabber_frames),
+	GBENU_STATS_P3(rx_undersized_frames),
+	GBENU_STATS_P3(rx_fragments),
+	GBENU_STATS_P3(ale_drop),
+	GBENU_STATS_P3(ale_overrun_drop),
+	GBENU_STATS_P3(rx_bytes),
+	GBENU_STATS_P3(tx_good_frames),
+	GBENU_STATS_P3(tx_broadcast_frames),
+	GBENU_STATS_P3(tx_multicast_frames),
+	GBENU_STATS_P3(tx_pause_frames),
+	GBENU_STATS_P3(tx_deferred_frames),
+	GBENU_STATS_P3(tx_collision_frames),
+	GBENU_STATS_P3(tx_single_coll_frames),
+	GBENU_STATS_P3(tx_mult_coll_frames),
+	GBENU_STATS_P3(tx_excessive_collisions),
+	GBENU_STATS_P3(tx_late_collisions),
+	GBENU_STATS_P3(rx_ipg_error),
+	GBENU_STATS_P3(tx_carrier_sense_errors),
+	GBENU_STATS_P3(tx_bytes),
+	GBENU_STATS_P3(tx_64B_frames),
+	GBENU_STATS_P3(tx_65_to_127B_frames),
+	GBENU_STATS_P3(tx_128_to_255B_frames),
+	GBENU_STATS_P3(tx_256_to_511B_frames),
+	GBENU_STATS_P3(tx_512_to_1023B_frames),
+	GBENU_STATS_P3(tx_1024B_frames),
+	GBENU_STATS_P3(net_bytes),
+	GBENU_STATS_P3(rx_bottom_fifo_drop),
+	GBENU_STATS_P3(rx_port_mask_drop),
+	GBENU_STATS_P3(rx_top_fifo_drop),
+	GBENU_STATS_P3(ale_rate_limit_drop),
+	GBENU_STATS_P3(ale_vid_ingress_drop),
+	GBENU_STATS_P3(ale_da_eq_sa_drop),
+	GBENU_STATS_P3(ale_unknown_ucast),
+	GBENU_STATS_P3(ale_unknown_ucast_bytes),
+	GBENU_STATS_P3(ale_unknown_mcast),
+	GBENU_STATS_P3(ale_unknown_mcast_bytes),
+	GBENU_STATS_P3(ale_unknown_bcast),
+	GBENU_STATS_P3(ale_unknown_bcast_bytes),
+	GBENU_STATS_P3(tx_mem_protect_err),
+	/* GBENU Module 4 */
+	GBENU_STATS_P4(rx_good_frames),
+	GBENU_STATS_P4(rx_broadcast_frames),
+	GBENU_STATS_P4(rx_multicast_frames),
+	GBENU_STATS_P4(rx_pause_frames),
+	GBENU_STATS_P4(rx_crc_errors),
+	GBENU_STATS_P4(rx_align_code_errors),
+	GBENU_STATS_P4(rx_oversized_frames),
+	GBENU_STATS_P4(rx_jabber_frames),
+	GBENU_STATS_P4(rx_undersized_frames),
+	GBENU_STATS_P4(rx_fragments),
+	GBENU_STATS_P4(ale_drop),
+	GBENU_STATS_P4(ale_overrun_drop),
+	GBENU_STATS_P4(rx_bytes),
+	GBENU_STATS_P4(tx_good_frames),
+	GBENU_STATS_P4(tx_broadcast_frames),
+	GBENU_STATS_P4(tx_multicast_frames),
+	GBENU_STATS_P4(tx_pause_frames),
+	GBENU_STATS_P4(tx_deferred_frames),
+	GBENU_STATS_P4(tx_collision_frames),
+	GBENU_STATS_P4(tx_single_coll_frames),
+	GBENU_STATS_P4(tx_mult_coll_frames),
+	GBENU_STATS_P4(tx_excessive_collisions),
+	GBENU_STATS_P4(tx_late_collisions),
+	GBENU_STATS_P4(rx_ipg_error),
+	GBENU_STATS_P4(tx_carrier_sense_errors),
+	GBENU_STATS_P4(tx_bytes),
+	GBENU_STATS_P4(tx_64B_frames),
+	GBENU_STATS_P4(tx_65_to_127B_frames),
+	GBENU_STATS_P4(tx_128_to_255B_frames),
+	GBENU_STATS_P4(tx_256_to_511B_frames),
+	GBENU_STATS_P4(tx_512_to_1023B_frames),
+	GBENU_STATS_P4(tx_1024B_frames),
+	GBENU_STATS_P4(net_bytes),
+	GBENU_STATS_P4(rx_bottom_fifo_drop),
+	GBENU_STATS_P4(rx_port_mask_drop),
+	GBENU_STATS_P4(rx_top_fifo_drop),
+	GBENU_STATS_P4(ale_rate_limit_drop),
+	GBENU_STATS_P4(ale_vid_ingress_drop),
+	GBENU_STATS_P4(ale_da_eq_sa_drop),
+	GBENU_STATS_P4(ale_unknown_ucast),
+	GBENU_STATS_P4(ale_unknown_ucast_bytes),
+	GBENU_STATS_P4(ale_unknown_mcast),
+	GBENU_STATS_P4(ale_unknown_mcast_bytes),
+	GBENU_STATS_P4(ale_unknown_bcast),
+	GBENU_STATS_P4(ale_unknown_bcast_bytes),
+	GBENU_STATS_P4(tx_mem_protect_err),
+	/* GBENU Module 5 */
+	GBENU_STATS_P5(rx_good_frames),
+	GBENU_STATS_P5(rx_broadcast_frames),
+	GBENU_STATS_P5(rx_multicast_frames),
+	GBENU_STATS_P5(rx_pause_frames),
+	GBENU_STATS_P5(rx_crc_errors),
+	GBENU_STATS_P5(rx_align_code_errors),
+	GBENU_STATS_P5(rx_oversized_frames),
+	GBENU_STATS_P5(rx_jabber_frames),
+	GBENU_STATS_P5(rx_undersized_frames),
+	GBENU_STATS_P5(rx_fragments),
+	GBENU_STATS_P5(ale_drop),
+	GBENU_STATS_P5(ale_overrun_drop),
+	GBENU_STATS_P5(rx_bytes),
+	GBENU_STATS_P5(tx_good_frames),
+	GBENU_STATS_P5(tx_broadcast_frames),
+	GBENU_STATS_P5(tx_multicast_frames),
+	GBENU_STATS_P5(tx_pause_frames),
+	GBENU_STATS_P5(tx_deferred_frames),
+	GBENU_STATS_P5(tx_collision_frames),
+	GBENU_STATS_P5(tx_single_coll_frames),
+	GBENU_STATS_P5(tx_mult_coll_frames),
+	GBENU_STATS_P5(tx_excessive_collisions),
+	GBENU_STATS_P5(tx_late_collisions),
+	GBENU_STATS_P5(rx_ipg_error),
+	GBENU_STATS_P5(tx_carrier_sense_errors),
+	GBENU_STATS_P5(tx_bytes),
+	GBENU_STATS_P5(tx_64B_frames),
+	GBENU_STATS_P5(tx_65_to_127B_frames),
+	GBENU_STATS_P5(tx_128_to_255B_frames),
+	GBENU_STATS_P5(tx_256_to_511B_frames),
+	GBENU_STATS_P5(tx_512_to_1023B_frames),
+	GBENU_STATS_P5(tx_1024B_frames),
+	GBENU_STATS_P5(net_bytes),
+	GBENU_STATS_P5(rx_bottom_fifo_drop),
+	GBENU_STATS_P5(rx_port_mask_drop),
+	GBENU_STATS_P5(rx_top_fifo_drop),
+	GBENU_STATS_P5(ale_rate_limit_drop),
+	GBENU_STATS_P5(ale_vid_ingress_drop),
+	GBENU_STATS_P5(ale_da_eq_sa_drop),
+	GBENU_STATS_P5(ale_unknown_ucast),
+	GBENU_STATS_P5(ale_unknown_ucast_bytes),
+	GBENU_STATS_P5(ale_unknown_mcast),
+	GBENU_STATS_P5(ale_unknown_mcast_bytes),
+	GBENU_STATS_P5(ale_unknown_bcast),
+	GBENU_STATS_P5(ale_unknown_bcast_bytes),
+	GBENU_STATS_P5(tx_mem_protect_err),
+	/* GBENU Module 6 */
+	GBENU_STATS_P6(rx_good_frames),
+	GBENU_STATS_P6(rx_broadcast_frames),
+	GBENU_STATS_P6(rx_multicast_frames),
+	GBENU_STATS_P6(rx_pause_frames),
+	GBENU_STATS_P6(rx_crc_errors),
+	GBENU_STATS_P6(rx_align_code_errors),
+	GBENU_STATS_P6(rx_oversized_frames),
+	GBENU_STATS_P6(rx_jabber_frames),
+	GBENU_STATS_P6(rx_undersized_frames),
+	GBENU_STATS_P6(rx_fragments),
+	GBENU_STATS_P6(ale_drop),
+	GBENU_STATS_P6(ale_overrun_drop),
+	GBENU_STATS_P6(rx_bytes),
+	GBENU_STATS_P6(tx_good_frames),
+	GBENU_STATS_P6(tx_broadcast_frames),
+	GBENU_STATS_P6(tx_multicast_frames),
+	GBENU_STATS_P6(tx_pause_frames),
+	GBENU_STATS_P6(tx_deferred_frames),
+	GBENU_STATS_P6(tx_collision_frames),
+	GBENU_STATS_P6(tx_single_coll_frames),
+	GBENU_STATS_P6(tx_mult_coll_frames),
+	GBENU_STATS_P6(tx_excessive_collisions),
+	GBENU_STATS_P6(tx_late_collisions),
+	GBENU_STATS_P6(rx_ipg_error),
+	GBENU_STATS_P6(tx_carrier_sense_errors),
+	GBENU_STATS_P6(tx_bytes),
+	GBENU_STATS_P6(tx_64B_frames),
+	GBENU_STATS_P6(tx_65_to_127B_frames),
+	GBENU_STATS_P6(tx_128_to_255B_frames),
+	GBENU_STATS_P6(tx_256_to_511B_frames),
+	GBENU_STATS_P6(tx_512_to_1023B_frames),
+	GBENU_STATS_P6(tx_1024B_frames),
+	GBENU_STATS_P6(net_bytes),
+	GBENU_STATS_P6(rx_bottom_fifo_drop),
+	GBENU_STATS_P6(rx_port_mask_drop),
+	GBENU_STATS_P6(rx_top_fifo_drop),
+	GBENU_STATS_P6(ale_rate_limit_drop),
+	GBENU_STATS_P6(ale_vid_ingress_drop),
+	GBENU_STATS_P6(ale_da_eq_sa_drop),
+	GBENU_STATS_P6(ale_unknown_ucast),
+	GBENU_STATS_P6(ale_unknown_ucast_bytes),
+	GBENU_STATS_P6(ale_unknown_mcast),
+	GBENU_STATS_P6(ale_unknown_mcast_bytes),
+	GBENU_STATS_P6(ale_unknown_bcast),
+	GBENU_STATS_P6(ale_unknown_bcast_bytes),
+	GBENU_STATS_P6(tx_mem_protect_err),
+	/* GBENU Module 7 */
+	GBENU_STATS_P7(rx_good_frames),
+	GBENU_STATS_P7(rx_broadcast_frames),
+	GBENU_STATS_P7(rx_multicast_frames),
+	GBENU_STATS_P7(rx_pause_frames),
+	GBENU_STATS_P7(rx_crc_errors),
+	GBENU_STATS_P7(rx_align_code_errors),
+	GBENU_STATS_P7(rx_oversized_frames),
+	GBENU_STATS_P7(rx_jabber_frames),
+	GBENU_STATS_P7(rx_undersized_frames),
+	GBENU_STATS_P7(rx_fragments),
+	GBENU_STATS_P7(ale_drop),
+	GBENU_STATS_P7(ale_overrun_drop),
+	GBENU_STATS_P7(rx_bytes),
+	GBENU_STATS_P7(tx_good_frames),
+	GBENU_STATS_P7(tx_broadcast_frames),
+	GBENU_STATS_P7(tx_multicast_frames),
+	GBENU_STATS_P7(tx_pause_frames),
+	GBENU_STATS_P7(tx_deferred_frames),
+	GBENU_STATS_P7(tx_collision_frames),
+	GBENU_STATS_P7(tx_single_coll_frames),
+	GBENU_STATS_P7(tx_mult_coll_frames),
+	GBENU_STATS_P7(tx_excessive_collisions),
+	GBENU_STATS_P7(tx_late_collisions),
+	GBENU_STATS_P7(rx_ipg_error),
+	GBENU_STATS_P7(tx_carrier_sense_errors),
+	GBENU_STATS_P7(tx_bytes),
+	GBENU_STATS_P7(tx_64B_frames),
+	GBENU_STATS_P7(tx_65_to_127B_frames),
+	GBENU_STATS_P7(tx_128_to_255B_frames),
+	GBENU_STATS_P7(tx_256_to_511B_frames),
+	GBENU_STATS_P7(tx_512_to_1023B_frames),
+	GBENU_STATS_P7(tx_1024B_frames),
+	GBENU_STATS_P7(net_bytes),
+	GBENU_STATS_P7(rx_bottom_fifo_drop),
+	GBENU_STATS_P7(rx_port_mask_drop),
+	GBENU_STATS_P7(rx_top_fifo_drop),
+	GBENU_STATS_P7(ale_rate_limit_drop),
+	GBENU_STATS_P7(ale_vid_ingress_drop),
+	GBENU_STATS_P7(ale_da_eq_sa_drop),
+	GBENU_STATS_P7(ale_unknown_ucast),
+	GBENU_STATS_P7(ale_unknown_ucast_bytes),
+	GBENU_STATS_P7(ale_unknown_mcast),
+	GBENU_STATS_P7(ale_unknown_mcast_bytes),
+	GBENU_STATS_P7(ale_unknown_bcast),
+	GBENU_STATS_P7(ale_unknown_bcast_bytes),
+	GBENU_STATS_P7(tx_mem_protect_err),
+	/* GBENU Module 8 */
+	GBENU_STATS_P8(rx_good_frames),
+	GBENU_STATS_P8(rx_broadcast_frames),
+	GBENU_STATS_P8(rx_multicast_frames),
+	GBENU_STATS_P8(rx_pause_frames),
+	GBENU_STATS_P8(rx_crc_errors),
+	GBENU_STATS_P8(rx_align_code_errors),
+	GBENU_STATS_P8(rx_oversized_frames),
+	GBENU_STATS_P8(rx_jabber_frames),
+	GBENU_STATS_P8(rx_undersized_frames),
+	GBENU_STATS_P8(rx_fragments),
+	GBENU_STATS_P8(ale_drop),
+	GBENU_STATS_P8(ale_overrun_drop),
+	GBENU_STATS_P8(rx_bytes),
+	GBENU_STATS_P8(tx_good_frames),
+	GBENU_STATS_P8(tx_broadcast_frames),
+	GBENU_STATS_P8(tx_multicast_frames),
+	GBENU_STATS_P8(tx_pause_frames),
+	GBENU_STATS_P8(tx_deferred_frames),
+	GBENU_STATS_P8(tx_collision_frames),
+	GBENU_STATS_P8(tx_single_coll_frames),
+	GBENU_STATS_P8(tx_mult_coll_frames),
+	GBENU_STATS_P8(tx_excessive_collisions),
+	GBENU_STATS_P8(tx_late_collisions),
+	GBENU_STATS_P8(rx_ipg_error),
+	GBENU_STATS_P8(tx_carrier_sense_errors),
+	GBENU_STATS_P8(tx_bytes),
+	GBENU_STATS_P8(tx_64B_frames),
+	GBENU_STATS_P8(tx_65_to_127B_frames),
+	GBENU_STATS_P8(tx_128_to_255B_frames),
+	GBENU_STATS_P8(tx_256_to_511B_frames),
+	GBENU_STATS_P8(tx_512_to_1023B_frames),
+	GBENU_STATS_P8(tx_1024B_frames),
+	GBENU_STATS_P8(net_bytes),
+	GBENU_STATS_P8(rx_bottom_fifo_drop),
+	GBENU_STATS_P8(rx_port_mask_drop),
+	GBENU_STATS_P8(rx_top_fifo_drop),
+	GBENU_STATS_P8(ale_rate_limit_drop),
+	GBENU_STATS_P8(ale_vid_ingress_drop),
+	GBENU_STATS_P8(ale_da_eq_sa_drop),
+	GBENU_STATS_P8(ale_unknown_ucast),
+	GBENU_STATS_P8(ale_unknown_ucast_bytes),
+	GBENU_STATS_P8(ale_unknown_mcast),
+	GBENU_STATS_P8(ale_unknown_mcast_bytes),
+	GBENU_STATS_P8(ale_unknown_bcast),
+	GBENU_STATS_P8(ale_unknown_bcast_bytes),
+	GBENU_STATS_P8(tx_mem_protect_err),
+};
+
+#define XGBE_STATS0_INFO(field)				\
+{							\
+	"GBE_0:"#field, XGBE_STATS0_MODULE,		\
+	FIELD_SIZEOF(struct xgbe_hw_stats, field),	\
+	offsetof(struct xgbe_hw_stats, field)		\
+}
+
+#define XGBE_STATS1_INFO(field)				\
+{							\
+	"GBE_1:"#field, XGBE_STATS1_MODULE,		\
+	FIELD_SIZEOF(struct xgbe_hw_stats, field),	\
+	offsetof(struct xgbe_hw_stats, field)		\
+}
+
+#define XGBE_STATS2_INFO(field)				\
+{							\
+	"GBE_2:"#field, XGBE_STATS2_MODULE,		\
+	FIELD_SIZEOF(struct xgbe_hw_stats, field),	\
+	offsetof(struct xgbe_hw_stats, field)		\
+}
 
 static const struct netcp_ethtool_stat xgbe10_et_stats[] = {
 	/* GBE module 0 */
-	{XGBE_STATS0_INFO(rx_good_frames)},
-	{XGBE_STATS0_INFO(rx_broadcast_frames)},
-	{XGBE_STATS0_INFO(rx_multicast_frames)},
-	{XGBE_STATS0_INFO(rx_oversized_frames)},
-	{XGBE_STATS0_INFO(rx_undersized_frames)},
-	{XGBE_STATS0_INFO(overrun_type4)},
-	{XGBE_STATS0_INFO(overrun_type5)},
-	{XGBE_STATS0_INFO(rx_bytes)},
-	{XGBE_STATS0_INFO(tx_good_frames)},
-	{XGBE_STATS0_INFO(tx_broadcast_frames)},
-	{XGBE_STATS0_INFO(tx_multicast_frames)},
-	{XGBE_STATS0_INFO(tx_bytes)},
-	{XGBE_STATS0_INFO(tx_64byte_frames)},
-	{XGBE_STATS0_INFO(tx_65_to_127byte_frames)},
-	{XGBE_STATS0_INFO(tx_128_to_255byte_frames)},
-	{XGBE_STATS0_INFO(tx_256_to_511byte_frames)},
-	{XGBE_STATS0_INFO(tx_512_to_1023byte_frames)},
-	{XGBE_STATS0_INFO(tx_1024byte_frames)},
-	{XGBE_STATS0_INFO(net_bytes)},
-	{XGBE_STATS0_INFO(rx_sof_overruns)},
-	{XGBE_STATS0_INFO(rx_mof_overruns)},
-	{XGBE_STATS0_INFO(rx_dma_overruns)},
+	XGBE_STATS0_INFO(rx_good_frames),
+	XGBE_STATS0_INFO(rx_broadcast_frames),
+	XGBE_STATS0_INFO(rx_multicast_frames),
+	XGBE_STATS0_INFO(rx_oversized_frames),
+	XGBE_STATS0_INFO(rx_undersized_frames),
+	XGBE_STATS0_INFO(overrun_type4),
+	XGBE_STATS0_INFO(overrun_type5),
+	XGBE_STATS0_INFO(rx_bytes),
+	XGBE_STATS0_INFO(tx_good_frames),
+	XGBE_STATS0_INFO(tx_broadcast_frames),
+	XGBE_STATS0_INFO(tx_multicast_frames),
+	XGBE_STATS0_INFO(tx_bytes),
+	XGBE_STATS0_INFO(tx_64byte_frames),
+	XGBE_STATS0_INFO(tx_65_to_127byte_frames),
+	XGBE_STATS0_INFO(tx_128_to_255byte_frames),
+	XGBE_STATS0_INFO(tx_256_to_511byte_frames),
+	XGBE_STATS0_INFO(tx_512_to_1023byte_frames),
+	XGBE_STATS0_INFO(tx_1024byte_frames),
+	XGBE_STATS0_INFO(net_bytes),
+	XGBE_STATS0_INFO(rx_sof_overruns),
+	XGBE_STATS0_INFO(rx_mof_overruns),
+	XGBE_STATS0_INFO(rx_dma_overruns),
 	/* XGBE module 1 */
-	{XGBE_STATS1_INFO(rx_good_frames)},
-	{XGBE_STATS1_INFO(rx_broadcast_frames)},
-	{XGBE_STATS1_INFO(rx_multicast_frames)},
-	{XGBE_STATS1_INFO(rx_pause_frames)},
-	{XGBE_STATS1_INFO(rx_crc_errors)},
-	{XGBE_STATS1_INFO(rx_align_code_errors)},
-	{XGBE_STATS1_INFO(rx_oversized_frames)},
-	{XGBE_STATS1_INFO(rx_jabber_frames)},
-	{XGBE_STATS1_INFO(rx_undersized_frames)},
-	{XGBE_STATS1_INFO(rx_fragments)},
-	{XGBE_STATS1_INFO(overrun_type4)},
-	{XGBE_STATS1_INFO(overrun_type5)},
-	{XGBE_STATS1_INFO(rx_bytes)},
-	{XGBE_STATS1_INFO(tx_good_frames)},
-	{XGBE_STATS1_INFO(tx_broadcast_frames)},
-	{XGBE_STATS1_INFO(tx_multicast_frames)},
-	{XGBE_STATS1_INFO(tx_pause_frames)},
-	{XGBE_STATS1_INFO(tx_deferred_frames)},
-	{XGBE_STATS1_INFO(tx_collision_frames)},
-	{XGBE_STATS1_INFO(tx_single_coll_frames)},
-	{XGBE_STATS1_INFO(tx_mult_coll_frames)},
-	{XGBE_STATS1_INFO(tx_excessive_collisions)},
-	{XGBE_STATS1_INFO(tx_late_collisions)},
-	{XGBE_STATS1_INFO(tx_underrun)},
-	{XGBE_STATS1_INFO(tx_carrier_sense_errors)},
-	{XGBE_STATS1_INFO(tx_bytes)},
-	{XGBE_STATS1_INFO(tx_64byte_frames)},
-	{XGBE_STATS1_INFO(tx_65_to_127byte_frames)},
-	{XGBE_STATS1_INFO(tx_128_to_255byte_frames)},
-	{XGBE_STATS1_INFO(tx_256_to_511byte_frames)},
-	{XGBE_STATS1_INFO(tx_512_to_1023byte_frames)},
-	{XGBE_STATS1_INFO(tx_1024byte_frames)},
-	{XGBE_STATS1_INFO(net_bytes)},
-	{XGBE_STATS1_INFO(rx_sof_overruns)},
-	{XGBE_STATS1_INFO(rx_mof_overruns)},
-	{XGBE_STATS1_INFO(rx_dma_overruns)},
+	XGBE_STATS1_INFO(rx_good_frames),
+	XGBE_STATS1_INFO(rx_broadcast_frames),
+	XGBE_STATS1_INFO(rx_multicast_frames),
+	XGBE_STATS1_INFO(rx_pause_frames),
+	XGBE_STATS1_INFO(rx_crc_errors),
+	XGBE_STATS1_INFO(rx_align_code_errors),
+	XGBE_STATS1_INFO(rx_oversized_frames),
+	XGBE_STATS1_INFO(rx_jabber_frames),
+	XGBE_STATS1_INFO(rx_undersized_frames),
+	XGBE_STATS1_INFO(rx_fragments),
+	XGBE_STATS1_INFO(overrun_type4),
+	XGBE_STATS1_INFO(overrun_type5),
+	XGBE_STATS1_INFO(rx_bytes),
+	XGBE_STATS1_INFO(tx_good_frames),
+	XGBE_STATS1_INFO(tx_broadcast_frames),
+	XGBE_STATS1_INFO(tx_multicast_frames),
+	XGBE_STATS1_INFO(tx_pause_frames),
+	XGBE_STATS1_INFO(tx_deferred_frames),
+	XGBE_STATS1_INFO(tx_collision_frames),
+	XGBE_STATS1_INFO(tx_single_coll_frames),
+	XGBE_STATS1_INFO(tx_mult_coll_frames),
+	XGBE_STATS1_INFO(tx_excessive_collisions),
+	XGBE_STATS1_INFO(tx_late_collisions),
+	XGBE_STATS1_INFO(tx_underrun),
+	XGBE_STATS1_INFO(tx_carrier_sense_errors),
+	XGBE_STATS1_INFO(tx_bytes),
+	XGBE_STATS1_INFO(tx_64byte_frames),
+	XGBE_STATS1_INFO(tx_65_to_127byte_frames),
+	XGBE_STATS1_INFO(tx_128_to_255byte_frames),
+	XGBE_STATS1_INFO(tx_256_to_511byte_frames),
+	XGBE_STATS1_INFO(tx_512_to_1023byte_frames),
+	XGBE_STATS1_INFO(tx_1024byte_frames),
+	XGBE_STATS1_INFO(net_bytes),
+	XGBE_STATS1_INFO(rx_sof_overruns),
+	XGBE_STATS1_INFO(rx_mof_overruns),
+	XGBE_STATS1_INFO(rx_dma_overruns),
 	/* XGBE module 2 */
-	{XGBE_STATS2_INFO(rx_good_frames)},
-	{XGBE_STATS2_INFO(rx_broadcast_frames)},
-	{XGBE_STATS2_INFO(rx_multicast_frames)},
-	{XGBE_STATS2_INFO(rx_pause_frames)},
-	{XGBE_STATS2_INFO(rx_crc_errors)},
-	{XGBE_STATS2_INFO(rx_align_code_errors)},
-	{XGBE_STATS2_INFO(rx_oversized_frames)},
-	{XGBE_STATS2_INFO(rx_jabber_frames)},
-	{XGBE_STATS2_INFO(rx_undersized_frames)},
-	{XGBE_STATS2_INFO(rx_fragments)},
-	{XGBE_STATS2_INFO(overrun_type4)},
-	{XGBE_STATS2_INFO(overrun_type5)},
-	{XGBE_STATS2_INFO(rx_bytes)},
-	{XGBE_STATS2_INFO(tx_good_frames)},
-	{XGBE_STATS2_INFO(tx_broadcast_frames)},
-	{XGBE_STATS2_INFO(tx_multicast_frames)},
-	{XGBE_STATS2_INFO(tx_pause_frames)},
-	{XGBE_STATS2_INFO(tx_deferred_frames)},
-	{XGBE_STATS2_INFO(tx_collision_frames)},
-	{XGBE_STATS2_INFO(tx_single_coll_frames)},
-	{XGBE_STATS2_INFO(tx_mult_coll_frames)},
-	{XGBE_STATS2_INFO(tx_excessive_collisions)},
-	{XGBE_STATS2_INFO(tx_late_collisions)},
-	{XGBE_STATS2_INFO(tx_underrun)},
-	{XGBE_STATS2_INFO(tx_carrier_sense_errors)},
-	{XGBE_STATS2_INFO(tx_bytes)},
-	{XGBE_STATS2_INFO(tx_64byte_frames)},
-	{XGBE_STATS2_INFO(tx_65_to_127byte_frames)},
-	{XGBE_STATS2_INFO(tx_128_to_255byte_frames)},
-	{XGBE_STATS2_INFO(tx_256_to_511byte_frames)},
-	{XGBE_STATS2_INFO(tx_512_to_1023byte_frames)},
-	{XGBE_STATS2_INFO(tx_1024byte_frames)},
-	{XGBE_STATS2_INFO(net_bytes)},
-	{XGBE_STATS2_INFO(rx_sof_overruns)},
-	{XGBE_STATS2_INFO(rx_mof_overruns)},
-	{XGBE_STATS2_INFO(rx_dma_overruns)},
+	XGBE_STATS2_INFO(rx_good_frames),
+	XGBE_STATS2_INFO(rx_broadcast_frames),
+	XGBE_STATS2_INFO(rx_multicast_frames),
+	XGBE_STATS2_INFO(rx_pause_frames),
+	XGBE_STATS2_INFO(rx_crc_errors),
+	XGBE_STATS2_INFO(rx_align_code_errors),
+	XGBE_STATS2_INFO(rx_oversized_frames),
+	XGBE_STATS2_INFO(rx_jabber_frames),
+	XGBE_STATS2_INFO(rx_undersized_frames),
+	XGBE_STATS2_INFO(rx_fragments),
+	XGBE_STATS2_INFO(overrun_type4),
+	XGBE_STATS2_INFO(overrun_type5),
+	XGBE_STATS2_INFO(rx_bytes),
+	XGBE_STATS2_INFO(tx_good_frames),
+	XGBE_STATS2_INFO(tx_broadcast_frames),
+	XGBE_STATS2_INFO(tx_multicast_frames),
+	XGBE_STATS2_INFO(tx_pause_frames),
+	XGBE_STATS2_INFO(tx_deferred_frames),
+	XGBE_STATS2_INFO(tx_collision_frames),
+	XGBE_STATS2_INFO(tx_single_coll_frames),
+	XGBE_STATS2_INFO(tx_mult_coll_frames),
+	XGBE_STATS2_INFO(tx_excessive_collisions),
+	XGBE_STATS2_INFO(tx_late_collisions),
+	XGBE_STATS2_INFO(tx_underrun),
+	XGBE_STATS2_INFO(tx_carrier_sense_errors),
+	XGBE_STATS2_INFO(tx_bytes),
+	XGBE_STATS2_INFO(tx_64byte_frames),
+	XGBE_STATS2_INFO(tx_65_to_127byte_frames),
+	XGBE_STATS2_INFO(tx_128_to_255byte_frames),
+	XGBE_STATS2_INFO(tx_256_to_511byte_frames),
+	XGBE_STATS2_INFO(tx_512_to_1023byte_frames),
+	XGBE_STATS2_INFO(tx_1024byte_frames),
+	XGBE_STATS2_INFO(net_bytes),
+	XGBE_STATS2_INFO(rx_sof_overruns),
+	XGBE_STATS2_INFO(rx_mof_overruns),
+	XGBE_STATS2_INFO(rx_dma_overruns),
 };
 
 #define for_each_intf(i, priv) \
@@ -1066,9 +1796,16 @@ static void netcp_ethss_update_link_state(struct gbe_priv *gbe_dev,
 	if (!slave->open)
 		return;
 
-	if (!SLAVE_LINK_IS_XGMII(slave))
-		sgmii_link_state = netcp_sgmii_get_port_link(SGMII_BASE(sp),
-							     sp);
+	if (!SLAVE_LINK_IS_XGMII(slave)) {
+		if (gbe_dev->ss_version == GBE_SS_VERSION_14)
+			sgmii_link_state =
+				netcp_sgmii_get_port_link(SGMII_BASE(sp), sp);
+		else
+			sgmii_link_state =
+				netcp_sgmii_get_port_link(
+						gbe_dev->sgmii_port_regs, sp);
+	}
+
 	phy_link_state = gbe_phy_link_status(slave);
 	link_state = phy_link_state & sgmii_link_state;
 
@@ -1137,6 +1874,7 @@ static int gbe_port_reset(struct gbe_slave *slave)
 static void gbe_port_config(struct gbe_priv *gbe_dev, struct gbe_slave *slave,
 			    int max_rx_len)
 {
+	void __iomem *rx_maxlen_reg;
 	u32 xgmii_mode;
 
 	if (max_rx_len > NETCP_MAX_FRAME_SIZE)
@@ -1150,7 +1888,12 @@ static void gbe_port_config(struct gbe_priv *gbe_dev, struct gbe_slave *slave,
 		writel(xgmii_mode, GBE_REG_ADDR(gbe_dev, ss_regs, control));
 	}
 
-	writel(max_rx_len, GBE_REG_ADDR(slave, emac_regs, rx_maxlen));
+	if (IS_SS_ID_MU(gbe_dev))
+		rx_maxlen_reg = GBE_REG_ADDR(slave, port_regs, rx_maxlen);
+	else
+		rx_maxlen_reg = GBE_REG_ADDR(slave, emac_regs, rx_maxlen);
+
+	writel(max_rx_len, rx_maxlen_reg);
 	writel(slave->mac_control, GBE_REG_ADDR(slave, emac_regs, mac_control));
 }
 
@@ -1242,6 +1985,12 @@ static int gbe_slave_open(struct gbe_intf *gbe_intf)
 static void gbe_init_host_port(struct gbe_priv *priv)
 {
 	int bypass_en = 1;
+
+	/* Host Tx Pri */
+	if (IS_SS_ID_NU(priv))
+		writel(HOST_TX_PRI_MAP_DEFAULT,
+		       GBE_REG_ADDR(priv, host_port_regs, tx_pri_map));
+
 	/* Max length register */
 	writel(NETCP_MAX_FRAME_SIZE, GBE_REG_ADDR(priv, host_port_regs,
 						  rx_maxlen));
@@ -1472,15 +2221,21 @@ static int gbe_open(void *intf_priv, struct net_device *ndev)
 		GBE_MAJOR_VERSION(reg), GBE_MINOR_VERSION(reg),
 		GBE_RTL_VERSION(reg), GBE_IDENT(reg));
 
+	/* For 10G and on NetCP 1.5, use directed to port */
+	if ((gbe_dev->ss_version == XGBE_SS_VERSION_10) || IS_SS_ID_MU(gbe_dev))
+		gbe_intf->tx_pipe.flags = SWITCH_TO_PORT_IN_TAGINFO;
+
 	if (gbe_dev->enable_ale)
-		gbe_intf->tx_pipe.dma_psflags = 0;
+		gbe_intf->tx_pipe.switch_to_port = 0;
 	else
-		gbe_intf->tx_pipe.dma_psflags = port_num;
+		gbe_intf->tx_pipe.switch_to_port = port_num;
 
-	dev_dbg(gbe_dev->dev, "opened TX channel %s: %p with psflags %d\n",
+	dev_dbg(gbe_dev->dev,
+		"opened TX channel %s: %p with to port %d, flags %d\n",
 		gbe_intf->tx_pipe.dma_chan_name,
 		gbe_intf->tx_pipe.dma_channel,
-		gbe_intf->tx_pipe.dma_psflags);
+		gbe_intf->tx_pipe.switch_to_port,
+		gbe_intf->tx_pipe.flags);
 
 	gbe_slave_stop(gbe_intf);
 
@@ -1491,8 +2246,8 @@ static int gbe_open(void *intf_priv, struct net_device *ndev)
 	writel(GBE_CTL_P0_ENABLE, GBE_REG_ADDR(gbe_dev, switch_regs, control));
 
 	/* All statistics enabled and STAT AB visible by default */
-	writel(GBE_REG_VAL_STAT_ENABLE_ALL, GBE_REG_ADDR(gbe_dev, switch_regs,
-							 stat_port_en));
+	writel(gbe_dev->stats_en_mask, GBE_REG_ADDR(gbe_dev, switch_regs,
+						    stat_port_en));
 
 	ret = gbe_slave_open(gbe_intf);
 	if (ret)
@@ -1529,6 +2284,7 @@ static int init_slave(struct gbe_priv *gbe_dev, struct gbe_slave *slave,
 {
 	int port_reg_num;
 	u32 port_reg_ofs, emac_reg_ofs;
+	u32 port_reg_blk_sz, emac_reg_blk_sz;
 
 	if (of_property_read_u32(node, "slave-port", &slave->slave_num)) {
 		dev_err(gbe_dev->dev, "missing slave-port parameter\n");
@@ -1560,23 +2316,29 @@ static int init_slave(struct gbe_priv *gbe_dev, struct gbe_slave *slave,
 		} else {
 			port_reg_ofs = GBE13_SLAVE_PORT_OFFSET;
 		}
+		emac_reg_ofs = GBE13_EMAC_OFFSET;
+		port_reg_blk_sz = 0x30;
+		emac_reg_blk_sz = 0x40;
+	} else if (IS_SS_ID_MU(gbe_dev)) {
+		port_reg_ofs = GBENU_SLAVE_PORT_OFFSET;
+		emac_reg_ofs = GBENU_EMAC_OFFSET;
+		port_reg_blk_sz = 0x1000;
+		emac_reg_blk_sz = 0x1000;
 	} else if (gbe_dev->ss_version == XGBE_SS_VERSION_10) {
 		port_reg_ofs = XGBE10_SLAVE_PORT_OFFSET;
+		emac_reg_ofs = XGBE10_EMAC_OFFSET;
+		port_reg_blk_sz = 0x30;
+		emac_reg_blk_sz = 0x40;
 	} else {
 		dev_err(gbe_dev->dev, "unknown ethss(0x%x)\n",
 			gbe_dev->ss_version);
 		return -EINVAL;
 	}
 
-	if (gbe_dev->ss_version == GBE_SS_VERSION_14)
-		emac_reg_ofs = GBE13_EMAC_OFFSET;
-	else if (gbe_dev->ss_version == XGBE_SS_VERSION_10)
-		emac_reg_ofs = XGBE10_EMAC_OFFSET;
-
-	slave->port_regs = gbe_dev->ss_regs + port_reg_ofs +
-				(0x30 * port_reg_num);
-	slave->emac_regs = gbe_dev->ss_regs + emac_reg_ofs +
-				(0x40 * slave->slave_num);
+	slave->port_regs = gbe_dev->switch_regs + port_reg_ofs +
+				(port_reg_blk_sz * port_reg_num);
+	slave->emac_regs = gbe_dev->switch_regs + emac_reg_ofs +
+				(emac_reg_blk_sz * slave->slave_num);
 
 	if (gbe_dev->ss_version == GBE_SS_VERSION_14) {
 		/* Initialize  slave port register offsets */
@@ -1595,6 +2357,23 @@ static int init_slave(struct gbe_priv *gbe_dev, struct gbe_slave *slave,
 		GBE_SET_REG_OFS(slave, emac_regs, soft_reset);
 		GBE_SET_REG_OFS(slave, emac_regs, rx_maxlen);
 
+	} else if (IS_SS_ID_MU(gbe_dev)) {
+		/* Initialize  slave port register offsets */
+		GBENU_SET_REG_OFS(slave, port_regs, port_vlan);
+		GBENU_SET_REG_OFS(slave, port_regs, tx_pri_map);
+		GBENU_SET_REG_OFS(slave, port_regs, sa_lo);
+		GBENU_SET_REG_OFS(slave, port_regs, sa_hi);
+		GBENU_SET_REG_OFS(slave, port_regs, ts_ctl);
+		GBENU_SET_REG_OFS(slave, port_regs, ts_seq_ltype);
+		GBENU_SET_REG_OFS(slave, port_regs, ts_vlan);
+		GBENU_SET_REG_OFS(slave, port_regs, ts_ctl_ltype2);
+		GBENU_SET_REG_OFS(slave, port_regs, ts_ctl2);
+		GBENU_SET_REG_OFS(slave, port_regs, rx_maxlen);
+
+		/* Initialize EMAC register offsets */
+		GBENU_SET_REG_OFS(slave, emac_regs, mac_control);
+		GBENU_SET_REG_OFS(slave, emac_regs, soft_reset);
+
 	} else if (gbe_dev->ss_version == XGBE_SS_VERSION_10) {
 		/* Initialize  slave port register offsets */
 		XGBE_SET_REG_OFS(slave, port_regs, port_vlan);
@@ -1654,6 +2433,8 @@ static void init_secondary_ports(struct gbe_priv *gbe_dev,
 			mac_phy_link = true;
 
 		slave->open = true;
+		if (gbe_dev->num_slaves >= gbe_dev->max_num_slaves)
+			break;
 	}
 
 	/* of_phy_connect() is needed only for MAC-PHY interface */
@@ -1724,24 +2505,41 @@ static int set_xgbe_ethss10_priv(struct gbe_priv *gbe_dev,
 	void __iomem *regs;
 	int ret, i;
 
-	ret = of_address_to_resource(node, 0, &res);
+	ret = of_address_to_resource(node, XGBE_SS_REG_INDEX, &res);
 	if (ret) {
-		dev_err(gbe_dev->dev, "Can't translate of node(%s) address for xgbe subsystem regs\n",
-			node->name);
+		dev_err(gbe_dev->dev,
+			"Can't xlate xgbe of node(%s) ss address at %d\n",
+			node->name, XGBE_SS_REG_INDEX);
 		return ret;
 	}
 
 	regs = devm_ioremap_resource(gbe_dev->dev, &res);
 	if (IS_ERR(regs)) {
-		dev_err(gbe_dev->dev, "Failed to map xgbe register base\n");
+		dev_err(gbe_dev->dev, "Failed to map xgbe ss register base\n");
 		return PTR_ERR(regs);
 	}
 	gbe_dev->ss_regs = regs;
 
+	ret = of_address_to_resource(node, XGBE_SM_REG_INDEX, &res);
+	if (ret) {
+		dev_err(gbe_dev->dev,
+			"Can't xlate xgbe of node(%s) sm address at %d\n",
+			node->name, XGBE_SM_REG_INDEX);
+		return ret;
+	}
+
+	regs = devm_ioremap_resource(gbe_dev->dev, &res);
+	if (IS_ERR(regs)) {
+		dev_err(gbe_dev->dev, "Failed to map xgbe sm register base\n");
+		return PTR_ERR(regs);
+	}
+	gbe_dev->switch_regs = regs;
+
 	ret = of_address_to_resource(node, XGBE_SERDES_REG_INDEX, &res);
 	if (ret) {
-		dev_err(gbe_dev->dev, "Can't translate of node(%s) address for xgbe serdes regs\n",
-			node->name);
+		dev_err(gbe_dev->dev,
+			"Can't xlate xgbe serdes of node(%s) address at %d\n",
+			node->name, XGBE_SERDES_REG_INDEX);
 		return ret;
 	}
 
@@ -1753,9 +2551,9 @@ static int set_xgbe_ethss10_priv(struct gbe_priv *gbe_dev,
 	gbe_dev->xgbe_serdes_regs = regs;
 
 	gbe_dev->hw_stats = devm_kzalloc(gbe_dev->dev,
-					  XGBE10_NUM_STAT_ENTRIES *
-					  (XGBE10_NUM_SLAVES + 1) * sizeof(u64),
-					  GFP_KERNEL);
+				  XGBE10_NUM_STAT_ENTRIES *
+				  (gbe_dev->max_num_ports) * sizeof(u64),
+				  GFP_KERNEL);
 	if (!gbe_dev->hw_stats) {
 		dev_err(gbe_dev->dev, "hw_stats memory allocation failed\n");
 		return -ENOMEM;
@@ -1764,19 +2562,19 @@ static int set_xgbe_ethss10_priv(struct gbe_priv *gbe_dev,
 	gbe_dev->ss_version = XGBE_SS_VERSION_10;
 	gbe_dev->sgmii_port_regs = gbe_dev->ss_regs +
 					XGBE10_SGMII_MODULE_OFFSET;
-	gbe_dev->switch_regs = gbe_dev->ss_regs + XGBE10_SWITCH_MODULE_OFFSET;
 	gbe_dev->host_port_regs = gbe_dev->ss_regs + XGBE10_HOST_PORT_OFFSET;
 
-	for (i = 0; i < XGBE10_NUM_HW_STATS_MOD; i++)
-		gbe_dev->hw_stats_regs[i] = gbe_dev->ss_regs +
+	for (i = 0; i < gbe_dev->max_num_ports; i++)
+		gbe_dev->hw_stats_regs[i] = gbe_dev->switch_regs +
 			XGBE10_HW_STATS_OFFSET + (GBE_HW_STATS_REG_MAP_SZ * i);
 
-	gbe_dev->ale_reg = gbe_dev->ss_regs + XGBE10_ALE_OFFSET;
-	gbe_dev->ale_ports = XGBE10_NUM_ALE_PORTS;
+	gbe_dev->ale_reg = gbe_dev->switch_regs + XGBE10_ALE_OFFSET;
+	gbe_dev->ale_ports = gbe_dev->max_num_ports;
 	gbe_dev->host_port = XGBE10_HOST_PORT_NUM;
 	gbe_dev->ale_entries = XGBE10_NUM_ALE_ENTRIES;
 	gbe_dev->et_stats = xgbe10_et_stats;
 	gbe_dev->num_et_stats = ARRAY_SIZE(xgbe10_et_stats);
+	gbe_dev->stats_en_mask = (1 << (gbe_dev->max_num_ports)) - 1;
 
 	/* Subsystem registers */
 	XGBE_SET_REG_OFS(gbe_dev, ss_regs, id_ver);
@@ -1803,10 +2601,11 @@ static int get_gbe_resource_version(struct gbe_priv *gbe_dev,
 	void __iomem *regs;
 	int ret;
 
-	ret = of_address_to_resource(node, 0, &res);
+	ret = of_address_to_resource(node, GBE_SS_REG_INDEX, &res);
 	if (ret) {
-		dev_err(gbe_dev->dev, "Can't translate of node(%s) address\n",
-			node->name);
+		dev_err(gbe_dev->dev,
+			"Can't translate of node(%s) of gbe ss address at %d\n",
+			node->name, GBE_SS_REG_INDEX);
 		return ret;
 	}
 
@@ -1823,34 +2622,67 @@ static int get_gbe_resource_version(struct gbe_priv *gbe_dev,
 static int set_gbe_ethss14_priv(struct gbe_priv *gbe_dev,
 				struct device_node *node)
 {
+	struct resource res;
 	void __iomem *regs;
-	int i;
+	int i, ret;
+
+	ret = of_address_to_resource(node, GBE_SGMII34_REG_INDEX, &res);
+	if (ret) {
+		dev_err(gbe_dev->dev,
+			"Can't translate of gbe node(%s) address at index %d\n",
+			node->name, GBE_SGMII34_REG_INDEX);
+		return ret;
+	}
+
+	regs = devm_ioremap_resource(gbe_dev->dev, &res);
+	if (IS_ERR(regs)) {
+		dev_err(gbe_dev->dev,
+			"Failed to map gbe sgmii port34 register base\n");
+		return PTR_ERR(regs);
+	}
+	gbe_dev->sgmii_port34_regs = regs;
+
+	ret = of_address_to_resource(node, GBE_SM_REG_INDEX, &res);
+	if (ret) {
+		dev_err(gbe_dev->dev,
+			"Can't translate of gbe node(%s) address at index %d\n",
+			node->name, GBE_SM_REG_INDEX);
+		return ret;
+	}
+
+	regs = devm_ioremap_resource(gbe_dev->dev, &res);
+	if (IS_ERR(regs)) {
+		dev_err(gbe_dev->dev,
+			"Failed to map gbe switch module register base\n");
+		return PTR_ERR(regs);
+	}
+	gbe_dev->switch_regs = regs;
 
 	gbe_dev->hw_stats = devm_kzalloc(gbe_dev->dev,
 					  GBE13_NUM_HW_STAT_ENTRIES *
-					  GBE13_NUM_SLAVES * sizeof(u64),
+					  gbe_dev->max_num_slaves * sizeof(u64),
 					  GFP_KERNEL);
 	if (!gbe_dev->hw_stats) {
 		dev_err(gbe_dev->dev, "hw_stats memory allocation failed\n");
 		return -ENOMEM;
 	}
 
-	regs = gbe_dev->ss_regs;
-	gbe_dev->sgmii_port_regs = regs + GBE13_SGMII_MODULE_OFFSET;
-	gbe_dev->sgmii_port34_regs = regs + GBE13_SGMII34_MODULE_OFFSET;
-	gbe_dev->switch_regs = regs + GBE13_SWITCH_MODULE_OFFSET;
-	gbe_dev->host_port_regs = regs + GBE13_HOST_PORT_OFFSET;
+	gbe_dev->sgmii_port_regs = gbe_dev->ss_regs + GBE13_SGMII_MODULE_OFFSET;
+	gbe_dev->host_port_regs = gbe_dev->switch_regs + GBE13_HOST_PORT_OFFSET;
 
-	for (i = 0; i < GBE13_NUM_HW_STATS_MOD; i++)
-		gbe_dev->hw_stats_regs[i] = regs + GBE13_HW_STATS_OFFSET +
-				(GBE_HW_STATS_REG_MAP_SZ * i);
+	for (i = 0; i < gbe_dev->max_num_slaves; i++) {
+		gbe_dev->hw_stats_regs[i] =
+			gbe_dev->switch_regs + GBE13_HW_STATS_OFFSET +
+			(GBE_HW_STATS_REG_MAP_SZ * i);
+	}
 
-	gbe_dev->ale_reg = regs + GBE13_ALE_OFFSET;
-	gbe_dev->ale_ports = GBE13_NUM_ALE_PORTS;
+	gbe_dev->ale_reg = gbe_dev->switch_regs + GBE13_ALE_OFFSET;
+	gbe_dev->ale_ports = gbe_dev->max_num_ports;
 	gbe_dev->host_port = GBE13_HOST_PORT_NUM;
 	gbe_dev->ale_entries = GBE13_NUM_ALE_ENTRIES;
 	gbe_dev->et_stats = gbe13_et_stats;
 	gbe_dev->num_et_stats = ARRAY_SIZE(gbe13_et_stats);
+	gbe_dev->stats_en_mask = GBE13_REG_VAL_STAT_ENABLE_ALL;
 
 	/* Subsystem registers */
 	GBE_SET_REG_OFS(gbe_dev, ss_regs, id_ver);
@@ -1869,6 +2701,80 @@ static int set_gbe_ethss14_priv(struct gbe_priv *gbe_dev,
 	return 0;
 }
 
+static int set_gbenu_ethss_priv(struct gbe_priv *gbe_dev,
+				struct device_node *node)
+{
+	struct resource res;
+	void __iomem *regs;
+	int i, ret;
+
+	gbe_dev->hw_stats = devm_kzalloc(gbe_dev->dev,
+				  GBENU_NUM_HW_STAT_ENTRIES *
+				  (gbe_dev->max_num_ports) * sizeof(u64),
+				  GFP_KERNEL);
+	if (!gbe_dev->hw_stats) {
+		dev_err(gbe_dev->dev, "hw_stats memory allocation failed\n");
+		return -ENOMEM;
+	}
+
+	ret = of_address_to_resource(node, GBENU_SM_REG_INDEX, &res);
+	if (ret) {
+		dev_err(gbe_dev->dev,
+			"Can't translate of gbenu node(%s) addr at index %d\n",
+			node->name, GBENU_SM_REG_INDEX);
+		return ret;
+	}
+
+	regs = devm_ioremap_resource(gbe_dev->dev, &res);
+	if (IS_ERR(regs)) {
+		dev_err(gbe_dev->dev,
+			"Failed to map gbenu switch module register base\n");
+		return PTR_ERR(regs);
+	}
+	gbe_dev->switch_regs = regs;
+
+	gbe_dev->sgmii_port_regs = gbe_dev->ss_regs + GBENU_SGMII_MODULE_OFFSET;
+	gbe_dev->host_port_regs = gbe_dev->switch_regs + GBENU_HOST_PORT_OFFSET;
+
+	for (i = 0; i < (gbe_dev->max_num_ports); i++)
+		gbe_dev->hw_stats_regs[i] = gbe_dev->switch_regs +
+			GBENU_HW_STATS_OFFSET + (GBENU_HW_STATS_REG_MAP_SZ * i);
+
+	gbe_dev->ale_reg = gbe_dev->switch_regs + GBENU_ALE_OFFSET;
+	gbe_dev->ale_ports = gbe_dev->max_num_ports;
+	gbe_dev->host_port = GBENU_HOST_PORT_NUM;
+	gbe_dev->ale_entries = GBE13_NUM_ALE_ENTRIES;
+	gbe_dev->et_stats = gbenu_et_stats;
+	gbe_dev->stats_en_mask = (1 << (gbe_dev->max_num_ports)) - 1;
+
+	if (IS_SS_ID_NU(gbe_dev))
+		gbe_dev->num_et_stats = GBENU_ET_STATS_HOST_SIZE +
+			(gbe_dev->max_num_slaves * GBENU_ET_STATS_PORT_SIZE);
+	else
+		gbe_dev->num_et_stats = GBENU_ET_STATS_HOST_SIZE +
+					GBENU_ET_STATS_PORT_SIZE;
+
+	/* Subsystem registers */
+	GBENU_SET_REG_OFS(gbe_dev, ss_regs, id_ver);
+
+	/* Switch module registers */
+	GBENU_SET_REG_OFS(gbe_dev, switch_regs, id_ver);
+	GBENU_SET_REG_OFS(gbe_dev, switch_regs, control);
+	GBENU_SET_REG_OFS(gbe_dev, switch_regs, stat_port_en);
+	GBENU_SET_REG_OFS(gbe_dev, switch_regs, ptype);
+
+	/* Host port registers */
+	GBENU_SET_REG_OFS(gbe_dev, host_port_regs, port_vlan);
+	GBENU_SET_REG_OFS(gbe_dev, host_port_regs, rx_maxlen);
+
+	/* For NU only.  2U does not need tx_pri_map.
+	 * NU cppi port 0 tx pkt streaming interface has (n-1)*8 egress threads
+	 * while 2U has only 1 such thread
+	 */
+	GBENU_SET_REG_OFS(gbe_dev, host_port_regs, tx_pri_map);
+	return 0;
+}
+
 static int gbe_probe(struct netcp_device *netcp_device, struct device *dev,
 		     struct device_node *node, void **inst_priv)
 {
@@ -1888,6 +2794,21 @@ static int gbe_probe(struct netcp_device *netcp_device, struct device *dev,
 	if (!gbe_dev)
 		return -ENOMEM;
 
+	if (of_device_is_compatible(node, "ti,netcp-gbe-5") ||
+	    of_device_is_compatible(node, "ti,netcp-gbe")) {
+		gbe_dev->max_num_slaves = 4;
+	} else if (of_device_is_compatible(node, "ti,netcp-gbe-9")) {
+		gbe_dev->max_num_slaves = 8;
+	} else if (of_device_is_compatible(node, "ti,netcp-gbe-2")) {
+		gbe_dev->max_num_slaves = 1;
+	} else if (of_device_is_compatible(node, "ti,netcp-xgbe")) {
+		gbe_dev->max_num_slaves = 2;
+	} else {
+		dev_err(dev, "device tree node for unknown device\n");
+		return -EINVAL;
+	}
+	gbe_dev->max_num_ports = gbe_dev->max_num_slaves + 1;
+
 	gbe_dev->dev = dev;
 	gbe_dev->netcp_device = netcp_device;
 	gbe_dev->rx_packet_max = NETCP_MAX_FRAME_SIZE;
@@ -1923,7 +2844,15 @@ static int gbe_probe(struct netcp_device *netcp_device, struct device *dev,
 		if (ret)
 			goto quit;
 
-		ret = set_gbe_ethss14_priv(gbe_dev, node);
+		dev_dbg(dev, "ss_version: 0x%08x\n", gbe_dev->ss_version);
+
+		if (gbe_dev->ss_version == GBE_SS_VERSION_14)
+			ret = set_gbe_ethss14_priv(gbe_dev, node);
+		else if (IS_SS_ID_MU(gbe_dev))
+			ret = set_gbenu_ethss_priv(gbe_dev, node);
+		else
+			ret = -ENODEV;
+
 		if (ret)
 			goto quit;
 	} else if (!strcmp(node->name, "xgbe")) {
@@ -1963,6 +2892,8 @@ static int gbe_probe(struct netcp_device *netcp_device, struct device *dev,
 			continue;
 		}
 		gbe_dev->num_slaves++;
+		if (gbe_dev->num_slaves >= gbe_dev->max_num_slaves)
+			break;
 	}
 
 	if (!gbe_dev->num_slaves)
@@ -1971,7 +2902,7 @@ static int gbe_probe(struct netcp_device *netcp_device, struct device *dev,
 	/* Initialize Secondary slave ports */
 	secondary_ports = of_get_child_by_name(node, "secondary-slave-ports");
 	INIT_LIST_HEAD(&gbe_dev->secondary_slaves);
-	if (secondary_ports)
+	if (secondary_ports && (gbe_dev->num_slaves <  gbe_dev->max_num_slaves))
 		init_secondary_ports(gbe_dev, secondary_ports);
 	of_node_put(secondary_ports);
 
diff --git a/drivers/net/ethernet/tile/tilegx.c b/drivers/net/ethernet/tile/tilegx.c
index bea8cd2bb56c..a789a2054388 100644
--- a/drivers/net/ethernet/tile/tilegx.c
+++ b/drivers/net/ethernet/tile/tilegx.c
@@ -838,7 +838,8 @@ static int ptp_mpipe_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return ret;
 }
 
-static int ptp_mpipe_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int ptp_mpipe_gettime(struct ptp_clock_info *ptp,
+			     struct timespec64 *ts)
 {
 	int ret = 0;
 	struct mpipe_data *md = container_of(ptp, struct mpipe_data, caps);
@@ -850,7 +851,7 @@ static int ptp_mpipe_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 }
 
 static int ptp_mpipe_settime(struct ptp_clock_info *ptp,
-			     const struct timespec *ts)
+			     const struct timespec64 *ts)
 {
 	int ret = 0;
 	struct mpipe_data *md = container_of(ptp, struct mpipe_data, caps);
@@ -876,8 +877,8 @@ static struct ptp_clock_info ptp_mpipe_caps = {
 	.pps		= 0,
 	.adjfreq	= ptp_mpipe_adjfreq,
 	.adjtime	= ptp_mpipe_adjtime,
-	.gettime	= ptp_mpipe_gettime,
-	.settime	= ptp_mpipe_settime,
+	.gettime64	= ptp_mpipe_gettime,
+	.settime64	= ptp_mpipe_settime,
 	.enable		= ptp_mpipe_enable,
 };
 
diff --git a/drivers/net/ethernet/toshiba/Kconfig b/drivers/net/ethernet/toshiba/Kconfig
index 74acb5cf6099..5d244b6b5e3a 100644
--- a/drivers/net/ethernet/toshiba/Kconfig
+++ b/drivers/net/ethernet/toshiba/Kconfig
@@ -5,7 +5,7 @@
 config NET_VENDOR_TOSHIBA
 	bool "Toshiba devices"
 	default y
-	depends on PCI && (PPC_IBM_CELL_BLADE || PPC_CELLEB || MIPS) || PPC_PS3
+	depends on PCI && (PPC_IBM_CELL_BLADE || MIPS) || PPC_PS3
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y
 	  and read the Ethernet-HOWTO, available from
@@ -42,7 +42,7 @@ config GELIC_WIRELESS
 
 config SPIDER_NET
 	tristate "Spider Gigabit Ethernet driver"
-	depends on PCI && (PPC_IBM_CELL_BLADE || PPC_CELLEB)
+	depends on PCI && PPC_IBM_CELL_BLADE
 	select FW_LOADER
 	select SUNGEM_PHY
 	---help---
diff --git a/drivers/net/ethernet/toshiba/ps3_gelic_net.c b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
index bb7992804664..ac62a5e248b0 100644
--- a/drivers/net/ethernet/toshiba/ps3_gelic_net.c
+++ b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
@@ -1065,7 +1065,7 @@ refill:
 
 	/*
 	 * this call can fail, but for now, just leave this
-	 * decriptor without skb
+	 * descriptor without skb
 	 */
 	gelic_descr_prepare_rx(card, descr);
 
diff --git a/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c b/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c
index 0a7f2e77557f..13214a6492ac 100644
--- a/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c
+++ b/drivers/net/ethernet/toshiba/ps3_gelic_wireless.c
@@ -1167,7 +1167,7 @@ static int gelic_wl_set_ap(struct net_device *netdev,
 	} else {
 		pr_debug("%s: clear bssid\n", __func__);
 		clear_bit(GELIC_WL_STAT_BSSID_SET, &wl->stat);
-		memset(wl->bssid, 0, ETH_ALEN);
+		eth_zero_addr(wl->bssid);
 	}
 	spin_unlock_irqrestore(&wl->lock, irqflag);
 	pr_debug("%s: ->\n", __func__);
@@ -1189,7 +1189,7 @@ static int gelic_wl_get_ap(struct net_device *netdev,
 		memcpy(data->ap_addr.sa_data, wl->active_bssid,
 		       ETH_ALEN);
 	} else
-		memset(data->ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(data->ap_addr.sa_data);
 
 	spin_unlock_irqrestore(&wl->lock, irqflag);
 	mutex_unlock(&wl->assoc_stat_lock);
diff --git a/drivers/net/ethernet/via/via-rhine.c b/drivers/net/ethernet/via/via-rhine.c
index 17e276651601..de2850497c09 100644
--- a/drivers/net/ethernet/via/via-rhine.c
+++ b/drivers/net/ethernet/via/via-rhine.c
@@ -70,12 +70,14 @@ static const int multicast_filter_limit = 32;
 /* Operational parameters that are set at compile time. */
 
 /* Keep the ring sizes a power of two for compile efficiency.
-   The compiler will convert <unsigned>'%'<2^N> into a bit mask.
-   Making the Tx ring too large decreases the effectiveness of channel
-   bonding and packet priority.
-   There are no ill effects from too-large receive rings. */
-#define TX_RING_SIZE	16
-#define TX_QUEUE_LEN	10	/* Limit ring entries actually used. */
+ * The compiler will convert <unsigned>'%'<2^N> into a bit mask.
+ * Making the Tx ring too large decreases the effectiveness of channel
+ * bonding and packet priority.
+ * With BQL support, we can increase TX ring safely.
+ * There are no ill effects from too-large receive rings.
+ */
+#define TX_RING_SIZE	64
+#define TX_QUEUE_LEN	(TX_RING_SIZE - 6)	/* Limit ring entries actually used. */
 #define RX_RING_SIZE	64
 
 /* Operational parameters that usually are not changed. */
@@ -286,7 +288,7 @@ MODULE_DEVICE_TABLE(pci, rhine_pci_tbl);
  * The .data field is currently only used to store quirks
  */
 static u32 vt8500_quirks = rqWOL | rqForceReset | rq6patterns;
-static struct of_device_id rhine_of_tbl[] = {
+static const struct of_device_id rhine_of_tbl[] = {
 	{ .compatible = "via,vt8500-rhine", .data = &vt8500_quirks },
 	{ }	/* terminate list */
 };
@@ -1295,6 +1297,7 @@ static void alloc_tbufs(struct net_device* dev)
 	}
 	rp->tx_ring[i-1].next_desc = cpu_to_le32(rp->tx_ring_dma);
 
+	netdev_reset_queue(dev);
 }
 
 static void free_tbufs(struct net_device* dev)
@@ -1795,6 +1798,7 @@ static netdev_tx_t rhine_start_tx(struct sk_buff *skb,
 	else
 		rp->tx_ring[entry].tx_status = 0;
 
+	netdev_sent_queue(dev, skb->len);
 	/* lock eth irq */
 	wmb();
 	rp->tx_ring[entry].tx_status |= cpu_to_le32(DescOwn);
@@ -1863,6 +1867,8 @@ static void rhine_tx(struct net_device *dev)
 	struct rhine_private *rp = netdev_priv(dev);
 	struct device *hwdev = dev->dev.parent;
 	int txstatus = 0, entry = rp->dirty_tx % TX_RING_SIZE;
+	unsigned int pkts_compl = 0, bytes_compl = 0;
+	struct sk_buff *skb;
 
 	/* find and cleanup dirty tx descriptors */
 	while (rp->dirty_tx != rp->cur_tx) {
@@ -1871,6 +1877,7 @@ static void rhine_tx(struct net_device *dev)
 			  entry, txstatus);
 		if (txstatus & DescOwn)
 			break;
+		skb = rp->tx_skbuff[entry];
 		if (txstatus & 0x8000) {
 			netif_dbg(rp, tx_done, dev,
 				  "Transmit error, Tx status %08x\n", txstatus);
@@ -1899,7 +1906,7 @@ static void rhine_tx(struct net_device *dev)
 				  (txstatus >> 3) & 0xF, txstatus & 0xF);
 
 			u64_stats_update_begin(&rp->tx_stats.syncp);
-			rp->tx_stats.bytes += rp->tx_skbuff[entry]->len;
+			rp->tx_stats.bytes += skb->len;
 			rp->tx_stats.packets++;
 			u64_stats_update_end(&rp->tx_stats.syncp);
 		}
@@ -1907,13 +1914,17 @@ static void rhine_tx(struct net_device *dev)
 		if (rp->tx_skbuff_dma[entry]) {
 			dma_unmap_single(hwdev,
 					 rp->tx_skbuff_dma[entry],
-					 rp->tx_skbuff[entry]->len,
+					 skb->len,
 					 DMA_TO_DEVICE);
 		}
-		dev_consume_skb_any(rp->tx_skbuff[entry]);
+		bytes_compl += skb->len;
+		pkts_compl++;
+		dev_consume_skb_any(skb);
 		rp->tx_skbuff[entry] = NULL;
 		entry = (++rp->dirty_tx) % TX_RING_SIZE;
 	}
+
+	netdev_completed_queue(dev, pkts_compl, bytes_compl);
 	if ((rp->cur_tx - rp->dirty_tx) < TX_QUEUE_LEN - 4)
 		netif_wake_queue(dev);
 }
diff --git a/drivers/net/ethernet/via/via-velocity.c b/drivers/net/ethernet/via/via-velocity.c
index c20206f83cc1..ae68afd50a15 100644
--- a/drivers/net/ethernet/via/via-velocity.c
+++ b/drivers/net/ethernet/via/via-velocity.c
@@ -392,7 +392,7 @@ MODULE_DEVICE_TABLE(pci, velocity_pci_id_table);
  *	Describe the OF device identifiers that we support in this
  *	device driver. Used for devicetree nodes.
  */
-static struct of_device_id velocity_of_ids[] = {
+static const struct of_device_id velocity_of_ids[] = {
 	{ .compatible = "via,velocity-vt6110", .data = &chip_info_table[0] },
 	{ /* Sentinel */ },
 };
diff --git a/drivers/net/ethernet/wiznet/w5100.c b/drivers/net/ethernet/wiznet/w5100.c
index a495931a66a1..8b282d0b169c 100644
--- a/drivers/net/ethernet/wiznet/w5100.c
+++ b/drivers/net/ethernet/wiznet/w5100.c
@@ -56,7 +56,7 @@ MODULE_LICENSE("GPL");
 
 #define W5100_S0_REGS		0x0400
 #define W5100_S0_MR		0x0400 /* S0 Mode Register */
-#define   S0_MR_MACRAW		  0x04 /* MAC RAW mode (promiscous) */
+#define   S0_MR_MACRAW		  0x04 /* MAC RAW mode (promiscuous) */
 #define   S0_MR_MACRAW_MF	  0x44 /* MAC RAW mode (filtered) */
 #define W5100_S0_CR		0x0401 /* S0 Command Register */
 #define   S0_CR_OPEN		  0x01 /* OPEN command */
@@ -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..8da7b930ff59 100644
--- a/drivers/net/ethernet/wiznet/w5300.c
+++ b/drivers/net/ethernet/wiznet/w5300.c
@@ -63,7 +63,7 @@ MODULE_LICENSE("GPL");
 #define   IDR_W5300		  0x5300  /* =0x5300 for WIZnet W5300 */
 #define W5300_S0_MR		0x0200	/* S0 Mode Register */
 #define   S0_MR_CLOSED		  0x0000  /* Close mode */
-#define   S0_MR_MACRAW		  0x0004  /* MAC RAW mode (promiscous) */
+#define   S0_MR_MACRAW		  0x0004  /* MAC RAW mode (promiscuous) */
 #define   S0_MR_MACRAW_MF	  0x0044  /* MAC RAW mode (filtered) */
 #define W5300_S0_CR		0x0202	/* S0 Command Register */
 #define   S0_CR_OPEN		  0x0001  /* OPEN command */
@@ -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/xilinx/ll_temac_main.c b/drivers/net/ethernet/xilinx/ll_temac_main.c
index dbcbf0c5bcfa..690a4c36b316 100644
--- a/drivers/net/ethernet/xilinx/ll_temac_main.c
+++ b/drivers/net/ethernet/xilinx/ll_temac_main.c
@@ -1157,7 +1157,7 @@ static int temac_of_remove(struct platform_device *op)
 	return 0;
 }
 
-static struct of_device_id temac_of_match[] = {
+static const struct of_device_id temac_of_match[] = {
 	{ .compatible = "xlnx,xps-ll-temac-1.01.b", },
 	{ .compatible = "xlnx,xps-ll-temac-2.00.a", },
 	{ .compatible = "xlnx,xps-ll-temac-2.02.a", },
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
index a6d2860b712c..28b7e7d9c272 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
@@ -48,7 +48,7 @@
 #define AXIENET_REGS_N		32
 
 /* Match table for of_platform binding */
-static struct of_device_id axienet_of_match[] = {
+static const struct of_device_id axienet_of_match[] = {
 	{ .compatible = "xlnx,axi-ethernet-1.00.a", },
 	{ .compatible = "xlnx,axi-ethernet-1.01.a", },
 	{ .compatible = "xlnx,axi-ethernet-2.01.a", },
diff --git a/drivers/net/ethernet/xilinx/xilinx_emaclite.c b/drivers/net/ethernet/xilinx/xilinx_emaclite.c
index 9d4ce388510a..6008eee01a33 100644
--- a/drivers/net/ethernet/xilinx/xilinx_emaclite.c
+++ b/drivers/net/ethernet/xilinx/xilinx_emaclite.c
@@ -1062,7 +1062,7 @@ static bool get_bool(struct platform_device *ofdev, const char *s)
 	} else {
 		dev_warn(&ofdev->dev, "Parameter %s not found,"
 			"defaulting to false\n", s);
-		return 0;
+		return false;
 	}
 }
 
@@ -1231,7 +1231,7 @@ static struct net_device_ops xemaclite_netdev_ops = {
 };
 
 /* Match table for OF platform binding */
-static struct of_device_id xemaclite_of_match[] = {
+static const struct of_device_id xemaclite_of_match[] = {
 	{ .compatible = "xlnx,opb-ethernetlite-1.01.a", },
 	{ .compatible = "xlnx,opb-ethernetlite-1.01.b", },
 	{ .compatible = "xlnx,xps-ethernetlite-1.00.a", },
diff --git a/drivers/net/ethernet/xscale/ixp4xx_eth.c b/drivers/net/ethernet/xscale/ixp4xx_eth.c
index 9e16a2819d48..5138407941cf 100644
--- a/drivers/net/ethernet/xscale/ixp4xx_eth.c
+++ b/drivers/net/ethernet/xscale/ixp4xx_eth.c
@@ -954,7 +954,7 @@ static void eth_set_mcast_list(struct net_device *dev)
 		return;
 	}
 
-	memset(diffs, 0, ETH_ALEN);
+	eth_zero_addr(diffs);
 
 	addr = NULL;
 	netdev_for_each_mc_addr(ha, dev) {
diff --git a/drivers/net/hamradio/6pack.c b/drivers/net/hamradio/6pack.c
index daca0dee88f3..7c4a4151ef0f 100644
--- a/drivers/net/hamradio/6pack.c
+++ b/drivers/net/hamradio/6pack.c
@@ -247,6 +247,9 @@ static netdev_tx_t sp_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct sixpack *sp = netdev_priv(dev);
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	spin_lock_bh(&sp->lock);
 	/* We were not busy, so we are now... :-) */
 	netif_stop_queue(dev);
@@ -284,18 +287,6 @@ static int sp_close(struct net_device *dev)
 	return 0;
 }
 
-/* Return the frame type ID */
-static int sp_header(struct sk_buff *skb, struct net_device *dev,
-		     unsigned short type, const void *daddr,
-		     const void *saddr, unsigned len)
-{
-#ifdef CONFIG_INET
-	if (type != ETH_P_AX25)
-		return ax25_hard_header(skb, dev, type, daddr, saddr, len);
-#endif
-	return 0;
-}
-
 static int sp_set_mac_address(struct net_device *dev, void *addr)
 {
 	struct sockaddr_ax25 *sa = addr;
@@ -309,20 +300,6 @@ static int sp_set_mac_address(struct net_device *dev, void *addr)
 	return 0;
 }
 
-static int sp_rebuild_header(struct sk_buff *skb)
-{
-#ifdef CONFIG_INET
-	return ax25_rebuild_header(skb);
-#else
-	return 0;
-#endif
-}
-
-static const struct header_ops sp_header_ops = {
-	.create		= sp_header,
-	.rebuild	= sp_rebuild_header,
-};
-
 static const struct net_device_ops sp_netdev_ops = {
 	.ndo_open		= sp_open_dev,
 	.ndo_stop		= sp_close,
@@ -337,7 +314,7 @@ static void sp_setup(struct net_device *dev)
 	dev->destructor		= free_netdev;
 	dev->mtu		= SIXP_MTU;
 	dev->hard_header_len	= AX25_MAX_HEADER_LEN;
-	dev->header_ops 	= &sp_header_ops;
+	dev->header_ops 	= &ax25_header_ops;
 
 	dev->addr_len		= AX25_ADDR_LEN;
 	dev->type		= ARPHRD_AX25;
diff --git a/drivers/net/hamradio/baycom_epp.c b/drivers/net/hamradio/baycom_epp.c
index a98c153f371e..83c7cce0d172 100644
--- a/drivers/net/hamradio/baycom_epp.c
+++ b/drivers/net/hamradio/baycom_epp.c
@@ -772,6 +772,9 @@ static int baycom_send_packet(struct sk_buff *skb, struct net_device *dev)
 {
 	struct baycom_state *bc = netdev_priv(dev);
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	if (skb->data[0] != 0) {
 		do_kiss_params(bc, skb->data, skb->len);
 		dev_kfree_skb(skb);
diff --git a/drivers/net/hamradio/bpqether.c b/drivers/net/hamradio/bpqether.c
index c2894e43840e..63ff08a26da8 100644
--- a/drivers/net/hamradio/bpqether.c
+++ b/drivers/net/hamradio/bpqether.c
@@ -251,6 +251,9 @@ static netdev_tx_t bpq_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct net_device *orig_dev;
 	int size;
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	/*
 	 * Just to be *really* sure not to send anything if the interface
 	 * is down, the ethernet device may have gone.
diff --git a/drivers/net/hamradio/dmascc.c b/drivers/net/hamradio/dmascc.c
index 0fad408f24aa..c3d377770616 100644
--- a/drivers/net/hamradio/dmascc.c
+++ b/drivers/net/hamradio/dmascc.c
@@ -920,6 +920,9 @@ static int scc_send_packet(struct sk_buff *skb, struct net_device *dev)
 	unsigned long flags;
 	int i;
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	/* Temporarily stop the scheduler feeding us packets */
 	netif_stop_queue(dev);
 
diff --git a/drivers/net/hamradio/hdlcdrv.c b/drivers/net/hamradio/hdlcdrv.c
index c67a27245072..49fe59b180a8 100644
--- a/drivers/net/hamradio/hdlcdrv.c
+++ b/drivers/net/hamradio/hdlcdrv.c
@@ -404,6 +404,9 @@ static netdev_tx_t hdlcdrv_send_packet(struct sk_buff *skb,
 {
 	struct hdlcdrv_state *sm = netdev_priv(dev);
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	if (skb->data[0] != 0) {
 		do_kiss_params(sm, skb->data, skb->len);
 		dev_kfree_skb(skb);
diff --git a/drivers/net/hamradio/mkiss.c b/drivers/net/hamradio/mkiss.c
index f990bb1c3e02..2ffbf13471d0 100644
--- a/drivers/net/hamradio/mkiss.c
+++ b/drivers/net/hamradio/mkiss.c
@@ -529,6 +529,9 @@ static netdev_tx_t ax_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct mkiss *ax = netdev_priv(dev);
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	if (!netif_running(dev))  {
 		printk(KERN_ERR "mkiss: %s: xmit call when iface is down\n", dev->name);
 		return NETDEV_TX_BUSY;
@@ -554,11 +557,9 @@ static netdev_tx_t ax_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	/* We were not busy, so we are now... :-) */
-	if (skb != NULL) {
-		netif_stop_queue(dev);
-		ax_encaps(dev, skb->data, skb->len);
-		kfree_skb(skb);
-	}
+	netif_stop_queue(dev);
+	ax_encaps(dev, skb->data, skb->len);
+	kfree_skb(skb);
 
 	return NETDEV_TX_OK;
 }
@@ -573,32 +574,6 @@ static int ax_open_dev(struct net_device *dev)
 	return 0;
 }
 
-#if defined(CONFIG_AX25) || defined(CONFIG_AX25_MODULE)
-
-/* Return the frame type ID */
-static int ax_header(struct sk_buff *skb, struct net_device *dev,
-		     unsigned short type, const void *daddr,
-		     const void *saddr, unsigned len)
-{
-#ifdef CONFIG_INET
-	if (type != ETH_P_AX25)
-		return ax25_hard_header(skb, dev, type, daddr, saddr, len);
-#endif
-	return 0;
-}
-
-
-static int ax_rebuild_header(struct sk_buff *skb)
-{
-#ifdef CONFIG_INET
-	return ax25_rebuild_header(skb);
-#else
-	return 0;
-#endif
-}
-
-#endif	/* CONFIG_{AX25,AX25_MODULE} */
-
 /* Open the low-level part of the AX25 channel. Easy! */
 static int ax_open(struct net_device *dev)
 {
@@ -662,11 +637,6 @@ static int ax_close(struct net_device *dev)
 	return 0;
 }
 
-static const struct header_ops ax_header_ops = {
-	.create    = ax_header,
-	.rebuild   = ax_rebuild_header,
-};
-
 static const struct net_device_ops ax_netdev_ops = {
 	.ndo_open            = ax_open_dev,
 	.ndo_stop            = ax_close,
@@ -682,7 +652,7 @@ static void ax_setup(struct net_device *dev)
 	dev->addr_len        = 0;
 	dev->type            = ARPHRD_AX25;
 	dev->tx_queue_len    = 10;
-	dev->header_ops      = &ax_header_ops;
+	dev->header_ops      = &ax25_header_ops;
 	dev->netdev_ops	     = &ax_netdev_ops;
 
 
diff --git a/drivers/net/hamradio/scc.c b/drivers/net/hamradio/scc.c
index 57be9e0e98a6..ce88df33fe17 100644
--- a/drivers/net/hamradio/scc.c
+++ b/drivers/net/hamradio/scc.c
@@ -1639,6 +1639,9 @@ static netdev_tx_t scc_net_tx(struct sk_buff *skb, struct net_device *dev)
 	unsigned long flags;
 	char kisscmd;
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	if (skb->len > scc->stat.bufsize || skb->len < 2) {
 		scc->dev_stat.tx_dropped++;	/* bogus frame */
 		dev_kfree_skb(skb);
diff --git a/drivers/net/hamradio/yam.c b/drivers/net/hamradio/yam.c
index 717433cfb81d..1a4729c36aa4 100644
--- a/drivers/net/hamradio/yam.c
+++ b/drivers/net/hamradio/yam.c
@@ -597,6 +597,9 @@ static netdev_tx_t yam_send_packet(struct sk_buff *skb,
 {
 	struct yam_port *yp = netdev_priv(dev);
 
+	if (skb->protocol == htons(ETH_P_IP))
+		return ax25_ip_xmit(skb);
+
 	skb_queue_tail(&yp->send_queue, skb);
 	dev->trans_start = jiffies;
 	return NETDEV_TX_OK;
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index 384ca4f4de4a..a10b31664709 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -128,9 +128,12 @@ struct ndis_tcp_ip_checksum_info;
 struct hv_netvsc_packet {
 	/* Bookkeeping stuff */
 	u32 status;
+	bool part_of_skb;
 
-	struct hv_device *device;
 	bool is_data_pkt;
+	bool xmit_more; /* from skb */
+	bool cp_partial; /* partial copy into send buffer */
+
 	u16 vlan_tci;
 
 	u16 q_idx;
@@ -145,11 +148,14 @@ struct hv_netvsc_packet {
 	/* This points to the memory after page_buf */
 	struct rndis_message *rndis_msg;
 
+	u32 rmsg_size; /* RNDIS header and PPI size */
+	u32 rmsg_pgcnt; /* page count of RNDIS header and PPI */
+
 	u32 total_data_buflen;
 	/* Points to the send/receive buffer where the ethernet frame is */
 	void *data;
 	u32 page_buf_cnt;
-	struct hv_page_buffer page_buf[0];
+	struct hv_page_buffer *page_buf;
 };
 
 struct netvsc_device_info {
@@ -187,6 +193,7 @@ int netvsc_send(struct hv_device *device,
 		struct hv_netvsc_packet *packet);
 void netvsc_linkstatus_callback(struct hv_device *device_obj,
 				struct rndis_message *resp);
+void netvsc_xmit_completion(void *context);
 int netvsc_recv_callback(struct hv_device *device_obj,
 			struct hv_netvsc_packet *packet,
 			struct ndis_tcp_ip_checksum_info *csum_info);
@@ -596,7 +603,16 @@ struct nvsp_message {
 
 #define VRSS_SEND_TAB_SIZE 16
 
-/* Per netvsc channel-specific */
+#define RNDIS_MAX_PKT_DEFAULT 8
+#define RNDIS_PKT_ALIGN_DEFAULT 8
+
+struct multi_send_data {
+	spinlock_t lock; /* protect struct multi_send_data */
+	struct hv_netvsc_packet *pkt; /* netvsc pkt pending */
+	u32 count; /* counter of batched packets */
+};
+
+/* Per netvsc device */
 struct netvsc_device {
 	struct hv_device *dev;
 
@@ -634,6 +650,7 @@ struct netvsc_device {
 
 	struct vmbus_channel *chn_table[NR_CPUS];
 	u32 send_table[VRSS_SEND_TAB_SIZE];
+	u32 max_chn;
 	u32 num_chn;
 	atomic_t queue_sends[NR_CPUS];
 
@@ -646,6 +663,10 @@ struct netvsc_device {
 	unsigned char *cb_buffer;
 	/* The sub channel callback buffer */
 	unsigned char *sub_cb_buf;
+
+	struct multi_send_data msd[NR_CPUS];
+	u32 max_pkt; /* max number of pkt in one send, e.g. 8 */
+	u32 pkt_align; /* alignment bytes, e.g. 8 */
 };
 
 /* NdisInitialize message */
@@ -943,6 +964,10 @@ struct ndis_tcp_lso_info {
 #define NDIS_HASH_PPI_SIZE (sizeof(struct rndis_per_packet_info) + \
 		sizeof(u32))
 
+/* Total size of all PPI data */
+#define NDIS_ALL_PPI_SIZE (NDIS_VLAN_PPI_SIZE + NDIS_CSUM_PPI_SIZE + \
+			   NDIS_LSO_PPI_SIZE + NDIS_HASH_PPI_SIZE)
+
 /* Format of Information buffer passed in a SetRequest for the OID */
 /* OID_GEN_RNDIS_CONFIG_PARAMETER. */
 struct rndis_config_parameter_info {
@@ -1155,6 +1180,8 @@ struct rndis_message {
 #define RNDIS_HEADER_SIZE	(sizeof(struct rndis_message) - \
 				 sizeof(union rndis_message_container))
 
+#define RNDIS_AND_PPI_SIZE (sizeof(struct rndis_message) + NDIS_ALL_PPI_SIZE)
+
 #define NDIS_PACKET_TYPE_DIRECTED	0x00000001
 #define NDIS_PACKET_TYPE_MULTICAST	0x00000002
 #define NDIS_PACKET_TYPE_ALL_MULTICAST	0x00000004
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index 208eb05446ba..2e8ad0636b46 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -37,6 +37,7 @@ static struct netvsc_device *alloc_net_device(struct hv_device *device)
 {
 	struct netvsc_device *net_device;
 	struct net_device *ndev = hv_get_drvdata(device);
+	int i;
 
 	net_device = kzalloc(sizeof(struct netvsc_device), GFP_KERNEL);
 	if (!net_device)
@@ -53,6 +54,11 @@ static struct netvsc_device *alloc_net_device(struct hv_device *device)
 	net_device->destroy = false;
 	net_device->dev = device;
 	net_device->ndev = ndev;
+	net_device->max_pkt = RNDIS_MAX_PKT_DEFAULT;
+	net_device->pkt_align = RNDIS_PKT_ALIGN_DEFAULT;
+
+	for (i = 0; i < num_online_cpus(); i++)
+		spin_lock_init(&net_device->msd[i].lock);
 
 	hv_set_drvdata(device, net_device);
 	return net_device;
@@ -687,14 +693,28 @@ static u32 netvsc_get_next_send_section(struct netvsc_device *net_device)
 
 static u32 netvsc_copy_to_send_buf(struct netvsc_device *net_device,
 				   unsigned int section_index,
+				   u32 pend_size,
 				   struct hv_netvsc_packet *packet)
 {
 	char *start = net_device->send_buf;
-	char *dest = (start + (section_index * net_device->send_section_size));
+	char *dest = start + (section_index * net_device->send_section_size)
+		     + pend_size;
 	int i;
 	u32 msg_size = 0;
+	u32 padding = 0;
+	u32 remain = packet->total_data_buflen % net_device->pkt_align;
+	u32 page_count = packet->cp_partial ? packet->rmsg_pgcnt :
+		packet->page_buf_cnt;
+
+	/* Add padding */
+	if (packet->is_data_pkt && packet->xmit_more && remain &&
+	    !packet->cp_partial) {
+		padding = net_device->pkt_align - remain;
+		packet->rndis_msg->msg_len += padding;
+		packet->total_data_buflen += padding;
+	}
 
-	for (i = 0; i < packet->page_buf_cnt; i++) {
+	for (i = 0; i < page_count; i++) {
 		char *src = phys_to_virt(packet->page_buf[i].pfn << PAGE_SHIFT);
 		u32 offset = packet->page_buf[i].offset;
 		u32 len = packet->page_buf[i].len;
@@ -703,79 +723,64 @@ static u32 netvsc_copy_to_send_buf(struct netvsc_device *net_device,
 		msg_size += len;
 		dest += len;
 	}
+
+	if (padding) {
+		memset(dest, 0, padding);
+		msg_size += padding;
+	}
+
 	return msg_size;
 }
 
-int netvsc_send(struct hv_device *device,
-			struct hv_netvsc_packet *packet)
+static inline int netvsc_send_pkt(
+	struct hv_netvsc_packet *packet,
+	struct netvsc_device *net_device)
 {
-	struct netvsc_device *net_device;
-	int ret = 0;
-	struct nvsp_message sendMessage;
-	struct net_device *ndev;
-	struct vmbus_channel *out_channel = NULL;
-	u64 req_id;
-	unsigned int section_index = NETVSC_INVALID_INDEX;
-	u32 msg_size = 0;
-	struct sk_buff *skb = NULL;
+	struct nvsp_message nvmsg;
+	struct vmbus_channel *out_channel = packet->channel;
 	u16 q_idx = packet->q_idx;
+	struct net_device *ndev = net_device->ndev;
+	u64 req_id;
+	int ret;
+	struct hv_page_buffer *pgbuf;
 
-
-	net_device = get_outbound_net_device(device);
-	if (!net_device)
-		return -ENODEV;
-	ndev = net_device->ndev;
-
-	sendMessage.hdr.msg_type = NVSP_MSG1_TYPE_SEND_RNDIS_PKT;
+	nvmsg.hdr.msg_type = NVSP_MSG1_TYPE_SEND_RNDIS_PKT;
 	if (packet->is_data_pkt) {
 		/* 0 is RMC_DATA; */
-		sendMessage.msg.v1_msg.send_rndis_pkt.channel_type = 0;
+		nvmsg.msg.v1_msg.send_rndis_pkt.channel_type = 0;
 	} else {
 		/* 1 is RMC_CONTROL; */
-		sendMessage.msg.v1_msg.send_rndis_pkt.channel_type = 1;
-	}
-
-	/* Attempt to send via sendbuf */
-	if (packet->total_data_buflen < net_device->send_section_size) {
-		section_index = netvsc_get_next_send_section(net_device);
-		if (section_index != NETVSC_INVALID_INDEX) {
-			msg_size = netvsc_copy_to_send_buf(net_device,
-							   section_index,
-							   packet);
-			skb = (struct sk_buff *)
-			      (unsigned long)packet->send_completion_tid;
-			packet->page_buf_cnt = 0;
-		}
+		nvmsg.msg.v1_msg.send_rndis_pkt.channel_type = 1;
 	}
-	packet->send_buf_index = section_index;
 
-
-	sendMessage.msg.v1_msg.send_rndis_pkt.send_buf_section_index =
-		section_index;
-	sendMessage.msg.v1_msg.send_rndis_pkt.send_buf_section_size = msg_size;
+	nvmsg.msg.v1_msg.send_rndis_pkt.send_buf_section_index =
+		packet->send_buf_index;
+	if (packet->send_buf_index == NETVSC_INVALID_INDEX)
+		nvmsg.msg.v1_msg.send_rndis_pkt.send_buf_section_size = 0;
+	else
+		nvmsg.msg.v1_msg.send_rndis_pkt.send_buf_section_size =
+			packet->total_data_buflen;
 
 	if (packet->send_completion)
 		req_id = (ulong)packet;
 	else
 		req_id = 0;
 
-	out_channel = net_device->chn_table[packet->q_idx];
-	if (out_channel == NULL)
-		out_channel = device->channel;
-	packet->channel = out_channel;
-
 	if (out_channel->rescind)
 		return -ENODEV;
 
 	if (packet->page_buf_cnt) {
+		pgbuf = packet->cp_partial ? packet->page_buf +
+			packet->rmsg_pgcnt : packet->page_buf;
 		ret = vmbus_sendpacket_pagebuffer(out_channel,
-						  packet->page_buf,
+						  pgbuf,
 						  packet->page_buf_cnt,
-						  &sendMessage,
+						  &nvmsg,
 						  sizeof(struct nvsp_message),
 						  req_id);
 	} else {
-		ret = vmbus_sendpacket(out_channel, &sendMessage,
+		ret = vmbus_sendpacket(
+				out_channel, &nvmsg,
 				sizeof(struct nvsp_message),
 				req_id,
 				VM_PKT_DATA_INBAND,
@@ -809,6 +814,121 @@ int netvsc_send(struct hv_device *device,
 			   packet, ret);
 	}
 
+	return ret;
+}
+
+int netvsc_send(struct hv_device *device,
+		struct hv_netvsc_packet *packet)
+{
+	struct netvsc_device *net_device;
+	int ret = 0, m_ret = 0;
+	struct vmbus_channel *out_channel;
+	u16 q_idx = packet->q_idx;
+	u32 pktlen = packet->total_data_buflen, msd_len = 0;
+	unsigned int section_index = NETVSC_INVALID_INDEX;
+	struct sk_buff *skb = NULL;
+	unsigned long flag;
+	struct multi_send_data *msdp;
+	struct hv_netvsc_packet *msd_send = NULL, *cur_send = NULL;
+	bool try_batch;
+
+	net_device = get_outbound_net_device(device);
+	if (!net_device)
+		return -ENODEV;
+
+	out_channel = net_device->chn_table[q_idx];
+	if (!out_channel) {
+		out_channel = device->channel;
+		q_idx = 0;
+		packet->q_idx = 0;
+	}
+	packet->channel = out_channel;
+	packet->send_buf_index = NETVSC_INVALID_INDEX;
+	packet->cp_partial = false;
+
+	msdp = &net_device->msd[q_idx];
+
+	/* batch packets in send buffer if possible */
+	spin_lock_irqsave(&msdp->lock, flag);
+	if (msdp->pkt)
+		msd_len = msdp->pkt->total_data_buflen;
+
+	try_batch = packet->is_data_pkt && msd_len > 0 && msdp->count <
+		    net_device->max_pkt;
+
+	if (try_batch && msd_len + pktlen + net_device->pkt_align <
+	    net_device->send_section_size) {
+		section_index = msdp->pkt->send_buf_index;
+
+	} else if (try_batch && msd_len + packet->rmsg_size <
+		   net_device->send_section_size) {
+		section_index = msdp->pkt->send_buf_index;
+		packet->cp_partial = true;
+
+	} else if (packet->is_data_pkt && pktlen + net_device->pkt_align <
+		   net_device->send_section_size) {
+		section_index = netvsc_get_next_send_section(net_device);
+		if (section_index != NETVSC_INVALID_INDEX) {
+				msd_send = msdp->pkt;
+				msdp->pkt = NULL;
+				msdp->count = 0;
+				msd_len = 0;
+		}
+	}
+
+	if (section_index != NETVSC_INVALID_INDEX) {
+		netvsc_copy_to_send_buf(net_device,
+					section_index, msd_len,
+					packet);
+
+		packet->send_buf_index = section_index;
+
+		if (packet->cp_partial) {
+			packet->page_buf_cnt -= packet->rmsg_pgcnt;
+			packet->total_data_buflen = msd_len + packet->rmsg_size;
+		} else {
+			packet->page_buf_cnt = 0;
+			packet->total_data_buflen += msd_len;
+			if (!packet->part_of_skb) {
+				skb = (struct sk_buff *)(unsigned long)packet->
+				       send_completion_tid;
+				packet->send_completion_tid = 0;
+			}
+		}
+
+		if (msdp->pkt)
+			netvsc_xmit_completion(msdp->pkt);
+
+		if (packet->xmit_more && !packet->cp_partial) {
+			msdp->pkt = packet;
+			msdp->count++;
+		} else {
+			cur_send = packet;
+			msdp->pkt = NULL;
+			msdp->count = 0;
+		}
+	} else {
+		msd_send = msdp->pkt;
+		msdp->pkt = NULL;
+		msdp->count = 0;
+		cur_send = packet;
+	}
+
+	spin_unlock_irqrestore(&msdp->lock, flag);
+
+	if (msd_send) {
+		m_ret = netvsc_send_pkt(msd_send, net_device);
+
+		if (m_ret != 0) {
+			netvsc_free_send_slot(net_device,
+					      msd_send->send_buf_index);
+			netvsc_xmit_completion(msd_send);
+		}
+	}
+
+	if (cur_send)
+		ret = netvsc_send_pkt(cur_send, net_device);
+
 	if (ret != 0) {
 		if (section_index != NETVSC_INVALID_INDEX)
 			netvsc_free_send_slot(net_device, section_index);
@@ -911,7 +1031,6 @@ static void netvsc_receive(struct netvsc_device *net_device,
 	}
 
 	count = vmxferpage_packet->range_cnt;
-	netvsc_packet->device = device;
 	netvsc_packet->channel = channel;
 
 	/* Each range represents 1 RNDIS pkt that contains 1 ethernet frame */
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 15d82eda0baf..a3a9d3898a6e 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -229,16 +229,16 @@ static u16 netvsc_select_queue(struct net_device *ndev, struct sk_buff *skb,
 	return q_idx;
 }
 
-static void netvsc_xmit_completion(void *context)
+void netvsc_xmit_completion(void *context)
 {
 	struct hv_netvsc_packet *packet = (struct hv_netvsc_packet *)context;
 	struct sk_buff *skb = (struct sk_buff *)
 		(unsigned long)packet->send_completion_tid;
-	u32 index = packet->send_buf_index;
 
-	kfree(packet);
+	if (!packet->part_of_skb)
+		kfree(packet);
 
-	if (skb && (index == NETVSC_INVALID_INDEX))
+	if (skb)
 		dev_kfree_skb_any(skb);
 }
 
@@ -277,15 +277,16 @@ static u32 fill_pg_buf(struct page *page, u32 offset, u32 len,
 }
 
 static u32 init_page_array(void *hdr, u32 len, struct sk_buff *skb,
-			   struct hv_page_buffer *pb)
+			   struct hv_netvsc_packet *packet)
 {
+	struct hv_page_buffer *pb = packet->page_buf;
 	u32 slots_used = 0;
 	char *data = skb->data;
 	int frags = skb_shinfo(skb)->nr_frags;
 	int i;
 
 	/* The packet is laid out thus:
-	 * 1. hdr
+	 * 1. hdr: RNDIS header and PPI
 	 * 2. skb linear data
 	 * 3. skb fragment data
 	 */
@@ -294,6 +295,9 @@ static u32 init_page_array(void *hdr, u32 len, struct sk_buff *skb,
 					offset_in_page(hdr),
 					len, &pb[slots_used]);
 
+	packet->rmsg_size = len;
+	packet->rmsg_pgcnt = slots_used;
+
 	slots_used += fill_pg_buf(virt_to_page(data),
 				offset_in_page(data),
 				skb_headlen(skb), &pb[slots_used]);
@@ -370,50 +374,73 @@ not_ip:
 static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 {
 	struct net_device_context *net_device_ctx = netdev_priv(net);
-	struct hv_netvsc_packet *packet;
+	struct hv_netvsc_packet *packet = NULL;
 	int ret;
 	unsigned int num_data_pgs;
 	struct rndis_message *rndis_msg;
 	struct rndis_packet *rndis_pkt;
 	u32 rndis_msg_size;
 	bool isvlan;
+	bool linear = false;
 	struct rndis_per_packet_info *ppi;
 	struct ndis_tcp_ip_checksum_info *csum_info;
 	struct ndis_tcp_lso_info *lso_info;
 	int  hdr_offset;
 	u32 net_trans_info;
 	u32 hash;
-	u32 skb_length = skb->len;
+	u32 skb_length;
+	u32 head_room;
+	u32 pkt_sz;
+	struct hv_page_buffer page_buf[MAX_PAGE_BUFFER_COUNT];
 
 
 	/* We will atmost need two pages to describe the rndis
 	 * header. We can only transmit MAX_PAGE_BUFFER_COUNT number
-	 * of pages in a single packet.
+	 * of pages in a single packet. If skb is scattered around
+	 * more pages we try linearizing it.
 	 */
+
+check_size:
+	skb_length = skb->len;
+	head_room = skb_headroom(skb);
 	num_data_pgs = netvsc_get_slots(skb) + 2;
-	if (num_data_pgs > MAX_PAGE_BUFFER_COUNT) {
-		netdev_err(net, "Packet too big: %u\n", skb->len);
-		dev_kfree_skb(skb);
-		net->stats.tx_dropped++;
-		return NETDEV_TX_OK;
+	if (num_data_pgs > MAX_PAGE_BUFFER_COUNT && linear) {
+		net_alert_ratelimited("packet too big: %u pages (%u bytes)\n",
+				      num_data_pgs, skb->len);
+		ret = -EFAULT;
+		goto drop;
+	} else if (num_data_pgs > MAX_PAGE_BUFFER_COUNT) {
+		if (skb_linearize(skb)) {
+			net_alert_ratelimited("failed to linearize skb\n");
+			ret = -ENOMEM;
+			goto drop;
+		}
+		linear = true;
+		goto check_size;
 	}
 
-	/* Allocate a netvsc packet based on # of frags. */
-	packet = kzalloc(sizeof(struct hv_netvsc_packet) +
-			 (num_data_pgs * sizeof(struct hv_page_buffer)) +
-			 sizeof(struct rndis_message) +
-			 NDIS_VLAN_PPI_SIZE + NDIS_CSUM_PPI_SIZE +
-			 NDIS_LSO_PPI_SIZE + NDIS_HASH_PPI_SIZE, GFP_ATOMIC);
-	if (!packet) {
-		/* out of memory, drop packet */
-		netdev_err(net, "unable to allocate hv_netvsc_packet\n");
-
-		dev_kfree_skb(skb);
-		net->stats.tx_dropped++;
-		return NETDEV_TX_OK;
+	pkt_sz = sizeof(struct hv_netvsc_packet) + RNDIS_AND_PPI_SIZE;
+
+	if (head_room < pkt_sz) {
+		packet = kmalloc(pkt_sz, GFP_ATOMIC);
+		if (!packet) {
+			/* out of memory, drop packet */
+			netdev_err(net, "unable to alloc hv_netvsc_packet\n");
+			ret = -ENOMEM;
+			goto drop;
+		}
+		packet->part_of_skb = false;
+	} else {
+		/* Use the headroom for building up the packet */
+		packet = (struct hv_netvsc_packet *)skb->head;
+		packet->part_of_skb = true;
 	}
 
+	packet->status = 0;
+	packet->xmit_more = skb->xmit_more;
+
 	packet->vlan_tci = skb->vlan_tci;
+	packet->page_buf = page_buf;
 
 	packet->q_idx = skb_get_queue_mapping(skb);
 
@@ -421,8 +448,9 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	packet->total_data_buflen = skb->len;
 
 	packet->rndis_msg = (struct rndis_message *)((unsigned long)packet +
-				sizeof(struct hv_netvsc_packet) +
-				(num_data_pgs * sizeof(struct hv_page_buffer)));
+				sizeof(struct hv_netvsc_packet));
+
+	memset(packet->rndis_msg, 0, RNDIS_AND_PPI_SIZE);
 
 	/* Set the completion routine */
 	packet->send_completion = netvsc_xmit_completion;
@@ -554,7 +582,7 @@ do_send:
 	rndis_msg->msg_len += rndis_msg_size;
 	packet->total_data_buflen = rndis_msg->msg_len;
 	packet->page_buf_cnt = init_page_array(rndis_msg, rndis_msg_size,
-					skb, &packet->page_buf[0]);
+					       skb, packet);
 
 	ret = netvsc_send(net_device_ctx->device_ctx, packet);
 
@@ -563,7 +591,8 @@ drop:
 		net->stats.tx_bytes += skb_length;
 		net->stats.tx_packets++;
 	} else {
-		kfree(packet);
+		if (packet && !packet->part_of_skb)
+			kfree(packet);
 		if (ret != -EAGAIN) {
 			dev_kfree_skb_any(skb);
 			net->stats.tx_dropped++;
@@ -687,6 +716,19 @@ static void netvsc_get_drvinfo(struct net_device *net,
 	strlcpy(info->fw_version, "N/A", sizeof(info->fw_version));
 }
 
+static void netvsc_get_channels(struct net_device *net,
+				struct ethtool_channels *channel)
+{
+	struct net_device_context *net_device_ctx = netdev_priv(net);
+	struct hv_device *dev = net_device_ctx->device_ctx;
+	struct netvsc_device *nvdev = hv_get_drvdata(dev);
+
+	if (nvdev) {
+		channel->max_combined	= nvdev->max_chn;
+		channel->combined_count = nvdev->num_chn;
+	}
+}
+
 static int netvsc_change_mtu(struct net_device *ndev, int mtu)
 {
 	struct net_device_context *ndevctx = netdev_priv(ndev);
@@ -760,6 +802,7 @@ static void netvsc_poll_controller(struct net_device *net)
 static const struct ethtool_ops ethtool_ops = {
 	.get_drvinfo	= netvsc_get_drvinfo,
 	.get_link	= ethtool_op_get_link,
+	.get_channels   = netvsc_get_channels,
 };
 
 static const struct net_device_ops device_ops = {
@@ -831,12 +874,16 @@ static int netvsc_probe(struct hv_device *dev,
 	struct netvsc_device_info device_info;
 	struct netvsc_device *nvdev;
 	int ret;
+	u32 max_needed_headroom;
 
 	net = alloc_etherdev_mq(sizeof(struct net_device_context),
 				num_online_cpus());
 	if (!net)
 		return -ENOMEM;
 
+	max_needed_headroom = sizeof(struct hv_netvsc_packet) +
+			      RNDIS_AND_PPI_SIZE;
+
 	netif_carrier_off(net);
 
 	net_device_ctx = netdev_priv(net);
@@ -855,6 +902,13 @@ static int netvsc_probe(struct hv_device *dev,
 	net->ethtool_ops = &ethtool_ops;
 	SET_NETDEV_DEV(net, &dev->device);
 
+	/*
+	 * Request additional head room in the skb.
+	 * We will use this space to build the rndis
+	 * heaser and other state we need to maintain.
+	 */
+	net->needed_headroom = max_needed_headroom;
+
 	/* Notify the netvsc driver of the new device */
 	device_info.ring_size = ring_size;
 	ret = rndis_filter_device_add(dev, &device_info);
diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index 7816d98bdddc..0d92efefd796 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -47,8 +47,6 @@ struct rndis_request {
 
 	/* Simplify allocation by having a netvsc packet inline */
 	struct hv_netvsc_packet	pkt;
-	/* Set 2 pages for rndis requests crossing page boundary */
-	struct hv_page_buffer buf[2];
 
 	struct rndis_message request_msg;
 	/*
@@ -210,6 +208,7 @@ static int rndis_filter_send_request(struct rndis_device *dev,
 {
 	int ret;
 	struct hv_netvsc_packet *packet;
+	struct hv_page_buffer page_buf[2];
 
 	/* Setup the packet to send it */
 	packet = &req->pkt;
@@ -217,6 +216,7 @@ static int rndis_filter_send_request(struct rndis_device *dev,
 	packet->is_data_pkt = false;
 	packet->total_data_buflen = req->request_msg.msg_len;
 	packet->page_buf_cnt = 1;
+	packet->page_buf = page_buf;
 
 	packet->page_buf[0].pfn = virt_to_phys(&req->request_msg) >>
 					PAGE_SHIFT;
@@ -237,6 +237,7 @@ static int rndis_filter_send_request(struct rndis_device *dev,
 	}
 
 	packet->send_completion = NULL;
+	packet->xmit_more = false;
 
 	ret = netvsc_send(dev->net_dev->dev, packet);
 	return ret;
@@ -855,6 +856,7 @@ static int rndis_filter_init_device(struct rndis_device *dev)
 	u32 status;
 	int ret;
 	unsigned long t;
+	struct netvsc_device *nvdev = dev->net_dev;
 
 	request = get_rndis_request(dev, RNDIS_MSG_INIT,
 			RNDIS_MESSAGE_SIZE(struct rndis_initialize_request));
@@ -889,6 +891,8 @@ static int rndis_filter_init_device(struct rndis_device *dev)
 	status = init_complete->status;
 	if (status == RNDIS_STATUS_SUCCESS) {
 		dev->state = RNDIS_DEV_INITIALIZED;
+		nvdev->max_pkt = init_complete->max_pkt_per_msg;
+		nvdev->pkt_align = 1 << init_complete->pkt_alignment_factor;
 		ret = 0;
 	} else {
 		dev->state = RNDIS_DEV_UNINITIALIZED;
@@ -1027,6 +1031,7 @@ int rndis_filter_device_add(struct hv_device *dev,
 
 	/* Initialize the rndis device */
 	net_device = hv_get_drvdata(dev);
+	net_device->max_chn = 1;
 	net_device->num_chn = 1;
 
 	net_device->extension = rndis_device;
@@ -1094,6 +1099,7 @@ int rndis_filter_device_add(struct hv_device *dev,
 	if (ret || rsscap.num_recv_que < 2)
 		goto out;
 
+	net_device->max_chn = rsscap.num_recv_que;
 	net_device->num_chn = (num_online_cpus() < rsscap.num_recv_que) ?
 			       num_online_cpus() : rsscap.num_recv_que;
 	if (net_device->num_chn == 1)
@@ -1135,13 +1141,13 @@ int rndis_filter_device_add(struct hv_device *dev,
 	net_device->num_chn = 1 +
 		init_packet->msg.v5_msg.subchn_comp.num_subchannels;
 
-	vmbus_are_subchannels_present(dev->channel);
-
 	ret = rndis_filter_set_rss_param(rndis_device, net_device->num_chn);
 
 out:
-	if (ret)
+	if (ret) {
+		net_device->max_chn = 1;
 		net_device->num_chn = 1;
+	}
 	return 0; /* return 0 because primary channel can be used alone */
 
 err_dev_remv:
diff --git a/drivers/net/ieee802154/at86rf230.c b/drivers/net/ieee802154/at86rf230.c
index 7b051eacb7f1..38026650c038 100644
--- a/drivers/net/ieee802154/at86rf230.c
+++ b/drivers/net/ieee802154/at86rf230.c
@@ -19,11 +19,12 @@
  */
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/hrtimer.h>
+#include <linux/jiffies.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/gpio.h>
 #include <linux/delay.h>
-#include <linux/spinlock.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/at86rf230.h>
 #include <linux/regmap.h>
@@ -46,19 +47,27 @@ struct at86rf2xx_chip_data {
 	u16 t_off_to_tx_on;
 	u16 t_frame;
 	u16 t_p_ack;
-	/* completion timeout for tx in msecs */
-	u16 t_tx_timeout;
 	int rssi_base_val;
 
 	int (*set_channel)(struct at86rf230_local *, u8, u8);
 	int (*get_desense_steps)(struct at86rf230_local *, s32);
 };
 
-#define AT86RF2XX_MAX_BUF (127 + 3)
+#define AT86RF2XX_MAX_BUF		(127 + 3)
+/* tx retries to access the TX_ON state
+ * if it's above then force change will be started.
+ *
+ * We assume the max_frame_retries (7) value of 802.15.4 here.
+ */
+#define AT86RF2XX_MAX_TX_RETRIES	7
+/* We use the recommended 5 minutes timeout to recalibrate */
+#define AT86RF2XX_CAL_LOOP_TIMEOUT	(5 * 60 * HZ)
 
 struct at86rf230_state_change {
 	struct at86rf230_local *lp;
+	int irq;
 
+	struct hrtimer timer;
 	struct spi_message msg;
 	struct spi_transfer trx;
 	u8 buf[AT86RF2XX_MAX_BUF];
@@ -83,10 +92,10 @@ struct at86rf230_local {
 	struct at86rf230_state_change irq;
 
 	bool tx_aret;
+	unsigned long cal_timeout;
 	s8 max_frame_retries;
 	bool is_tx;
-	/* spinlock for is_tx protection */
-	spinlock_t lock;
+	u8 tx_retry;
 	struct sk_buff *tx_skb;
 	struct at86rf230_state_change tx;
 };
@@ -313,7 +322,7 @@ at86rf230_read_subreg(struct at86rf230_local *lp,
 	int rc;
 
 	rc = __at86rf230_read(lp, addr, data);
-	if (rc > 0)
+	if (!rc)
 		*data = (*data & mask) >> shift;
 
 	return rc;
@@ -409,6 +418,8 @@ at86rf230_reg_volatile(struct device *dev, unsigned int reg)
 	case RG_PHY_ED_LEVEL:
 	case RG_IRQ_STATUS:
 	case RG_VREG_CTRL:
+	case RG_PLL_CF:
+	case RG_PLL_DCU:
 		return true;
 	default:
 		return false;
@@ -446,6 +457,7 @@ at86rf230_async_error_recover(void *context)
 	struct at86rf230_state_change *ctx = context;
 	struct at86rf230_local *lp = ctx->lp;
 
+	lp->is_tx = 0;
 	at86rf230_async_state_change(lp, ctx, STATE_RX_AACK_ON, NULL, false);
 	ieee802154_wake_queue(lp->hw);
 }
@@ -472,18 +484,25 @@ at86rf230_async_read_reg(struct at86rf230_local *lp, const u8 reg,
 	u8 *tx_buf = ctx->buf;
 
 	tx_buf[0] = (reg & CMD_REG_MASK) | CMD_REG;
-	ctx->trx.len = 2;
 	ctx->msg.complete = complete;
 	ctx->irq_enable = irq_enable;
 	rc = spi_async(lp->spi, &ctx->msg);
 	if (rc) {
 		if (irq_enable)
-			enable_irq(lp->spi->irq);
+			enable_irq(ctx->irq);
 
 		at86rf230_async_error(lp, ctx, rc);
 	}
 }
 
+static inline u8 at86rf230_state_to_force(u8 state)
+{
+	if (state == STATE_TX_ON)
+		return STATE_FORCE_TX_ON;
+	else
+		return STATE_FORCE_TRX_OFF;
+}
+
 static void
 at86rf230_async_state_assert(void *context)
 {
@@ -514,10 +533,21 @@ at86rf230_async_state_assert(void *context)
 			 * in STATE_BUSY_RX_AACK, we run a force state change
 			 * to STATE_TX_ON. This is a timeout handling, if the
 			 * transceiver stucks in STATE_BUSY_RX_AACK.
+			 *
+			 * Additional we do several retries to try to get into
+			 * TX_ON state without forcing. If the retries are
+			 * higher or equal than AT86RF2XX_MAX_TX_RETRIES we
+			 * will do a force change.
 			 */
-			if (ctx->to_state == STATE_TX_ON) {
-				at86rf230_async_state_change(lp, ctx,
-							     STATE_FORCE_TX_ON,
+			if (ctx->to_state == STATE_TX_ON ||
+			    ctx->to_state == STATE_TRX_OFF) {
+				u8 state = ctx->to_state;
+
+				if (lp->tx_retry >= AT86RF2XX_MAX_TX_RETRIES)
+					state = at86rf230_state_to_force(state);
+				lp->tx_retry++;
+
+				at86rf230_async_state_change(lp, ctx, state,
 							     ctx->complete,
 							     ctx->irq_enable);
 				return;
@@ -533,6 +563,19 @@ done:
 		ctx->complete(context);
 }
 
+static enum hrtimer_restart at86rf230_async_state_timer(struct hrtimer *timer)
+{
+	struct at86rf230_state_change *ctx =
+		container_of(timer, struct at86rf230_state_change, timer);
+	struct at86rf230_local *lp = ctx->lp;
+
+	at86rf230_async_read_reg(lp, RG_TRX_STATUS, ctx,
+				 at86rf230_async_state_assert,
+				 ctx->irq_enable);
+
+	return HRTIMER_NORESTART;
+}
+
 /* Do state change timing delay. */
 static void
 at86rf230_async_state_delay(void *context)
@@ -541,6 +584,7 @@ at86rf230_async_state_delay(void *context)
 	struct at86rf230_local *lp = ctx->lp;
 	struct at86rf2xx_chip_data *c = lp->data;
 	bool force = false;
+	ktime_t tim;
 
 	/* The force state changes are will show as normal states in the
 	 * state status subregister. We change the to_state to the
@@ -564,11 +608,15 @@ at86rf230_async_state_delay(void *context)
 	case STATE_TRX_OFF:
 		switch (ctx->to_state) {
 		case STATE_RX_AACK_ON:
-			usleep_range(c->t_off_to_aack, c->t_off_to_aack + 10);
+			tim = ktime_set(0, c->t_off_to_aack * NSEC_PER_USEC);
 			goto change;
 		case STATE_TX_ON:
-			usleep_range(c->t_off_to_tx_on,
-				     c->t_off_to_tx_on + 10);
+			tim = ktime_set(0, c->t_off_to_tx_on * NSEC_PER_USEC);
+			/* state change from TRX_OFF to TX_ON to do a
+			 * calibration, we need to reset the timeout for the
+			 * next one.
+			 */
+			lp->cal_timeout = jiffies + AT86RF2XX_CAL_LOOP_TIMEOUT;
 			goto change;
 		default:
 			break;
@@ -576,14 +624,15 @@ at86rf230_async_state_delay(void *context)
 		break;
 	case STATE_BUSY_RX_AACK:
 		switch (ctx->to_state) {
+		case STATE_TRX_OFF:
 		case STATE_TX_ON:
 			/* Wait for worst case receiving time if we
 			 * didn't make a force change from BUSY_RX_AACK
-			 * to TX_ON.
+			 * to TX_ON or TRX_OFF.
 			 */
 			if (!force) {
-				usleep_range(c->t_frame + c->t_p_ack,
-					     c->t_frame + c->t_p_ack + 1000);
+				tim = ktime_set(0, (c->t_frame + c->t_p_ack) *
+						   NSEC_PER_USEC);
 				goto change;
 			}
 			break;
@@ -595,7 +644,7 @@ at86rf230_async_state_delay(void *context)
 	case STATE_P_ON:
 		switch (ctx->to_state) {
 		case STATE_TRX_OFF:
-			usleep_range(c->t_reset_to_off, c->t_reset_to_off + 10);
+			tim = ktime_set(0, c->t_reset_to_off * NSEC_PER_USEC);
 			goto change;
 		default:
 			break;
@@ -606,12 +655,10 @@ at86rf230_async_state_delay(void *context)
 	}
 
 	/* Default delay is 1us in the most cases */
-	udelay(1);
+	tim = ktime_set(0, NSEC_PER_USEC);
 
 change:
-	at86rf230_async_read_reg(lp, RG_TRX_STATUS, ctx,
-				 at86rf230_async_state_assert,
-				 ctx->irq_enable);
+	hrtimer_start(&ctx->timer, tim, HRTIMER_MODE_REL);
 }
 
 static void
@@ -647,12 +694,11 @@ at86rf230_async_state_change_start(void *context)
 	 */
 	buf[0] = (RG_TRX_STATE & CMD_REG_MASK) | CMD_REG | CMD_WRITE;
 	buf[1] = ctx->to_state;
-	ctx->trx.len = 2;
 	ctx->msg.complete = at86rf230_async_state_delay;
 	rc = spi_async(lp->spi, &ctx->msg);
 	if (rc) {
 		if (ctx->irq_enable)
-			enable_irq(lp->spi->irq);
+			enable_irq(ctx->irq);
 
 		at86rf230_async_error(lp, ctx, rc);
 	}
@@ -689,7 +735,7 @@ at86rf230_sync_state_change_complete(void *context)
 static int
 at86rf230_sync_state_change(struct at86rf230_local *lp, unsigned int state)
 {
-	int rc;
+	unsigned long rc;
 
 	at86rf230_async_state_change(lp, &lp->state, state,
 				     at86rf230_sync_state_change_complete,
@@ -710,11 +756,10 @@ at86rf230_tx_complete(void *context)
 {
 	struct at86rf230_state_change *ctx = context;
 	struct at86rf230_local *lp = ctx->lp;
-	struct sk_buff *skb = lp->tx_skb;
 
-	enable_irq(lp->spi->irq);
+	enable_irq(ctx->irq);
 
-	ieee802154_xmit_complete(lp->hw, skb, !lp->tx_aret);
+	ieee802154_xmit_complete(lp->hw, lp->tx_skb, !lp->tx_aret);
 }
 
 static void
@@ -723,7 +768,7 @@ at86rf230_tx_on(void *context)
 	struct at86rf230_state_change *ctx = context;
 	struct at86rf230_local *lp = ctx->lp;
 
-	at86rf230_async_state_change(lp, &lp->irq, STATE_RX_AACK_ON,
+	at86rf230_async_state_change(lp, ctx, STATE_RX_AACK_ON,
 				     at86rf230_tx_complete, true);
 }
 
@@ -767,14 +812,25 @@ at86rf230_tx_trac_status(void *context)
 }
 
 static void
-at86rf230_rx(struct at86rf230_local *lp,
-	     const u8 *data, const u8 len, const u8 lqi)
+at86rf230_rx_read_frame_complete(void *context)
 {
-	struct sk_buff *skb;
+	struct at86rf230_state_change *ctx = context;
+	struct at86rf230_local *lp = ctx->lp;
 	u8 rx_local_buf[AT86RF2XX_MAX_BUF];
+	const u8 *buf = ctx->buf;
+	struct sk_buff *skb;
+	u8 len, lqi;
+
+	len = buf[1];
+	if (!ieee802154_is_valid_psdu_len(len)) {
+		dev_vdbg(&lp->spi->dev, "corrupted frame received\n");
+		len = IEEE802154_MTU;
+	}
+	lqi = buf[2 + len];
 
-	memcpy(rx_local_buf, data, len);
-	enable_irq(lp->spi->irq);
+	memcpy(rx_local_buf, buf + 2, len);
+	ctx->trx.len = 2;
+	enable_irq(ctx->irq);
 
 	skb = dev_alloc_skb(IEEE802154_MTU);
 	if (!skb) {
@@ -787,60 +843,41 @@ at86rf230_rx(struct at86rf230_local *lp,
 }
 
 static void
-at86rf230_rx_read_frame_complete(void *context)
+at86rf230_rx_read_frame(void *context)
 {
 	struct at86rf230_state_change *ctx = context;
 	struct at86rf230_local *lp = ctx->lp;
-	const u8 *buf = lp->irq.buf;
-	u8 len = buf[1];
-
-	if (!ieee802154_is_valid_psdu_len(len)) {
-		dev_vdbg(&lp->spi->dev, "corrupted frame received\n");
-		len = IEEE802154_MTU;
-	}
-
-	at86rf230_rx(lp, buf + 2, len, buf[2 + len]);
-}
-
-static void
-at86rf230_rx_read_frame(struct at86rf230_local *lp)
-{
+	u8 *buf = ctx->buf;
 	int rc;
 
-	u8 *buf = lp->irq.buf;
-
 	buf[0] = CMD_FB;
-	lp->irq.trx.len = AT86RF2XX_MAX_BUF;
-	lp->irq.msg.complete = at86rf230_rx_read_frame_complete;
-	rc = spi_async(lp->spi, &lp->irq.msg);
+	ctx->trx.len = AT86RF2XX_MAX_BUF;
+	ctx->msg.complete = at86rf230_rx_read_frame_complete;
+	rc = spi_async(lp->spi, &ctx->msg);
 	if (rc) {
-		enable_irq(lp->spi->irq);
-		at86rf230_async_error(lp, &lp->irq, rc);
+		ctx->trx.len = 2;
+		enable_irq(ctx->irq);
+		at86rf230_async_error(lp, ctx, rc);
 	}
 }
 
 static void
 at86rf230_rx_trac_check(void *context)
 {
-	struct at86rf230_state_change *ctx = context;
-	struct at86rf230_local *lp = ctx->lp;
-
 	/* Possible check on trac status here. This could be useful to make
 	 * some stats why receive is failed. Not used at the moment, but it's
 	 * maybe timing relevant. Datasheet doesn't say anything about this.
 	 * The programming guide say do it so.
 	 */
 
-	at86rf230_rx_read_frame(lp);
+	at86rf230_rx_read_frame(context);
 }
 
 static void
 at86rf230_irq_trx_end(struct at86rf230_local *lp)
 {
-	spin_lock(&lp->lock);
 	if (lp->is_tx) {
 		lp->is_tx = 0;
-		spin_unlock(&lp->lock);
 
 		if (lp->tx_aret)
 			at86rf230_async_state_change(lp, &lp->irq,
@@ -853,7 +890,6 @@ at86rf230_irq_trx_end(struct at86rf230_local *lp)
 						     at86rf230_tx_complete,
 						     true);
 	} else {
-		spin_unlock(&lp->lock);
 		at86rf230_async_read_reg(lp, RG_TRX_STATE, &lp->irq,
 					 at86rf230_rx_trac_check, true);
 	}
@@ -864,13 +900,13 @@ at86rf230_irq_status(void *context)
 {
 	struct at86rf230_state_change *ctx = context;
 	struct at86rf230_local *lp = ctx->lp;
-	const u8 *buf = lp->irq.buf;
+	const u8 *buf = ctx->buf;
 	const u8 irq = buf[1];
 
 	if (irq & IRQ_TRX_END) {
 		at86rf230_irq_trx_end(lp);
 	} else {
-		enable_irq(lp->spi->irq);
+		enable_irq(ctx->irq);
 		dev_err(&lp->spi->dev, "not supported irq %02x received\n",
 			irq);
 	}
@@ -886,7 +922,6 @@ static irqreturn_t at86rf230_isr(int irq, void *data)
 	disable_irq_nosync(irq);
 
 	buf[0] = (RG_IRQ_STATUS & CMD_REG_MASK) | CMD_REG;
-	ctx->trx.len = 2;
 	ctx->msg.complete = at86rf230_irq_status;
 	rc = spi_async(lp->spi, &ctx->msg);
 	if (rc) {
@@ -921,21 +956,21 @@ at86rf230_write_frame(void *context)
 	struct at86rf230_state_change *ctx = context;
 	struct at86rf230_local *lp = ctx->lp;
 	struct sk_buff *skb = lp->tx_skb;
-	u8 *buf = lp->tx.buf;
+	u8 *buf = ctx->buf;
 	int rc;
 
-	spin_lock(&lp->lock);
 	lp->is_tx = 1;
-	spin_unlock(&lp->lock);
 
 	buf[0] = CMD_FB | CMD_WRITE;
 	buf[1] = skb->len + 2;
 	memcpy(buf + 2, skb->data, skb->len);
-	lp->tx.trx.len = skb->len + 2;
-	lp->tx.msg.complete = at86rf230_write_frame_complete;
-	rc = spi_async(lp->spi, &lp->tx.msg);
-	if (rc)
+	ctx->trx.len = skb->len + 2;
+	ctx->msg.complete = at86rf230_write_frame_complete;
+	rc = spi_async(lp->spi, &ctx->msg);
+	if (rc) {
+		ctx->trx.len = 2;
 		at86rf230_async_error(lp, ctx, rc);
+	}
 }
 
 static void
@@ -948,24 +983,45 @@ at86rf230_xmit_tx_on(void *context)
 				     at86rf230_write_frame, false);
 }
 
+static void
+at86rf230_xmit_start(void *context)
+{
+	struct at86rf230_state_change *ctx = context;
+	struct at86rf230_local *lp = ctx->lp;
+
+	/* In ARET mode we need to go into STATE_TX_ARET_ON after we
+	 * are in STATE_TX_ON. The pfad differs here, so we change
+	 * the complete handler.
+	 */
+	if (lp->tx_aret)
+		at86rf230_async_state_change(lp, ctx, STATE_TX_ON,
+					     at86rf230_xmit_tx_on, false);
+	else
+		at86rf230_async_state_change(lp, ctx, STATE_TX_ON,
+					     at86rf230_write_frame, false);
+}
+
 static int
 at86rf230_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
 {
 	struct at86rf230_local *lp = hw->priv;
 	struct at86rf230_state_change *ctx = &lp->tx;
 
-	void (*tx_complete)(void *context) = at86rf230_write_frame;
-
 	lp->tx_skb = skb;
+	lp->tx_retry = 0;
 
-	/* In ARET mode we need to go into STATE_TX_ARET_ON after we
-	 * are in STATE_TX_ON. The pfad differs here, so we change
-	 * the complete handler.
+	/* After 5 minutes in PLL and the same frequency we run again the
+	 * calibration loops which is recommended by at86rf2xx datasheets.
+	 *
+	 * The calibration is initiate by a state change from TRX_OFF
+	 * to TX_ON, the lp->cal_timeout should be reinit by state_delay
+	 * function then to start in the next 5 minutes.
 	 */
-	if (lp->tx_aret)
-		tx_complete = at86rf230_xmit_tx_on;
-
-	at86rf230_async_state_change(lp, ctx, STATE_TX_ON, tx_complete, false);
+	if (time_is_before_jiffies(lp->cal_timeout))
+		at86rf230_async_state_change(lp, ctx, STATE_TRX_OFF,
+					     at86rf230_xmit_start, false);
+	else
+		at86rf230_xmit_start(ctx);
 
 	return 0;
 }
@@ -981,6 +1037,9 @@ at86rf230_ed(struct ieee802154_hw *hw, u8 *level)
 static int
 at86rf230_start(struct ieee802154_hw *hw)
 {
+	struct at86rf230_local *lp = hw->priv;
+
+	lp->cal_timeout = jiffies + AT86RF2XX_CAL_LOOP_TIMEOUT;
 	return at86rf230_sync_state_change(hw->priv, STATE_RX_AACK_ON);
 }
 
@@ -1061,6 +1120,8 @@ at86rf230_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
 	/* Wait for PLL */
 	usleep_range(lp->data->t_channel_switch,
 		     lp->data->t_channel_switch + 10);
+
+	lp->cal_timeout = jiffies + AT86RF2XX_CAL_LOOP_TIMEOUT;
 	return rc;
 }
 
@@ -1112,7 +1173,7 @@ at86rf230_set_hw_addr_filt(struct ieee802154_hw *hw,
 }
 
 static int
-at86rf230_set_txpower(struct ieee802154_hw *hw, int db)
+at86rf230_set_txpower(struct ieee802154_hw *hw, s8 db)
 {
 	struct at86rf230_local *lp = hw->priv;
 
@@ -1281,7 +1342,6 @@ static struct at86rf2xx_chip_data at86rf233_data = {
 	.t_off_to_tx_on = 80,
 	.t_frame = 4096,
 	.t_p_ack = 545,
-	.t_tx_timeout = 2000,
 	.rssi_base_val = -91,
 	.set_channel = at86rf23x_set_channel,
 	.get_desense_steps = at86rf23x_get_desens_steps
@@ -1295,7 +1355,6 @@ static struct at86rf2xx_chip_data at86rf231_data = {
 	.t_off_to_tx_on = 110,
 	.t_frame = 4096,
 	.t_p_ack = 545,
-	.t_tx_timeout = 2000,
 	.rssi_base_val = -91,
 	.set_channel = at86rf23x_set_channel,
 	.get_desense_steps = at86rf23x_get_desens_steps
@@ -1309,13 +1368,12 @@ static struct at86rf2xx_chip_data at86rf212_data = {
 	.t_off_to_tx_on = 200,
 	.t_frame = 4096,
 	.t_p_ack = 545,
-	.t_tx_timeout = 2000,
 	.rssi_base_val = -100,
 	.set_channel = at86rf212_set_channel,
 	.get_desense_steps = at86rf212_get_desens_steps
 };
 
-static int at86rf230_hw_init(struct at86rf230_local *lp)
+static int at86rf230_hw_init(struct at86rf230_local *lp, u8 xtal_trim)
 {
 	int rc, irq_type, irq_pol = IRQ_ACTIVE_HIGH;
 	unsigned int dvdd;
@@ -1326,7 +1384,12 @@ static int at86rf230_hw_init(struct at86rf230_local *lp)
 		return rc;
 
 	irq_type = irq_get_trigger_type(lp->spi->irq);
-	if (irq_type == IRQ_TYPE_EDGE_FALLING)
+	if (irq_type == IRQ_TYPE_EDGE_RISING ||
+	    irq_type == IRQ_TYPE_EDGE_FALLING)
+		dev_warn(&lp->spi->dev,
+			 "Using edge triggered irq's are not recommended!\n");
+	if (irq_type == IRQ_TYPE_EDGE_FALLING ||
+	    irq_type == IRQ_TYPE_LEVEL_LOW)
 		irq_pol = IRQ_ACTIVE_LOW;
 
 	rc = at86rf230_write_subreg(lp, SR_IRQ_POLARITY, irq_pol);
@@ -1341,6 +1404,11 @@ static int at86rf230_hw_init(struct at86rf230_local *lp)
 	if (rc)
 		return rc;
 
+	/* reset values differs in at86rf231 and at86rf233 */
+	rc = at86rf230_write_subreg(lp, SR_IRQ_MASK_MODE, 0);
+	if (rc)
+		return rc;
+
 	get_random_bytes(csma_seed, ARRAY_SIZE(csma_seed));
 	rc = at86rf230_write_subreg(lp, SR_CSMA_SEED_0, csma_seed[0]);
 	if (rc)
@@ -1362,6 +1430,45 @@ static int at86rf230_hw_init(struct at86rf230_local *lp)
 	usleep_range(lp->data->t_sleep_cycle,
 		     lp->data->t_sleep_cycle + 100);
 
+	/* xtal_trim value is calculated by:
+	 * CL = 0.5 * (CX + CTRIM + CPAR)
+	 *
+	 * whereas:
+	 * CL = capacitor of used crystal
+	 * CX = connected capacitors at xtal pins
+	 * CPAR = in all at86rf2xx datasheets this is a constant value 3 pF,
+	 *	  but this is different on each board setup. You need to fine
+	 *	  tuning this value via CTRIM.
+	 * CTRIM = variable capacitor setting. Resolution is 0.3 pF range is
+	 *	   0 pF upto 4.5 pF.
+	 *
+	 * Examples:
+	 * atben transceiver:
+	 *
+	 * CL = 8 pF
+	 * CX = 12 pF
+	 * CPAR = 3 pF (We assume the magic constant from datasheet)
+	 * CTRIM = 0.9 pF
+	 *
+	 * (12+0.9+3)/2 = 7.95 which is nearly at 8 pF
+	 *
+	 * xtal_trim = 0x3
+	 *
+	 * openlabs transceiver:
+	 *
+	 * CL = 16 pF
+	 * CX = 22 pF
+	 * CPAR = 3 pF (We assume the magic constant from datasheet)
+	 * CTRIM = 4.5 pF
+	 *
+	 * (22+4.5+3)/2 = 14.75 which is the nearest value to 16 pF
+	 *
+	 * xtal_trim = 0xf
+	 */
+	rc = at86rf230_write_subreg(lp, SR_XTAL_TRIM, xtal_trim);
+	if (rc)
+		return rc;
+
 	rc = at86rf230_read_subreg(lp, SR_DVDD_OK, &dvdd);
 	if (rc)
 		return rc;
@@ -1377,24 +1484,30 @@ static int at86rf230_hw_init(struct at86rf230_local *lp)
 	return at86rf230_write_subreg(lp, SR_SLOTTED_OPERATION, 0);
 }
 
-static struct at86rf230_platform_data *
-at86rf230_get_pdata(struct spi_device *spi)
+static int
+at86rf230_get_pdata(struct spi_device *spi, int *rstn, int *slp_tr,
+		    u8 *xtal_trim)
 {
-	struct at86rf230_platform_data *pdata;
+	struct at86rf230_platform_data *pdata = spi->dev.platform_data;
+	int ret;
 
-	if (!IS_ENABLED(CONFIG_OF) || !spi->dev.of_node)
-		return spi->dev.platform_data;
+	if (!IS_ENABLED(CONFIG_OF) || !spi->dev.of_node) {
+		if (!pdata)
+			return -ENOENT;
 
-	pdata = devm_kzalloc(&spi->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata)
-		goto done;
+		*rstn = pdata->rstn;
+		*slp_tr = pdata->slp_tr;
+		*xtal_trim = pdata->xtal_trim;
+		return 0;
+	}
 
-	pdata->rstn = of_get_named_gpio(spi->dev.of_node, "reset-gpio", 0);
-	pdata->slp_tr = of_get_named_gpio(spi->dev.of_node, "sleep-gpio", 0);
+	*rstn = of_get_named_gpio(spi->dev.of_node, "reset-gpio", 0);
+	*slp_tr = of_get_named_gpio(spi->dev.of_node, "sleep-gpio", 0);
+	ret = of_property_read_u8(spi->dev.of_node, "xtal-trim", xtal_trim);
+	if (ret < 0 && ret != -EINVAL)
+		return ret;
 
-	spi->dev.platform_data = pdata;
-done:
-	return pdata;
+	return 0;
 }
 
 static int
@@ -1478,66 +1591,78 @@ static void
 at86rf230_setup_spi_messages(struct at86rf230_local *lp)
 {
 	lp->state.lp = lp;
+	lp->state.irq = lp->spi->irq;
 	spi_message_init(&lp->state.msg);
 	lp->state.msg.context = &lp->state;
+	lp->state.trx.len = 2;
 	lp->state.trx.tx_buf = lp->state.buf;
 	lp->state.trx.rx_buf = lp->state.buf;
 	spi_message_add_tail(&lp->state.trx, &lp->state.msg);
+	hrtimer_init(&lp->state.timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	lp->state.timer.function = at86rf230_async_state_timer;
 
 	lp->irq.lp = lp;
+	lp->irq.irq = lp->spi->irq;
 	spi_message_init(&lp->irq.msg);
 	lp->irq.msg.context = &lp->irq;
+	lp->irq.trx.len = 2;
 	lp->irq.trx.tx_buf = lp->irq.buf;
 	lp->irq.trx.rx_buf = lp->irq.buf;
 	spi_message_add_tail(&lp->irq.trx, &lp->irq.msg);
+	hrtimer_init(&lp->irq.timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	lp->irq.timer.function = at86rf230_async_state_timer;
 
 	lp->tx.lp = lp;
+	lp->tx.irq = lp->spi->irq;
 	spi_message_init(&lp->tx.msg);
 	lp->tx.msg.context = &lp->tx;
+	lp->tx.trx.len = 2;
 	lp->tx.trx.tx_buf = lp->tx.buf;
 	lp->tx.trx.rx_buf = lp->tx.buf;
 	spi_message_add_tail(&lp->tx.trx, &lp->tx.msg);
+	hrtimer_init(&lp->tx.timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	lp->tx.timer.function = at86rf230_async_state_timer;
 }
 
 static int at86rf230_probe(struct spi_device *spi)
 {
-	struct at86rf230_platform_data *pdata;
 	struct ieee802154_hw *hw;
 	struct at86rf230_local *lp;
 	unsigned int status;
-	int rc, irq_type;
+	int rc, irq_type, rstn, slp_tr;
+	u8 xtal_trim = 0;
 
 	if (!spi->irq) {
 		dev_err(&spi->dev, "no IRQ specified\n");
 		return -EINVAL;
 	}
 
-	pdata = at86rf230_get_pdata(spi);
-	if (!pdata) {
-		dev_err(&spi->dev, "no platform_data\n");
-		return -EINVAL;
+	rc = at86rf230_get_pdata(spi, &rstn, &slp_tr, &xtal_trim);
+	if (rc < 0) {
+		dev_err(&spi->dev, "failed to parse platform_data: %d\n", rc);
+		return rc;
 	}
 
-	if (gpio_is_valid(pdata->rstn)) {
-		rc = devm_gpio_request_one(&spi->dev, pdata->rstn,
+	if (gpio_is_valid(rstn)) {
+		rc = devm_gpio_request_one(&spi->dev, rstn,
 					   GPIOF_OUT_INIT_HIGH, "rstn");
 		if (rc)
 			return rc;
 	}
 
-	if (gpio_is_valid(pdata->slp_tr)) {
-		rc = devm_gpio_request_one(&spi->dev, pdata->slp_tr,
+	if (gpio_is_valid(slp_tr)) {
+		rc = devm_gpio_request_one(&spi->dev, slp_tr,
 					   GPIOF_OUT_INIT_LOW, "slp_tr");
 		if (rc)
 			return rc;
 	}
 
 	/* Reset */
-	if (gpio_is_valid(pdata->rstn)) {
+	if (gpio_is_valid(rstn)) {
 		udelay(1);
-		gpio_set_value(pdata->rstn, 0);
+		gpio_set_value(rstn, 0);
 		udelay(1);
-		gpio_set_value(pdata->rstn, 1);
+		gpio_set_value(rstn, 1);
 		usleep_range(120, 240);
 	}
 
@@ -1566,12 +1691,11 @@ static int at86rf230_probe(struct spi_device *spi)
 	if (rc < 0)
 		goto free_dev;
 
-	spin_lock_init(&lp->lock);
 	init_completion(&lp->state_complete);
 
 	spi_set_drvdata(spi, lp);
 
-	rc = at86rf230_hw_init(lp);
+	rc = at86rf230_hw_init(lp, xtal_trim);
 	if (rc)
 		goto free_dev;
 
diff --git a/drivers/net/ieee802154/cc2520.c b/drivers/net/ieee802154/cc2520.c
index 181b349b060e..f833b8bb6663 100644
--- a/drivers/net/ieee802154/cc2520.c
+++ b/drivers/net/ieee802154/cc2520.c
@@ -714,11 +714,45 @@ static irqreturn_t cc2520_sfd_isr(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+static int cc2520_get_platform_data(struct spi_device *spi,
+				    struct cc2520_platform_data *pdata)
+{
+	struct device_node *np = spi->dev.of_node;
+	struct cc2520_private *priv = spi_get_drvdata(spi);
+
+	if (!np) {
+		struct cc2520_platform_data *spi_pdata = spi->dev.platform_data;
+		if (!spi_pdata)
+			return -ENOENT;
+		*pdata = *spi_pdata;
+		return 0;
+	}
+
+	pdata->fifo = of_get_named_gpio(np, "fifo-gpio", 0);
+	priv->fifo_pin = pdata->fifo;
+
+	pdata->fifop = of_get_named_gpio(np, "fifop-gpio", 0);
+
+	pdata->sfd = of_get_named_gpio(np, "sfd-gpio", 0);
+	pdata->cca = of_get_named_gpio(np, "cca-gpio", 0);
+	pdata->vreg = of_get_named_gpio(np, "vreg-gpio", 0);
+	pdata->reset = of_get_named_gpio(np, "reset-gpio", 0);
+
+	pdata->amplified = of_property_read_bool(np, "amplified");
+
+	return 0;
+}
+
 static int cc2520_hw_init(struct cc2520_private *priv)
 {
 	u8 status = 0, state = 0xff;
 	int ret;
 	int timeout = 100;
+	struct cc2520_platform_data pdata;
+
+	ret = cc2520_get_platform_data(priv->spi, &pdata);
+	if (ret)
+		goto err_ret;
 
 	ret = cc2520_read_register(priv, CC2520_FSMSTAT1, &state);
 	if (ret)
@@ -741,11 +775,47 @@ static int cc2520_hw_init(struct cc2520_private *priv)
 
 	dev_vdbg(&priv->spi->dev, "oscillator brought up\n");
 
-	/* Registers default value: section 28.1 in Datasheet */
-	ret = cc2520_write_register(priv, CC2520_TXPOWER, 0xF7);
-	if (ret)
-		goto err_ret;
+	/* If the CC2520 is connected to a CC2591 amplifier, we must both
+	 * configure GPIOs on the CC2520 to correctly configure the CC2591
+	 * and change a couple settings of the CC2520 to work with the
+	 * amplifier. See section 8 page 17 of TI application note AN065.
+	 * http://www.ti.com/lit/an/swra229a/swra229a.pdf
+	 */
+	if (pdata.amplified) {
+		ret = cc2520_write_register(priv, CC2520_TXPOWER, 0xF9);
+		if (ret)
+			goto err_ret;
+
+		ret = cc2520_write_register(priv, CC2520_AGCCTRL1, 0x16);
+		if (ret)
+			goto err_ret;
+
+		ret = cc2520_write_register(priv, CC2520_GPIOCTRL0, 0x46);
+		if (ret)
+			goto err_ret;
+
+		ret = cc2520_write_register(priv, CC2520_GPIOCTRL5, 0x47);
+		if (ret)
+			goto err_ret;
+
+		ret = cc2520_write_register(priv, CC2520_GPIOPOLARITY, 0x1e);
+		if (ret)
+			goto err_ret;
+
+		ret = cc2520_write_register(priv, CC2520_TXCTRL, 0xc1);
+		if (ret)
+			goto err_ret;
+	} else {
+		ret = cc2520_write_register(priv, CC2520_TXPOWER, 0xF7);
+		if (ret)
+			goto err_ret;
 
+		ret = cc2520_write_register(priv, CC2520_AGCCTRL1, 0x11);
+		if (ret)
+			goto err_ret;
+	}
+
+	/* Registers default value: section 28.1 in Datasheet */
 	ret = cc2520_write_register(priv, CC2520_CCACTRL0, 0x1A);
 	if (ret)
 		goto err_ret;
@@ -770,10 +840,6 @@ static int cc2520_hw_init(struct cc2520_private *priv)
 	if (ret)
 		goto err_ret;
 
-	ret = cc2520_write_register(priv, CC2520_AGCCTRL1, 0x11);
-	if (ret)
-		goto err_ret;
-
 	ret = cc2520_write_register(priv, CC2520_ADCTEST0, 0x10);
 	if (ret)
 		goto err_ret;
@@ -808,40 +874,10 @@ err_ret:
 	return ret;
 }
 
-static struct cc2520_platform_data *
-cc2520_get_platform_data(struct spi_device *spi)
-{
-	struct cc2520_platform_data *pdata;
-	struct device_node *np = spi->dev.of_node;
-	struct cc2520_private *priv = spi_get_drvdata(spi);
-
-	if (!np)
-		return spi->dev.platform_data;
-
-	pdata = devm_kzalloc(&spi->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata)
-		goto done;
-
-	pdata->fifo = of_get_named_gpio(np, "fifo-gpio", 0);
-	priv->fifo_pin = pdata->fifo;
-
-	pdata->fifop = of_get_named_gpio(np, "fifop-gpio", 0);
-
-	pdata->sfd = of_get_named_gpio(np, "sfd-gpio", 0);
-	pdata->cca = of_get_named_gpio(np, "cca-gpio", 0);
-	pdata->vreg = of_get_named_gpio(np, "vreg-gpio", 0);
-	pdata->reset = of_get_named_gpio(np, "reset-gpio", 0);
-
-	spi->dev.platform_data = pdata;
-
-done:
-	return pdata;
-}
-
 static int cc2520_probe(struct spi_device *spi)
 {
 	struct cc2520_private *priv;
-	struct cc2520_platform_data *pdata;
+	struct cc2520_platform_data pdata;
 	int ret;
 
 	priv = devm_kzalloc(&spi->dev, sizeof(*priv), GFP_KERNEL);
@@ -850,8 +886,8 @@ static int cc2520_probe(struct spi_device *spi)
 
 	spi_set_drvdata(spi, priv);
 
-	pdata = cc2520_get_platform_data(spi);
-	if (!pdata) {
+	ret = cc2520_get_platform_data(spi, &pdata);
+	if (ret < 0) {
 		dev_err(&spi->dev, "no platform data\n");
 		return -EINVAL;
 	}
@@ -869,76 +905,76 @@ static int cc2520_probe(struct spi_device *spi)
 	init_completion(&priv->tx_complete);
 
 	/* Request all the gpio's */
-	if (!gpio_is_valid(pdata->fifo)) {
+	if (!gpio_is_valid(pdata.fifo)) {
 		dev_err(&spi->dev, "fifo gpio is not valid\n");
 		ret = -EINVAL;
 		goto err_hw_init;
 	}
 
-	ret = devm_gpio_request_one(&spi->dev, pdata->fifo,
+	ret = devm_gpio_request_one(&spi->dev, pdata.fifo,
 				    GPIOF_IN, "fifo");
 	if (ret)
 		goto err_hw_init;
 
-	if (!gpio_is_valid(pdata->cca)) {
+	if (!gpio_is_valid(pdata.cca)) {
 		dev_err(&spi->dev, "cca gpio is not valid\n");
 		ret = -EINVAL;
 		goto err_hw_init;
 	}
 
-	ret = devm_gpio_request_one(&spi->dev, pdata->cca,
+	ret = devm_gpio_request_one(&spi->dev, pdata.cca,
 				    GPIOF_IN, "cca");
 	if (ret)
 		goto err_hw_init;
 
-	if (!gpio_is_valid(pdata->fifop)) {
+	if (!gpio_is_valid(pdata.fifop)) {
 		dev_err(&spi->dev, "fifop gpio is not valid\n");
 		ret = -EINVAL;
 		goto err_hw_init;
 	}
 
-	ret = devm_gpio_request_one(&spi->dev, pdata->fifop,
+	ret = devm_gpio_request_one(&spi->dev, pdata.fifop,
 				    GPIOF_IN, "fifop");
 	if (ret)
 		goto err_hw_init;
 
-	if (!gpio_is_valid(pdata->sfd)) {
+	if (!gpio_is_valid(pdata.sfd)) {
 		dev_err(&spi->dev, "sfd gpio is not valid\n");
 		ret = -EINVAL;
 		goto err_hw_init;
 	}
 
-	ret = devm_gpio_request_one(&spi->dev, pdata->sfd,
+	ret = devm_gpio_request_one(&spi->dev, pdata.sfd,
 				    GPIOF_IN, "sfd");
 	if (ret)
 		goto err_hw_init;
 
-	if (!gpio_is_valid(pdata->reset)) {
+	if (!gpio_is_valid(pdata.reset)) {
 		dev_err(&spi->dev, "reset gpio is not valid\n");
 		ret = -EINVAL;
 		goto err_hw_init;
 	}
 
-	ret = devm_gpio_request_one(&spi->dev, pdata->reset,
+	ret = devm_gpio_request_one(&spi->dev, pdata.reset,
 				    GPIOF_OUT_INIT_LOW, "reset");
 	if (ret)
 		goto err_hw_init;
 
-	if (!gpio_is_valid(pdata->vreg)) {
+	if (!gpio_is_valid(pdata.vreg)) {
 		dev_err(&spi->dev, "vreg gpio is not valid\n");
 		ret = -EINVAL;
 		goto err_hw_init;
 	}
 
-	ret = devm_gpio_request_one(&spi->dev, pdata->vreg,
+	ret = devm_gpio_request_one(&spi->dev, pdata.vreg,
 				    GPIOF_OUT_INIT_LOW, "vreg");
 	if (ret)
 		goto err_hw_init;
 
-	gpio_set_value(pdata->vreg, HIGH);
+	gpio_set_value(pdata.vreg, HIGH);
 	usleep_range(100, 150);
 
-	gpio_set_value(pdata->reset, HIGH);
+	gpio_set_value(pdata.reset, HIGH);
 	usleep_range(200, 250);
 
 	ret = cc2520_hw_init(priv);
@@ -947,7 +983,7 @@ static int cc2520_probe(struct spi_device *spi)
 
 	/* Set up fifop interrupt */
 	ret = devm_request_irq(&spi->dev,
-			       gpio_to_irq(pdata->fifop),
+			       gpio_to_irq(pdata.fifop),
 			       cc2520_fifop_isr,
 			       IRQF_TRIGGER_RISING,
 			       dev_name(&spi->dev),
@@ -959,7 +995,7 @@ static int cc2520_probe(struct spi_device *spi)
 
 	/* Set up sfd interrupt */
 	ret = devm_request_irq(&spi->dev,
-			       gpio_to_irq(pdata->sfd),
+			       gpio_to_irq(pdata.sfd),
 			       cc2520_sfd_isr,
 			       IRQF_TRIGGER_FALLING,
 			       dev_name(&spi->dev),
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..c30b5c300c05 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
@@ -330,7 +342,7 @@ static int ipvlan_process_v4_outbound(struct sk_buff *skb)
 	struct rtable *rt;
 	int err, ret = NET_XMIT_DROP;
 	struct flowi4 fl4 = {
-		.flowi4_oif = dev->iflink,
+		.flowi4_oif = dev_get_iflink(dev),
 		.flowi4_tos = RT_TOS(ip4h->tos),
 		.flowi4_flags = FLOWI_FLAG_ANYSRC,
 		.daddr = ip4h->daddr,
diff --git a/drivers/net/ipvlan/ipvlan_main.c b/drivers/net/ipvlan/ipvlan_main.c
index 4f4099d5603d..77b92a0fe557 100644
--- a/drivers/net/ipvlan/ipvlan_main.c
+++ b/drivers/net/ipvlan/ipvlan_main.c
@@ -114,7 +114,6 @@ static int ipvlan_init(struct net_device *dev)
 	dev->features = phy_dev->features & IPVLAN_FEATURES;
 	dev->features |= NETIF_F_LLTX;
 	dev->gso_max_size = phy_dev->gso_max_size;
-	dev->iflink = phy_dev->ifindex;
 	dev->hard_header_len = phy_dev->hard_header_len;
 
 	ipvlan_set_lockdep_class(dev);
@@ -305,6 +304,13 @@ static int ipvlan_vlan_rx_kill_vid(struct net_device *dev, __be16 proto,
 	return 0;
 }
 
+static int ipvlan_get_iflink(const struct net_device *dev)
+{
+	struct ipvl_dev *ipvlan = netdev_priv(dev);
+
+	return ipvlan->phy_dev->ifindex;
+}
+
 static const struct net_device_ops ipvlan_netdev_ops = {
 	.ndo_init		= ipvlan_init,
 	.ndo_uninit		= ipvlan_uninit,
@@ -317,6 +323,7 @@ static const struct net_device_ops ipvlan_netdev_ops = {
 	.ndo_get_stats64	= ipvlan_get_stats64,
 	.ndo_vlan_rx_add_vid	= ipvlan_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= ipvlan_vlan_rx_kill_vid,
+	.ndo_get_iflink		= ipvlan_get_iflink,
 };
 
 static int ipvlan_hard_header(struct sk_buff *skb, struct net_device *dev,
@@ -336,7 +343,6 @@ static int ipvlan_hard_header(struct sk_buff *skb, struct net_device *dev,
 
 static const struct header_ops ipvlan_header_ops = {
 	.create  	= ipvlan_hard_header,
-	.rebuild	= eth_rebuild_header,
 	.parse		= eth_header_parse,
 	.cache		= eth_header_cache,
 	.cache_update	= eth_header_cache_update,
@@ -505,7 +511,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 +613,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 +626,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 +641,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 +685,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 +698,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 +714,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/macvlan.c b/drivers/net/macvlan.c
index 1df38bdae2ee..b227a13f6473 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -550,7 +550,6 @@ static int macvlan_hard_header(struct sk_buff *skb, struct net_device *dev,
 
 static const struct header_ops macvlan_hard_header_ops = {
 	.create  	= macvlan_hard_header,
-	.rebuild	= eth_rebuild_header,
 	.parse		= eth_header_parse,
 	.cache		= eth_header_cache,
 	.cache_update	= eth_header_cache_update,
@@ -787,7 +786,6 @@ static int macvlan_init(struct net_device *dev)
 	dev->hw_features	|= NETIF_F_LRO;
 	dev->vlan_features	= lowerdev->vlan_features & MACVLAN_FEATURES;
 	dev->gso_max_size	= lowerdev->gso_max_size;
-	dev->iflink		= lowerdev->ifindex;
 	dev->hard_header_len	= lowerdev->hard_header_len;
 
 	macvlan_set_lockdep_class(dev);
@@ -996,6 +994,13 @@ static void macvlan_dev_netpoll_cleanup(struct net_device *dev)
 }
 #endif	/* CONFIG_NET_POLL_CONTROLLER */
 
+static int macvlan_dev_get_iflink(const struct net_device *dev)
+{
+	struct macvlan_dev *vlan = netdev_priv(dev);
+
+	return vlan->lowerdev->ifindex;
+}
+
 static const struct ethtool_ops macvlan_ethtool_ops = {
 	.get_link		= ethtool_op_get_link,
 	.get_settings		= macvlan_ethtool_get_settings,
@@ -1026,6 +1031,7 @@ static const struct net_device_ops macvlan_netdev_ops = {
 	.ndo_netpoll_setup	= macvlan_dev_netpoll_setup,
 	.ndo_netpoll_cleanup	= macvlan_dev_netpoll_cleanup,
 #endif
+	.ndo_get_iflink		= macvlan_dev_get_iflink,
 };
 
 void macvlan_common_setup(struct net_device *dev)
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 27ecc5c4fa26..9c91ff872485 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -1118,8 +1118,6 @@ static const struct file_operations macvtap_fops = {
 	.owner		= THIS_MODULE,
 	.open		= macvtap_open,
 	.release	= macvtap_release,
-	.read		= new_sync_read,
-	.write		= new_sync_write,
 	.read_iter	= macvtap_read_iter,
 	.write_iter	= macvtap_write_iter,
 	.poll		= macvtap_poll,
@@ -1130,16 +1128,15 @@ static const struct file_operations macvtap_fops = {
 #endif
 };
 
-static int macvtap_sendmsg(struct kiocb *iocb, struct socket *sock,
-			   struct msghdr *m, size_t total_len)
+static int macvtap_sendmsg(struct socket *sock, struct msghdr *m,
+			   size_t total_len)
 {
 	struct macvtap_queue *q = container_of(sock, struct macvtap_queue, sock);
 	return macvtap_get_user(q, m, &m->msg_iter, m->msg_flags & MSG_DONTWAIT);
 }
 
-static int macvtap_recvmsg(struct kiocb *iocb, struct socket *sock,
-			   struct msghdr *m, size_t total_len,
-			   int flags)
+static int macvtap_recvmsg(struct socket *sock, struct msghdr *m,
+			   size_t total_len, int flags)
 {
 	struct macvtap_queue *q = container_of(sock, struct macvtap_queue, sock);
 	int ret;
diff --git a/drivers/net/netconsole.c b/drivers/net/netconsole.c
index ba2f5e710af1..15731d1db918 100644
--- a/drivers/net/netconsole.c
+++ b/drivers/net/netconsole.c
@@ -47,6 +47,7 @@
 #include <linux/netpoll.h>
 #include <linux/inet.h>
 #include <linux/configfs.h>
+#include <linux/etherdevice.h>
 
 MODULE_AUTHOR("Maintainer: Matt Mackall <mpm@selenic.com>");
 MODULE_DESCRIPTION("Console driver for network interfaces");
@@ -185,7 +186,7 @@ static struct netconsole_target *alloc_param_target(char *target_config)
 	nt->np.local_port = 6665;
 	nt->np.remote_port = 6666;
 	mutex_init(&nt->mutex);
-	memset(nt->np.remote_mac, 0xff, ETH_ALEN);
+	eth_broadcast_addr(nt->np.remote_mac);
 
 	/* Parse parameters and setup netpoll */
 	err = netpoll_parse_options(&nt->np, target_config);
@@ -604,7 +605,7 @@ static struct config_item *make_netconsole_target(struct config_group *group,
 	nt->np.local_port = 6665;
 	nt->np.remote_port = 6666;
 	mutex_init(&nt->mutex);
-	memset(nt->np.remote_mac, 0xff, ETH_ALEN);
+	eth_broadcast_addr(nt->np.remote_mac);
 
 	/* Initialize the config_item member */
 	config_item_init_type_name(&nt->item, name, &netconsole_target_type);
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 16adbc481772..8fadaa14b9f0 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -68,8 +68,8 @@ config SMSC_PHY
 config BROADCOM_PHY
 	tristate "Drivers for Broadcom PHYs"
 	---help---
-	  Currently supports the BCM5411, BCM5421, BCM5461, BCM5464, BCM5481
-	  and BCM5482 PHYs.
+	  Currently supports the BCM5411, BCM5421, BCM5461, BCM54616S, BCM5464,
+	  BCM5481 and BCM5482 PHYs.
 
 config BCM63XX_PHY
 	tristate "Drivers for Broadcom 63xx SOCs internal PHY"
diff --git a/drivers/net/phy/amd-xgbe-phy.c b/drivers/net/phy/amd-xgbe-phy.c
index 32efbd48f326..fb276f64cd64 100644
--- a/drivers/net/phy/amd-xgbe-phy.c
+++ b/drivers/net/phy/amd-xgbe-phy.c
@@ -78,6 +78,7 @@
 #include <linux/bitops.h>
 #include <linux/property.h>
 #include <linux/acpi.h>
+#include <linux/jiffies.h>
 
 MODULE_AUTHOR("Tom Lendacky <thomas.lendacky@amd.com>");
 MODULE_LICENSE("Dual BSD/GPL");
@@ -100,6 +101,8 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #define XGBE_PHY_SPEED_2500		1
 #define XGBE_PHY_SPEED_10000		2
 
+#define XGBE_AN_MS_TIMEOUT		500
+
 #define XGBE_AN_INT_CMPLT		0x01
 #define XGBE_AN_INC_LINK		0x02
 #define XGBE_AN_PG_RCV			0x04
@@ -434,6 +437,7 @@ struct amd_xgbe_phy_priv {
 	unsigned int an_supported;
 	unsigned int parallel_detect;
 	unsigned int fec_ability;
+	unsigned long an_start;
 
 	unsigned int lpm_ctrl;		/* CTRL1 for resume */
 };
@@ -902,8 +906,23 @@ static enum amd_xgbe_phy_an amd_xgbe_an_page_received(struct phy_device *phydev)
 {
 	struct amd_xgbe_phy_priv *priv = phydev->priv;
 	enum amd_xgbe_phy_rx *state;
+	unsigned long an_timeout;
 	int ret;
 
+	if (!priv->an_start) {
+		priv->an_start = jiffies;
+	} else {
+		an_timeout = priv->an_start +
+			     msecs_to_jiffies(XGBE_AN_MS_TIMEOUT);
+		if (time_after(jiffies, an_timeout)) {
+			/* Auto-negotiation timed out, reset state */
+			priv->kr_state = AMD_XGBE_RX_BPA;
+			priv->kx_state = AMD_XGBE_RX_BPA;
+
+			priv->an_start = jiffies;
+		}
+	}
+
 	state = amd_xgbe_phy_in_kr_mode(phydev) ? &priv->kr_state
 						: &priv->kx_state;
 
@@ -932,8 +951,8 @@ static enum amd_xgbe_phy_an amd_xgbe_an_incompat_link(struct phy_device *phydev)
 	if (amd_xgbe_phy_in_kr_mode(phydev)) {
 		priv->kr_state = AMD_XGBE_RX_ERROR;
 
-		if (!(phydev->supported & SUPPORTED_1000baseKX_Full) &&
-		    !(phydev->supported & SUPPORTED_2500baseX_Full))
+		if (!(phydev->advertising & SUPPORTED_1000baseKX_Full) &&
+		    !(phydev->advertising & SUPPORTED_2500baseX_Full))
 			return AMD_XGBE_AN_NO_LINK;
 
 		if (priv->kx_state != AMD_XGBE_RX_BPA)
@@ -941,7 +960,7 @@ static enum amd_xgbe_phy_an amd_xgbe_an_incompat_link(struct phy_device *phydev)
 	} else {
 		priv->kx_state = AMD_XGBE_RX_ERROR;
 
-		if (!(phydev->supported & SUPPORTED_10000baseKR_Full))
+		if (!(phydev->advertising & SUPPORTED_10000baseKR_Full))
 			return AMD_XGBE_AN_NO_LINK;
 
 		if (priv->kr_state != AMD_XGBE_RX_BPA)
@@ -1078,6 +1097,7 @@ again:
 		priv->an_state = AMD_XGBE_AN_READY;
 		priv->kr_state = AMD_XGBE_RX_BPA;
 		priv->kx_state = AMD_XGBE_RX_BPA;
+		priv->an_start = 0;
 	}
 
 	if (cur_state != priv->an_state)
@@ -1101,7 +1121,7 @@ static int amd_xgbe_an_init(struct phy_device *phydev)
 	if (ret < 0)
 		return ret;
 
-	if (phydev->supported & SUPPORTED_10000baseR_FEC)
+	if (phydev->advertising & SUPPORTED_10000baseR_FEC)
 		ret |= 0xc000;
 	else
 		ret &= ~0xc000;
@@ -1113,13 +1133,13 @@ static int amd_xgbe_an_init(struct phy_device *phydev)
 	if (ret < 0)
 		return ret;
 
-	if (phydev->supported & SUPPORTED_10000baseKR_Full)
+	if (phydev->advertising & SUPPORTED_10000baseKR_Full)
 		ret |= 0x80;
 	else
 		ret &= ~0x80;
 
-	if ((phydev->supported & SUPPORTED_1000baseKX_Full) ||
-	    (phydev->supported & SUPPORTED_2500baseX_Full))
+	if ((phydev->advertising & SUPPORTED_1000baseKX_Full) ||
+	    (phydev->advertising & SUPPORTED_2500baseX_Full))
 		ret |= 0x20;
 	else
 		ret &= ~0x20;
@@ -1131,12 +1151,12 @@ static int amd_xgbe_an_init(struct phy_device *phydev)
 	if (ret < 0)
 		return ret;
 
-	if (phydev->supported & SUPPORTED_Pause)
+	if (phydev->advertising & SUPPORTED_Pause)
 		ret |= 0x400;
 	else
 		ret &= ~0x400;
 
-	if (phydev->supported & SUPPORTED_Asym_Pause)
+	if (phydev->advertising & SUPPORTED_Asym_Pause)
 		ret |= 0x800;
 	else
 		ret &= ~0x800;
@@ -1212,38 +1232,14 @@ static int amd_xgbe_phy_config_init(struct phy_device *phydev)
 		priv->an_irq_allocated = 1;
 	}
 
-	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_FEC_ABILITY);
-	if (ret < 0)
-		return ret;
-	priv->fec_ability = ret & XGBE_PHY_FEC_MASK;
-
-	/* Initialize supported features */
-	phydev->supported = SUPPORTED_Autoneg;
-	phydev->supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
-	phydev->supported |= SUPPORTED_Backplane;
-	phydev->supported |= SUPPORTED_10000baseKR_Full;
-	switch (priv->speed_set) {
-	case AMD_XGBE_PHY_SPEEDSET_1000_10000:
-		phydev->supported |= SUPPORTED_1000baseKX_Full;
-		break;
-	case AMD_XGBE_PHY_SPEEDSET_2500_10000:
-		phydev->supported |= SUPPORTED_2500baseX_Full;
-		break;
-	}
-
-	if (priv->fec_ability & XGBE_PHY_FEC_ENABLE)
-		phydev->supported |= SUPPORTED_10000baseR_FEC;
-
-	phydev->advertising = phydev->supported;
-
 	/* Set initial mode - call the mode setting routines
 	 * directly to insure we are properly configured
 	 */
-	if (phydev->supported & SUPPORTED_10000baseKR_Full)
+	if (phydev->advertising & SUPPORTED_10000baseKR_Full)
 		ret = amd_xgbe_phy_xgmii_mode(phydev);
-	else if (phydev->supported & SUPPORTED_1000baseKX_Full)
+	else if (phydev->advertising & SUPPORTED_1000baseKX_Full)
 		ret = amd_xgbe_phy_gmii_mode(phydev);
-	else if (phydev->supported & SUPPORTED_2500baseX_Full)
+	else if (phydev->advertising & SUPPORTED_2500baseX_Full)
 		ret = amd_xgbe_phy_gmii_2500_mode(phydev);
 	else
 		ret = -EINVAL;
@@ -1315,10 +1311,10 @@ static int __amd_xgbe_phy_config_aneg(struct phy_device *phydev)
 	disable_irq(priv->an_irq);
 
 	/* Start auto-negotiation in a supported mode */
-	if (phydev->supported & SUPPORTED_10000baseKR_Full)
+	if (phydev->advertising & SUPPORTED_10000baseKR_Full)
 		ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KR);
-	else if ((phydev->supported & SUPPORTED_1000baseKX_Full) ||
-		 (phydev->supported & SUPPORTED_2500baseX_Full))
+	else if ((phydev->advertising & SUPPORTED_1000baseKX_Full) ||
+		 (phydev->advertising & SUPPORTED_2500baseX_Full))
 		ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KX);
 	else
 		ret = -EINVAL;
@@ -1746,6 +1742,29 @@ static int amd_xgbe_phy_probe(struct phy_device *phydev)
 		       sizeof(priv->serdes_dfe_tap_ena));
 	}
 
+	/* Initialize supported features */
+	phydev->supported = SUPPORTED_Autoneg;
+	phydev->supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
+	phydev->supported |= SUPPORTED_Backplane;
+	phydev->supported |= SUPPORTED_10000baseKR_Full;
+	switch (priv->speed_set) {
+	case AMD_XGBE_PHY_SPEEDSET_1000_10000:
+		phydev->supported |= SUPPORTED_1000baseKX_Full;
+		break;
+	case AMD_XGBE_PHY_SPEEDSET_2500_10000:
+		phydev->supported |= SUPPORTED_2500baseX_Full;
+		break;
+	}
+
+	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_FEC_ABILITY);
+	if (ret < 0)
+		return ret;
+	priv->fec_ability = ret & XGBE_PHY_FEC_MASK;
+	if (priv->fec_ability & XGBE_PHY_FEC_ENABLE)
+		phydev->supported |= SUPPORTED_10000baseR_FEC;
+
+	phydev->advertising = phydev->supported;
+
 	phydev->priv = priv;
 
 	if (!priv->adev || acpi_disabled)
@@ -1817,6 +1836,7 @@ static struct phy_driver amd_xgbe_phy_driver[] = {
 		.phy_id_mask		= XGBE_PHY_MASK,
 		.name			= "AMD XGBE PHY",
 		.features		= 0,
+		.flags			= PHY_IS_INTERNAL,
 		.probe			= amd_xgbe_phy_probe,
 		.remove			= amd_xgbe_phy_remove,
 		.soft_reset		= amd_xgbe_phy_soft_reset,
diff --git a/drivers/net/phy/at803x.c b/drivers/net/phy/at803x.c
index f80e19ac6704..fabf11d32d27 100644
--- a/drivers/net/phy/at803x.c
+++ b/drivers/net/phy/at803x.c
@@ -192,16 +192,17 @@ static int at803x_probe(struct phy_device *phydev)
 {
 	struct device *dev = &phydev->dev;
 	struct at803x_priv *priv;
+	struct gpio_desc *gpiod_reset;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
-	priv->gpiod_reset = devm_gpiod_get(dev, "reset");
-	if (IS_ERR(priv->gpiod_reset))
-		priv->gpiod_reset = NULL;
-	else
-		gpiod_direction_output(priv->gpiod_reset, 1);
+	gpiod_reset = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(gpiod_reset))
+		return PTR_ERR(gpiod_reset);
+
+	priv->gpiod_reset = gpiod_reset;
 
 	phydev->priv = priv;
 
diff --git a/drivers/net/phy/bcm7xxx.c b/drivers/net/phy/bcm7xxx.c
index 974ec4515269..64c74c6a4828 100644
--- a/drivers/net/phy/bcm7xxx.c
+++ b/drivers/net/phy/bcm7xxx.c
@@ -396,6 +396,7 @@ static struct phy_driver bcm7xxx_driver[] = {
 	BCM7XXX_28NM_GPHY(PHY_ID_BCM7364, "Broadcom BCM7364"),
 	BCM7XXX_28NM_GPHY(PHY_ID_BCM7366, "Broadcom BCM7366"),
 	BCM7XXX_28NM_GPHY(PHY_ID_BCM7439, "Broadcom BCM7439"),
+	BCM7XXX_28NM_GPHY(PHY_ID_BCM7439_2, "Broadcom BCM7439 (2)"),
 	BCM7XXX_28NM_GPHY(PHY_ID_BCM7445, "Broadcom BCM7445"),
 {
 	.phy_id         = PHY_ID_BCM7425,
diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index a52afb26421b..9c71295f2fef 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -549,6 +549,19 @@ static struct phy_driver broadcom_drivers[] = {
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
 }, {
+	.phy_id		= PHY_ID_BCM54616S,
+	.phy_id_mask	= 0xfffffff0,
+	.name		= "Broadcom BCM54616S",
+	.features	= PHY_GBIT_FEATURES |
+			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
+	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.config_init	= bcm54xx_config_init,
+	.config_aneg	= genphy_config_aneg,
+	.read_status	= genphy_read_status,
+	.ack_interrupt	= bcm54xx_ack_interrupt,
+	.config_intr	= bcm54xx_config_intr,
+	.driver		= { .owner = THIS_MODULE },
+}, {
 	.phy_id		= PHY_ID_BCM5464,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5464",
@@ -660,6 +673,7 @@ static struct mdio_device_id __maybe_unused broadcom_tbl[] = {
 	{ PHY_ID_BCM5411, 0xfffffff0 },
 	{ PHY_ID_BCM5421, 0xfffffff0 },
 	{ PHY_ID_BCM5461, 0xfffffff0 },
+	{ PHY_ID_BCM54616S, 0xfffffff0 },
 	{ PHY_ID_BCM5464, 0xfffffff0 },
 	{ PHY_ID_BCM5482, 0xfffffff0 },
 	{ PHY_ID_BCM5482, 0xfffffff0 },
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index e22e602beef3..496e02f961d3 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -257,7 +257,7 @@ static void ext_write(int broadcast, struct phy_device *phydev,
 
 /* Caller must hold extreg_lock. */
 static int tdr_write(int bc, struct phy_device *dev,
-		     const struct timespec *ts, u16 cmd)
+		     const struct timespec64 *ts, u16 cmd)
 {
 	ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec & 0xffff);/* ns[15:0]  */
 	ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec >> 16);   /* ns[31:16] */
@@ -411,12 +411,12 @@ static int ptp_dp83640_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	struct dp83640_clock *clock =
 		container_of(ptp, struct dp83640_clock, caps);
 	struct phy_device *phydev = clock->chosen->phydev;
-	struct timespec ts;
+	struct timespec64 ts;
 	int err;
 
 	delta += ADJTIME_FIX;
 
-	ts = ns_to_timespec(delta);
+	ts = ns_to_timespec64(delta);
 
 	mutex_lock(&clock->extreg_lock);
 
@@ -427,7 +427,8 @@ static int ptp_dp83640_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return err;
 }
 
-static int ptp_dp83640_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int ptp_dp83640_gettime(struct ptp_clock_info *ptp,
+			       struct timespec64 *ts)
 {
 	struct dp83640_clock *clock =
 		container_of(ptp, struct dp83640_clock, caps);
@@ -452,7 +453,7 @@ static int ptp_dp83640_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 }
 
 static int ptp_dp83640_settime(struct ptp_clock_info *ptp,
-			       const struct timespec *ts)
+			       const struct timespec64 *ts)
 {
 	struct dp83640_clock *clock =
 		container_of(ptp, struct dp83640_clock, caps);
@@ -605,7 +606,7 @@ static void recalibrate(struct dp83640_clock *clock)
 {
 	s64 now, diff;
 	struct phy_txts event_ts;
-	struct timespec ts;
+	struct timespec64 ts;
 	struct list_head *this;
 	struct dp83640_private *tmp;
 	struct phy_device *master = clock->chosen->phydev;
@@ -614,7 +615,7 @@ static void recalibrate(struct dp83640_clock *clock)
 	trigger = CAL_TRIGGER;
 	cal_gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PHYSYNC, 0);
 	if (cal_gpio < 1) {
-		pr_err("PHY calibration pin not avaible - PHY is not calibrated.");
+		pr_err("PHY calibration pin not available - PHY is not calibrated.");
 		return;
 	}
 
@@ -697,7 +698,7 @@ static void recalibrate(struct dp83640_clock *clock)
 		diff = now - (s64) phy2txts(&event_ts);
 		pr_info("slave offset %lld nanoseconds\n", diff);
 		diff += ADJTIME_FIX;
-		ts = ns_to_timespec(diff);
+		ts = ns_to_timespec64(diff);
 		tdr_write(0, tmp->phydev, &ts, PTP_STEP_CLK);
 	}
 
@@ -998,8 +999,8 @@ static void dp83640_clock_init(struct dp83640_clock *clock, struct mii_bus *bus)
 	clock->caps.pps		= 0;
 	clock->caps.adjfreq	= ptp_dp83640_adjfreq;
 	clock->caps.adjtime	= ptp_dp83640_adjtime;
-	clock->caps.gettime	= ptp_dp83640_gettime;
-	clock->caps.settime	= ptp_dp83640_settime;
+	clock->caps.gettime64	= ptp_dp83640_gettime;
+	clock->caps.settime64	= ptp_dp83640_settime;
 	clock->caps.enable	= ptp_dp83640_enable;
 	clock->caps.verify	= ptp_dp83640_verify;
 	/*
diff --git a/drivers/net/phy/fixed_phy.c b/drivers/net/phy/fixed_phy.c
index a08a3c78ba97..1960b46add65 100644
--- a/drivers/net/phy/fixed_phy.c
+++ b/drivers/net/phy/fixed_phy.c
@@ -183,6 +183,35 @@ int fixed_phy_set_link_update(struct phy_device *phydev,
 }
 EXPORT_SYMBOL_GPL(fixed_phy_set_link_update);
 
+int fixed_phy_update_state(struct phy_device *phydev,
+			   const struct fixed_phy_status *status,
+			   const struct fixed_phy_status *changed)
+{
+	struct fixed_mdio_bus *fmb = &platform_fmb;
+	struct fixed_phy *fp;
+
+	if (!phydev || !phydev->bus)
+		return -EINVAL;
+
+	list_for_each_entry(fp, &fmb->phys, node) {
+		if (fp->addr == phydev->addr) {
+#define _UPD(x) if (changed->x) \
+	fp->status.x = status->x
+			_UPD(link);
+			_UPD(speed);
+			_UPD(duplex);
+			_UPD(pause);
+			_UPD(asym_pause);
+#undef _UPD
+			fixed_phy_update_regs(fp);
+			return 0;
+		}
+	}
+
+	return -ENOENT;
+}
+EXPORT_SYMBOL(fixed_phy_update_state);
+
 int fixed_phy_add(unsigned int irq, int phy_addr,
 		  struct fixed_phy_status *status)
 {
diff --git a/drivers/net/phy/mdio-bcm-unimac.c b/drivers/net/phy/mdio-bcm-unimac.c
index 6deac6d32f57..414fdf1f343f 100644
--- a/drivers/net/phy/mdio-bcm-unimac.c
+++ b/drivers/net/phy/mdio-bcm-unimac.c
@@ -187,7 +187,7 @@ static int unimac_mdio_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id unimac_mdio_ids[] = {
+static const struct of_device_id unimac_mdio_ids[] = {
 	{ .compatible = "brcm,genet-mdio-v4", },
 	{ .compatible = "brcm,genet-mdio-v3", },
 	{ .compatible = "brcm,genet-mdio-v2", },
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index 0a0578a592b8..49ce7ece5af3 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -249,7 +249,7 @@ static int mdio_gpio_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id mdio_gpio_of_match[] = {
+static const struct of_device_id mdio_gpio_of_match[] = {
 	{ .compatible = "virtual,mdio-gpio", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/net/phy/mdio-mux-gpio.c b/drivers/net/phy/mdio-mux-gpio.c
index 320eb15315c8..1a87a585e74d 100644
--- a/drivers/net/phy/mdio-mux-gpio.c
+++ b/drivers/net/phy/mdio-mux-gpio.c
@@ -99,7 +99,7 @@ static int mdio_mux_gpio_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id mdio_mux_gpio_match[] = {
+static const struct of_device_id mdio_mux_gpio_match[] = {
 	{
 		.compatible = "mdio-mux-gpio",
 	},
diff --git a/drivers/net/phy/mdio-mux-mmioreg.c b/drivers/net/phy/mdio-mux-mmioreg.c
index 0aa985c74014..2377c1341172 100644
--- a/drivers/net/phy/mdio-mux-mmioreg.c
+++ b/drivers/net/phy/mdio-mux-mmioreg.c
@@ -145,7 +145,7 @@ static int mdio_mux_mmioreg_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id mdio_mux_mmioreg_match[] = {
+static const struct of_device_id mdio_mux_mmioreg_match[] = {
 	{
 		.compatible = "mdio-mux-mmioreg",
 	},
diff --git a/drivers/net/phy/mdio-octeon.c b/drivers/net/phy/mdio-octeon.c
index c81052486edc..c838ad6155f7 100644
--- a/drivers/net/phy/mdio-octeon.c
+++ b/drivers/net/phy/mdio-octeon.c
@@ -252,7 +252,7 @@ static int octeon_mdiobus_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id octeon_mdiobus_match[] = {
+static const struct of_device_id octeon_mdiobus_match[] = {
 	{
 		.compatible = "cavium,octeon-3860-mdio",
 	},
diff --git a/drivers/net/ppp/pppoe.c b/drivers/net/ppp/pppoe.c
index d2408a5e43a6..ff059e1d8ac6 100644
--- a/drivers/net/ppp/pppoe.c
+++ b/drivers/net/ppp/pppoe.c
@@ -455,6 +455,18 @@ out:
 	return NET_RX_DROP;
 }
 
+static void pppoe_unbind_sock_work(struct work_struct *work)
+{
+	struct pppox_sock *po = container_of(work, struct pppox_sock,
+					     proto.pppoe.padt_work);
+	struct sock *sk = sk_pppox(po);
+
+	lock_sock(sk);
+	pppox_unbind_sock(sk);
+	release_sock(sk);
+	sock_put(sk);
+}
+
 /************************************************************************
  *
  * Receive a PPPoE Discovery frame.
@@ -500,7 +512,8 @@ static int pppoe_disc_rcv(struct sk_buff *skb, struct net_device *dev,
 		}
 
 		bh_unlock_sock(sk);
-		sock_put(sk);
+		if (!schedule_work(&po->proto.pppoe.padt_work))
+			sock_put(sk);
 	}
 
 abort:
@@ -613,6 +626,8 @@ static int pppoe_connect(struct socket *sock, struct sockaddr *uservaddr,
 
 	lock_sock(sk);
 
+	INIT_WORK(&po->proto.pppoe.padt_work, pppoe_unbind_sock_work);
+
 	error = -EINVAL;
 	if (sp->sa_protocol != PX_PROTO_OE)
 		goto end;
@@ -820,8 +835,8 @@ static int pppoe_ioctl(struct socket *sock, unsigned int cmd,
 	return err;
 }
 
-static int pppoe_sendmsg(struct kiocb *iocb, struct socket *sock,
-		  struct msghdr *m, size_t total_len)
+static int pppoe_sendmsg(struct socket *sock, struct msghdr *m,
+			 size_t total_len)
 {
 	struct sk_buff *skb;
 	struct sock *sk = sock->sk;
@@ -962,8 +977,8 @@ static const struct ppp_channel_ops pppoe_chan_ops = {
 	.start_xmit = pppoe_xmit,
 };
 
-static int pppoe_recvmsg(struct kiocb *iocb, struct socket *sock,
-		  struct msghdr *m, size_t total_len, int flags)
+static int pppoe_recvmsg(struct socket *sock, struct msghdr *m,
+			 size_t total_len, int flags)
 {
 	struct sock *sk = sock->sk;
 	struct sk_buff *skb;
diff --git a/drivers/net/ppp/pptp.c b/drivers/net/ppp/pptp.c
index 1dc628ffce2b..e3bfbd4d0136 100644
--- a/drivers/net/ppp/pptp.c
+++ b/drivers/net/ppp/pptp.c
@@ -281,7 +281,7 @@ static int pptp_xmit(struct ppp_channel *chan, struct sk_buff *skb)
 	nf_reset(skb);
 
 	skb->ip_summed = CHECKSUM_NONE;
-	ip_select_ident(skb, NULL);
+	ip_select_ident(sock_net(sk), skb, NULL);
 	ip_send_check(iph);
 
 	ip_local_out(skb);
diff --git a/drivers/net/sb1000.c b/drivers/net/sb1000.c
index 66c2f1a01963..aad0b59d41e3 100644
--- a/drivers/net/sb1000.c
+++ b/drivers/net/sb1000.c
@@ -1175,17 +1175,4 @@ MODULE_AUTHOR("Franco Venturi <fventuri@mediaone.net>");
 MODULE_DESCRIPTION("General Instruments SB1000 driver");
 MODULE_LICENSE("GPL");
 
-static int __init
-sb1000_init(void)
-{
-	return pnp_register_driver(&sb1000_driver);
-}
-
-static void __exit
-sb1000_exit(void)
-{
-	pnp_unregister_driver(&sb1000_driver);
-}
-
-module_init(sb1000_init);
-module_exit(sb1000_exit);
+module_pnp_driver(sb1000_driver);
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 7d394846afc2..6928448f6b7f 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -1935,6 +1935,9 @@ static netdev_features_t team_fix_features(struct net_device *dev,
 						     mask);
 	}
 	rcu_read_unlock();
+
+	features = netdev_add_tso_features(features, mask);
+
 	return features;
 }
 
@@ -1976,6 +1979,7 @@ static const struct net_device_ops team_netdev_ops = {
 	.ndo_change_carrier     = team_change_carrier,
 	.ndo_bridge_setlink     = ndo_dflt_netdev_switch_port_bridge_setlink,
 	.ndo_bridge_dellink     = ndo_dflt_netdev_switch_port_bridge_dellink,
+	.ndo_features_check	= passthru_features_check,
 };
 
 /***********************
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index 857dca47bf80..e470ae59d405 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -1448,8 +1448,7 @@ static void tun_sock_write_space(struct sock *sk)
 	kill_fasync(&tfile->fasync, SIGIO, POLL_OUT);
 }
 
-static int tun_sendmsg(struct kiocb *iocb, struct socket *sock,
-		       struct msghdr *m, size_t total_len)
+static int tun_sendmsg(struct socket *sock, struct msghdr *m, size_t total_len)
 {
 	int ret;
 	struct tun_file *tfile = container_of(sock, struct tun_file, socket);
@@ -1464,8 +1463,7 @@ static int tun_sendmsg(struct kiocb *iocb, struct socket *sock,
 	return ret;
 }
 
-static int tun_recvmsg(struct kiocb *iocb, struct socket *sock,
-		       struct msghdr *m, size_t total_len,
+static int tun_recvmsg(struct socket *sock, struct msghdr *m, size_t total_len,
 		       int flags)
 {
 	struct tun_file *tfile = container_of(sock, struct tun_file, socket);
@@ -2225,8 +2223,6 @@ static void tun_chr_show_fdinfo(struct seq_file *m, struct file *f)
 static const struct file_operations tun_fops = {
 	.owner	= THIS_MODULE,
 	.llseek = no_llseek,
-	.read  = new_sync_read,
-	.write = new_sync_write,
 	.read_iter  = tun_chr_read_iter,
 	.write_iter = tun_chr_write_iter,
 	.poll	= tun_chr_poll,
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/catc.c b/drivers/net/usb/catc.c
index 8cfc3bb0c6a6..4e2b26a88b15 100644
--- a/drivers/net/usb/catc.c
+++ b/drivers/net/usb/catc.c
@@ -641,7 +641,7 @@ static void catc_set_multicast_list(struct net_device *netdev)
 	u8 broadcast[ETH_ALEN];
 	u8 rx = RxEnable | RxPolarity | RxMultiCast;
 
-	memset(broadcast, 0xff, ETH_ALEN);
+	eth_broadcast_addr(broadcast);
 	memset(catc->multicast, 0, 64);
 
 	catc_multicast(broadcast, catc->multicast);
@@ -880,7 +880,7 @@ static int catc_probe(struct usb_interface *intf, const struct usb_device_id *id
 		
 		dev_dbg(dev, "Filling the multicast list.\n");
 	  
-		memset(broadcast, 0xff, ETH_ALEN);
+		eth_broadcast_addr(broadcast);
 		catc_multicast(broadcast, catc->multicast);
 		catc_multicast(netdev->dev_addr, catc->multicast);
 		catc_write_mem(catc, 0xfa80, catc->multicast, 64);
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_mbim.c b/drivers/net/usb/cdc_mbim.c
index 96fc8a5bde84..e4b7a47a825c 100644
--- a/drivers/net/usb/cdc_mbim.c
+++ b/drivers/net/usb/cdc_mbim.c
@@ -394,7 +394,7 @@ static struct sk_buff *cdc_mbim_process_dgram(struct usbnet *dev, u8 *buf, size_
 	skb_put(skb, ETH_HLEN);
 	skb_reset_mac_header(skb);
 	eth_hdr(skb)->h_proto = proto;
-	memset(eth_hdr(skb)->h_source, 0, ETH_ALEN);
+	eth_zero_addr(eth_hdr(skb)->h_source);
 	memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN);
 
 	/* add datagram */
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..e221bfcee76b 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 778e91531fac..111d907e0c11 100644
--- a/drivers/net/usb/hso.c
+++ b/drivers/net/usb/hso.c
@@ -1477,6 +1477,7 @@ static void tiocmget_intr_callback(struct urb *urb)
 	struct uart_icount *icount;
 	struct hso_serial_state_notification *serial_state_notification;
 	struct usb_device *usb;
+	struct usb_interface *interface;
 	int if_num;
 
 	/* Sanity checks */
@@ -1494,7 +1495,9 @@ static void tiocmget_intr_callback(struct urb *urb)
 	BUG_ON((serial->parent->port_spec & HSO_PORT_MASK) != HSO_PORT_MODEM);
 
 	usb = serial->parent->usb;
-	if_num = serial->parent->interface->altsetting->desc.bInterfaceNumber;
+	interface = serial->parent->interface;
+
+	if_num = interface->cur_altsetting->desc.bInterfaceNumber;
 
 	/* wIndex should be the USB interface number of the port to which the
 	 * notification applies, which should always be the Modem port.
@@ -1675,6 +1678,7 @@ static int hso_serial_tiocmset(struct tty_struct *tty,
 	unsigned long flags;
 	int if_num;
 	struct hso_serial *serial = tty->driver_data;
+	struct usb_interface *interface;
 
 	/* sanity check */
 	if (!serial) {
@@ -1685,7 +1689,8 @@ static int hso_serial_tiocmset(struct tty_struct *tty,
 	if ((serial->parent->port_spec & HSO_PORT_MASK) != HSO_PORT_MODEM)
 		return -EINVAL;
 
-	if_num = serial->parent->interface->altsetting->desc.bInterfaceNumber;
+	interface = serial->parent->interface;
+	if_num = interface->cur_altsetting->desc.bInterfaceNumber;
 
 	spin_lock_irqsave(&serial->serial_lock, flags);
 	if (set & TIOCM_RTS)
@@ -2808,7 +2813,7 @@ static int hso_get_config_data(struct usb_interface *interface)
 {
 	struct usb_device *usbdev = interface_to_usbdev(interface);
 	u8 *config_data = kmalloc(17, GFP_KERNEL);
-	u32 if_num = interface->altsetting->desc.bInterfaceNumber;
+	u32 if_num = interface->cur_altsetting->desc.bInterfaceNumber;
 	s32 result;
 
 	if (!config_data)
@@ -2886,7 +2891,7 @@ static int hso_probe(struct usb_interface *interface,
 		return -ENODEV;
 	}
 
-	if_num = interface->altsetting->desc.bInterfaceNumber;
+	if_num = interface->cur_altsetting->desc.bInterfaceNumber;
 
 	/* Get the interface/port specification from either driver_info or from
 	 * the device itself */
diff --git a/drivers/net/usb/lg-vl600.c b/drivers/net/usb/lg-vl600.c
index 8f37efd2d2fb..5714107533bb 100644
--- a/drivers/net/usb/lg-vl600.c
+++ b/drivers/net/usb/lg-vl600.c
@@ -201,7 +201,7 @@ static int vl600_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 					&buf->data[sizeof(*ethhdr) + 0x12],
 					ETH_ALEN);
 		} else {
-			memset(ethhdr->h_source, 0, ETH_ALEN);
+			eth_zero_addr(ethhdr->h_source);
 			memcpy(ethhdr->h_dest, dev->net->dev_addr, ETH_ALEN);
 
 			/* Inbound IPv6 packets have an IPv4 ethertype (0x800)
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index 602dc6668c3a..f603f362504b 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -108,7 +108,7 @@ static int qmi_wwan_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 	skb_push(skb, ETH_HLEN);
 	skb_reset_mac_header(skb);
 	eth_hdr(skb)->h_proto = proto;
-	memset(eth_hdr(skb)->h_source, 0, ETH_ALEN);
+	eth_zero_addr(eth_hdr(skb)->h_source);
 fix_dest:
 	memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN);
 	return 1;
diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
index 438fc6bcaef1..ac4d03b328b1 100644
--- a/drivers/net/usb/r8152.c
+++ b/drivers/net/usb/r8152.c
@@ -104,7 +104,8 @@
 #define USB_TX_AGG		0xd40a
 #define USB_RX_BUF_TH		0xd40c
 #define USB_USB_TIMER		0xd428
-#define USB_RX_EARLY_AGG	0xd42c
+#define USB_RX_EARLY_TIMEOUT	0xd42c
+#define USB_RX_EARLY_SIZE	0xd42e
 #define USB_PM_CTRL_STATUS	0xd432
 #define USB_TX_DMA		0xd434
 #define USB_TOLERANCE		0xd490
@@ -349,10 +350,10 @@
 /* USB_MISC_0 */
 #define PCUT_STATUS		0x0001
 
-/* USB_RX_EARLY_AGG */
-#define EARLY_AGG_SUPPER	0x0e832981
-#define EARLY_AGG_HIGH		0x0e837a12
-#define EARLY_AGG_SLOW		0x0e83ffff
+/* USB_RX_EARLY_TIMEOUT */
+#define COALESCE_SUPER		 85000U
+#define COALESCE_HIGH		250000U
+#define COALESCE_SLOW		524280U
 
 /* USB_WDT11_CTRL */
 #define TIMER11_EN		0x0001
@@ -492,6 +493,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
@@ -606,6 +608,7 @@ struct r8152 {
 	u32 saved_wolopts;
 	u32 msg_enable;
 	u32 tx_qlen;
+	u32 coalesce;
 	u16 ocp_base;
 	u8 *intr_buff;
 	u8 version;
@@ -2142,28 +2145,19 @@ static int rtl8152_enable(struct r8152 *tp)
 	return rtl_enable(tp);
 }
 
-static void r8153_set_rx_agg(struct r8152 *tp)
+static void r8153_set_rx_early_timeout(struct r8152 *tp)
 {
-	u8 speed;
+	u32 ocp_data = tp->coalesce / 8;
 
-	speed = rtl8152_get_speed(tp);
-	if (speed & _1000bps) {
-		if (tp->udev->speed == USB_SPEED_SUPER) {
-			ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_BUF_TH,
-					RX_THR_SUPPER);
-			ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_EARLY_AGG,
-					EARLY_AGG_SUPPER);
-		} else {
-			ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_BUF_TH,
-					RX_THR_HIGH);
-			ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_EARLY_AGG,
-					EARLY_AGG_HIGH);
-		}
-	} else {
-		ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_BUF_TH, RX_THR_SLOW);
-		ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_EARLY_AGG,
-				EARLY_AGG_SLOW);
-	}
+	ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_TIMEOUT, ocp_data);
+}
+
+static void r8153_set_rx_early_size(struct r8152 *tp)
+{
+	u32 mtu = tp->netdev->mtu;
+	u32 ocp_data = (agg_buf_sz - mtu - VLAN_ETH_HLEN - VLAN_HLEN) / 4;
+
+	ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_SIZE, ocp_data);
 }
 
 static int rtl8153_enable(struct r8152 *tp)
@@ -2173,7 +2167,8 @@ static int rtl8153_enable(struct r8152 *tp)
 
 	set_tx_qlen(tp);
 	rtl_set_eee_plus(tp);
-	r8153_set_rx_agg(tp);
+	r8153_set_rx_early_timeout(tp);
+	r8153_set_rx_early_size(tp);
 
 	return rtl_enable(tp);
 }
@@ -3719,6 +3714,61 @@ out:
 	return ret;
 }
 
+static int rtl8152_get_coalesce(struct net_device *netdev,
+				struct ethtool_coalesce *coalesce)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+
+	switch (tp->version) {
+	case RTL_VER_01:
+	case RTL_VER_02:
+		return -EOPNOTSUPP;
+	default:
+		break;
+	}
+
+	coalesce->rx_coalesce_usecs = tp->coalesce;
+
+	return 0;
+}
+
+static int rtl8152_set_coalesce(struct net_device *netdev,
+				struct ethtool_coalesce *coalesce)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	int ret;
+
+	switch (tp->version) {
+	case RTL_VER_01:
+	case RTL_VER_02:
+		return -EOPNOTSUPP;
+	default:
+		break;
+	}
+
+	if (coalesce->rx_coalesce_usecs > COALESCE_SLOW)
+		return -EINVAL;
+
+	ret = usb_autopm_get_interface(tp->intf);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&tp->control);
+
+	if (tp->coalesce != coalesce->rx_coalesce_usecs) {
+		tp->coalesce = coalesce->rx_coalesce_usecs;
+
+		if (netif_running(tp->netdev) && netif_carrier_ok(netdev))
+			r8153_set_rx_early_timeout(tp);
+	}
+
+	mutex_unlock(&tp->control);
+
+	usb_autopm_put_interface(tp->intf);
+
+	return ret;
+}
+
 static struct ethtool_ops ops = {
 	.get_drvinfo = rtl8152_get_drvinfo,
 	.get_settings = rtl8152_get_settings,
@@ -3732,6 +3782,8 @@ static struct ethtool_ops ops = {
 	.get_strings = rtl8152_get_strings,
 	.get_sset_count = rtl8152_get_sset_count,
 	.get_ethtool_stats = rtl8152_get_ethtool_stats,
+	.get_coalesce = rtl8152_get_coalesce,
+	.set_coalesce = rtl8152_set_coalesce,
 	.get_eee = rtl_ethtool_get_eee,
 	.set_eee = rtl_ethtool_set_eee,
 };
@@ -3783,6 +3835,7 @@ out:
 static int rtl8152_change_mtu(struct net_device *dev, int new_mtu)
 {
 	struct r8152 *tp = netdev_priv(dev);
+	int ret;
 
 	switch (tp->version) {
 	case RTL_VER_01:
@@ -3795,9 +3848,22 @@ static int rtl8152_change_mtu(struct net_device *dev, int new_mtu)
 	if (new_mtu < 68 || new_mtu > RTL8153_MAX_MTU)
 		return -EINVAL;
 
+	ret = usb_autopm_get_interface(tp->intf);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&tp->control);
+
 	dev->mtu = new_mtu;
 
-	return 0;
+	if (netif_running(dev) && netif_carrier_ok(dev))
+		r8153_set_rx_early_size(tp);
+
+	mutex_unlock(&tp->control);
+
+	usb_autopm_put_interface(tp->intf);
+
+	return ret;
 }
 
 static const struct net_device_ops rtl8152_netdev_ops = {
@@ -3966,6 +4032,18 @@ static int rtl8152_probe(struct usb_interface *intf,
 	tp->mii.reg_num_mask = 0x1f;
 	tp->mii.phy_id = R8152_PHY_ID;
 
+	switch (udev->speed) {
+	case USB_SPEED_SUPER:
+		tp->coalesce = COALESCE_SUPER;
+		break;
+	case USB_SPEED_HIGH:
+		tp->coalesce = COALESCE_HIGH;
+		break;
+	default:
+		tp->coalesce = COALESCE_SLOW;
+		break;
+	}
+
 	intf->needs_remote_wakeup = 1;
 
 	tp->rtl_ops.init(tp);
@@ -4037,6 +4115,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..733f4feb2ef3 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -1072,7 +1072,7 @@ static void __handle_set_rx_mode(struct usbnet *dev)
  * especially now that control transfers can be queued.
  */
 static void
-kevent (struct work_struct *work)
+usbnet_deferred_kevent (struct work_struct *work)
 {
 	struct usbnet		*dev =
 		container_of(work, struct usbnet, kevent);
@@ -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);
@@ -1615,7 +1626,7 @@ usbnet_probe (struct usb_interface *udev, const struct usb_device_id *prod)
 	skb_queue_head_init(&dev->rxq_pause);
 	dev->bh.func = usbnet_bh;
 	dev->bh.data = (unsigned long) dev;
-	INIT_WORK (&dev->kevent, kevent);
+	INIT_WORK (&dev->kevent, usbnet_deferred_kevent);
 	init_usb_anchor(&dev->deferred);
 	dev->delay.function = usbnet_bh;
 	dev->delay.data = (unsigned long) dev;
diff --git a/drivers/net/veth.c b/drivers/net/veth.c
index 4cca36ebc4fb..c8186ffda1a3 100644
--- a/drivers/net/veth.c
+++ b/drivers/net/veth.c
@@ -263,6 +263,20 @@ static void veth_poll_controller(struct net_device *dev)
 }
 #endif	/* CONFIG_NET_POLL_CONTROLLER */
 
+static int veth_get_iflink(const struct net_device *dev)
+{
+	struct veth_priv *priv = netdev_priv(dev);
+	struct net_device *peer;
+	int iflink;
+
+	rcu_read_lock();
+	peer = rcu_dereference(priv->peer);
+	iflink = peer ? peer->ifindex : 0;
+	rcu_read_unlock();
+
+	return iflink;
+}
+
 static const struct net_device_ops veth_netdev_ops = {
 	.ndo_init            = veth_dev_init,
 	.ndo_open            = veth_open,
@@ -275,6 +289,7 @@ static const struct net_device_ops veth_netdev_ops = {
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= veth_poll_controller,
 #endif
+	.ndo_get_iflink		= veth_get_iflink,
 };
 
 #define VETH_FEATURES (NETIF_F_SG | NETIF_F_FRAGLIST | NETIF_F_ALL_TSO |    \
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index f1ff3666f090..63c7810e1545 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -749,9 +749,9 @@ static int virtnet_poll(struct napi_struct *napi, int budget)
 {
 	struct receive_queue *rq =
 		container_of(napi, struct receive_queue, napi);
-	unsigned int r, received = 0;
+	unsigned int r, received;
 
-	received += virtnet_receive(rq, budget - received);
+	received = virtnet_receive(rq, budget);
 
 	/* Out of packets? */
 	if (received < budget) {
@@ -939,8 +939,16 @@ static netdev_tx_t start_xmit(struct sk_buff *skb, struct net_device *dev)
 	skb_orphan(skb);
 	nf_reset(skb);
 
-	/* Apparently nice girls don't return TX_BUSY; stop the queue
-	 * before it gets out of hand.  Naturally, this wastes entries. */
+	/* If running out of space, stop queue to avoid getting packets that we
+	 * are then unable to transmit.
+	 * An alternative would be to force queuing layer to requeue the skb by
+	 * returning NETDEV_TX_BUSY. However, NETDEV_TX_BUSY should not be
+	 * returned in a normal path of operation: it means that driver is not
+	 * maintaining the TX queue stop/start state properly, and causes
+	 * the stack to do a non-trivial amount of useless work.
+	 * Since most packets only take 1 or 2 ring slots, stopping the queue
+	 * early means 16 slots are typically wasted.
+	 */
 	if (sq->vq->num_free < 2+MAX_SKB_FRAGS) {
 		netif_stop_subqueue(dev, qnum);
 		if (unlikely(!virtqueue_enable_cb_delayed(sq->vq))) {
@@ -1448,8 +1456,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 +1958,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/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index 294214c15292..61c0840c448c 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -819,6 +819,7 @@ vmxnet3_parse_and_copy_hdr(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 			   struct vmxnet3_adapter *adapter)
 {
 	struct Vmxnet3_TxDataDesc *tdd;
+	u8 protocol = 0;
 
 	if (ctx->mss) {	/* TSO */
 		ctx->eth_ip_hdr_size = skb_transport_offset(skb);
@@ -831,16 +832,25 @@ vmxnet3_parse_and_copy_hdr(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 			if (ctx->ipv4) {
 				const struct iphdr *iph = ip_hdr(skb);
 
-				if (iph->protocol == IPPROTO_TCP)
-					ctx->l4_hdr_size = tcp_hdrlen(skb);
-				else if (iph->protocol == IPPROTO_UDP)
-					ctx->l4_hdr_size = sizeof(struct udphdr);
-				else
-					ctx->l4_hdr_size = 0;
-			} else {
-				/* for simplicity, don't copy L4 headers */
+				protocol = iph->protocol;
+			} else if (ctx->ipv6) {
+				const struct ipv6hdr *ipv6h = ipv6_hdr(skb);
+
+				protocol = ipv6h->nexthdr;
+			}
+
+			switch (protocol) {
+			case IPPROTO_TCP:
+				ctx->l4_hdr_size = tcp_hdrlen(skb);
+				break;
+			case IPPROTO_UDP:
+				ctx->l4_hdr_size = sizeof(struct udphdr);
+				break;
+			default:
 				ctx->l4_hdr_size = 0;
+				break;
 			}
+
 			ctx->copy_size = min(ctx->eth_ip_hdr_size +
 					 ctx->l4_hdr_size, skb->len);
 		} else {
@@ -887,7 +897,7 @@ vmxnet3_prepare_tso(struct sk_buff *skb,
 		iph->check = 0;
 		tcph->check = ~csum_tcpudp_magic(iph->saddr, iph->daddr, 0,
 						 IPPROTO_TCP, 0);
-	} else {
+	} else if (ctx->ipv6) {
 		struct ipv6hdr *iph = ipv6_hdr(skb);
 
 		tcph->check = ~csum_ipv6_magic(&iph->saddr, &iph->daddr, 0,
@@ -938,6 +948,7 @@ vmxnet3_tq_xmit(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 	count = txd_estimate(skb);
 
 	ctx.ipv4 = (vlan_get_protocol(skb) == cpu_to_be16(ETH_P_IP));
+	ctx.ipv6 = (vlan_get_protocol(skb) == cpu_to_be16(ETH_P_IPV6));
 
 	ctx.mss = skb_shinfo(skb)->gso_size;
 	if (ctx.mss) {
diff --git a/drivers/net/vmxnet3/vmxnet3_ethtool.c b/drivers/net/vmxnet3/vmxnet3_ethtool.c
index 4c8a944d58b4..c1d0e7a9da04 100644
--- a/drivers/net/vmxnet3/vmxnet3_ethtool.c
+++ b/drivers/net/vmxnet3/vmxnet3_ethtool.c
@@ -104,7 +104,7 @@ vmxnet3_rq_driver_stats[] = {
 					  rx_buf_alloc_failure) },
 };
 
-/* gloabl stats maintained by the driver */
+/* global stats maintained by the driver */
 static const struct vmxnet3_stat_desc
 vmxnet3_global_stats[] = {
 	/* description,         offset */
@@ -272,7 +272,7 @@ int vmxnet3_set_features(struct net_device *netdev, netdev_features_t features)
 			adapter->shared->devRead.misc.uptFeatures &=
 			~UPT1_F_RXCSUM;
 
-		/* update harware LRO capability accordingly */
+		/* update hardware LRO capability accordingly */
 		if (features & NETIF_F_LRO)
 			adapter->shared->devRead.misc.uptFeatures |=
 							UPT1_F_LRO;
diff --git a/drivers/net/vmxnet3/vmxnet3_int.h b/drivers/net/vmxnet3/vmxnet3_int.h
index cd71c77f78f2..6bb769ae7de9 100644
--- a/drivers/net/vmxnet3/vmxnet3_int.h
+++ b/drivers/net/vmxnet3/vmxnet3_int.h
@@ -69,10 +69,10 @@
 /*
  * Version numbers
  */
-#define VMXNET3_DRIVER_VERSION_STRING   "1.3.4.0-k"
+#define VMXNET3_DRIVER_VERSION_STRING   "1.3.5.0-k"
 
 /* a 32-bit int, each byte encode a verion number in VMXNET3_DRIVER_VERSION */
-#define VMXNET3_DRIVER_VERSION_NUM      0x01030400
+#define VMXNET3_DRIVER_VERSION_NUM      0x01030500
 
 #if defined(CONFIG_PCI_MSI)
 	/* RSS only makes sense if MSI-X is supported. */
@@ -211,6 +211,7 @@ struct vmxnet3_tq_driver_stats {
 
 struct vmxnet3_tx_ctx {
 	bool   ipv4;
+	bool   ipv6;
 	u16 mss;
 	u32 eth_ip_hdr_size; /* only valid for pkts requesting tso or csum
 				 * offloading
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 1e0a775ea882..154116aafd0d 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -127,10 +127,6 @@ struct vxlan_dev {
 	__u8		  ttl;
 	u32		  flags;	/* VXLAN_F_* in vxlan.h */
 
-	struct work_struct sock_work;
-	struct work_struct igmp_join;
-	struct work_struct igmp_leave;
-
 	unsigned long	  age_interval;
 	struct timer_list age_timer;
 	spinlock_t	  hash_lock;
@@ -144,58 +140,56 @@ struct vxlan_dev {
 static u32 vxlan_salt __read_mostly;
 static struct workqueue_struct *vxlan_wq;
 
-static void vxlan_sock_work(struct work_struct *work);
-
 #if IS_ENABLED(CONFIG_IPV6)
 static inline
 bool vxlan_addr_equal(const union vxlan_addr *a, const union vxlan_addr *b)
 {
-       if (a->sa.sa_family != b->sa.sa_family)
-               return false;
-       if (a->sa.sa_family == AF_INET6)
-               return ipv6_addr_equal(&a->sin6.sin6_addr, &b->sin6.sin6_addr);
-       else
-               return a->sin.sin_addr.s_addr == b->sin.sin_addr.s_addr;
+	if (a->sa.sa_family != b->sa.sa_family)
+		return false;
+	if (a->sa.sa_family == AF_INET6)
+		return ipv6_addr_equal(&a->sin6.sin6_addr, &b->sin6.sin6_addr);
+	else
+		return a->sin.sin_addr.s_addr == b->sin.sin_addr.s_addr;
 }
 
 static inline bool vxlan_addr_any(const union vxlan_addr *ipa)
 {
-       if (ipa->sa.sa_family == AF_INET6)
-               return ipv6_addr_any(&ipa->sin6.sin6_addr);
-       else
-               return ipa->sin.sin_addr.s_addr == htonl(INADDR_ANY);
+	if (ipa->sa.sa_family == AF_INET6)
+		return ipv6_addr_any(&ipa->sin6.sin6_addr);
+	else
+		return ipa->sin.sin_addr.s_addr == htonl(INADDR_ANY);
 }
 
 static inline bool vxlan_addr_multicast(const union vxlan_addr *ipa)
 {
-       if (ipa->sa.sa_family == AF_INET6)
-               return ipv6_addr_is_multicast(&ipa->sin6.sin6_addr);
-       else
-               return IN_MULTICAST(ntohl(ipa->sin.sin_addr.s_addr));
+	if (ipa->sa.sa_family == AF_INET6)
+		return ipv6_addr_is_multicast(&ipa->sin6.sin6_addr);
+	else
+		return IN_MULTICAST(ntohl(ipa->sin.sin_addr.s_addr));
 }
 
 static int vxlan_nla_get_addr(union vxlan_addr *ip, struct nlattr *nla)
 {
-       if (nla_len(nla) >= sizeof(struct in6_addr)) {
-               nla_memcpy(&ip->sin6.sin6_addr, nla, sizeof(struct in6_addr));
-               ip->sa.sa_family = AF_INET6;
-               return 0;
-       } else if (nla_len(nla) >= sizeof(__be32)) {
-               ip->sin.sin_addr.s_addr = nla_get_be32(nla);
-               ip->sa.sa_family = AF_INET;
-               return 0;
-       } else {
-               return -EAFNOSUPPORT;
-       }
+	if (nla_len(nla) >= sizeof(struct in6_addr)) {
+		ip->sin6.sin6_addr = nla_get_in6_addr(nla);
+		ip->sa.sa_family = AF_INET6;
+		return 0;
+	} else if (nla_len(nla) >= sizeof(__be32)) {
+		ip->sin.sin_addr.s_addr = nla_get_in_addr(nla);
+		ip->sa.sa_family = AF_INET;
+		return 0;
+	} else {
+		return -EAFNOSUPPORT;
+	}
 }
 
 static int vxlan_nla_put_addr(struct sk_buff *skb, int attr,
-                             const union vxlan_addr *ip)
+			      const union vxlan_addr *ip)
 {
-       if (ip->sa.sa_family == AF_INET6)
-               return nla_put(skb, attr, sizeof(struct in6_addr), &ip->sin6.sin6_addr);
-       else
-               return nla_put_be32(skb, attr, ip->sin.sin_addr.s_addr);
+	if (ip->sa.sa_family == AF_INET6)
+		return nla_put_in6_addr(skb, attr, &ip->sin6.sin6_addr);
+	else
+		return nla_put_in_addr(skb, attr, ip->sin.sin_addr.s_addr);
 }
 
 #else /* !CONFIG_IPV6 */
@@ -203,36 +197,36 @@ static int vxlan_nla_put_addr(struct sk_buff *skb, int attr,
 static inline
 bool vxlan_addr_equal(const union vxlan_addr *a, const union vxlan_addr *b)
 {
-       return a->sin.sin_addr.s_addr == b->sin.sin_addr.s_addr;
+	return a->sin.sin_addr.s_addr == b->sin.sin_addr.s_addr;
 }
 
 static inline bool vxlan_addr_any(const union vxlan_addr *ipa)
 {
-       return ipa->sin.sin_addr.s_addr == htonl(INADDR_ANY);
+	return ipa->sin.sin_addr.s_addr == htonl(INADDR_ANY);
 }
 
 static inline bool vxlan_addr_multicast(const union vxlan_addr *ipa)
 {
-       return IN_MULTICAST(ntohl(ipa->sin.sin_addr.s_addr));
+	return IN_MULTICAST(ntohl(ipa->sin.sin_addr.s_addr));
 }
 
 static int vxlan_nla_get_addr(union vxlan_addr *ip, struct nlattr *nla)
 {
-       if (nla_len(nla) >= sizeof(struct in6_addr)) {
-               return -EAFNOSUPPORT;
-       } else if (nla_len(nla) >= sizeof(__be32)) {
-               ip->sin.sin_addr.s_addr = nla_get_be32(nla);
-               ip->sa.sa_family = AF_INET;
-               return 0;
-       } else {
-               return -EAFNOSUPPORT;
-       }
+	if (nla_len(nla) >= sizeof(struct in6_addr)) {
+		return -EAFNOSUPPORT;
+	} else if (nla_len(nla) >= sizeof(__be32)) {
+		ip->sin.sin_addr.s_addr = nla_get_in_addr(nla);
+		ip->sa.sa_family = AF_INET;
+		return 0;
+	} else {
+		return -EAFNOSUPPORT;
+	}
 }
 
 static int vxlan_nla_put_addr(struct sk_buff *skb, int attr,
-                             const union vxlan_addr *ip)
+			      const union vxlan_addr *ip)
 {
-       return nla_put_be32(skb, attr, ip->sin.sin_addr.s_addr);
+	return nla_put_in_addr(skb, attr, ip->sin.sin_addr.s_addr);
 }
 #endif
 
@@ -995,7 +989,7 @@ out:
 
 /* Watch incoming packets to learn mapping between Ethernet address
  * and Tunnel endpoint.
- * Return true if packet is bogus and should be droppped.
+ * Return true if packet is bogus and should be dropped.
  */
 static bool vxlan_snoop(struct net_device *dev,
 			union vxlan_addr *src_ip, const u8 *src_mac)
@@ -1072,11 +1066,6 @@ static bool vxlan_group_used(struct vxlan_net *vn, struct vxlan_dev *dev)
 	return false;
 }
 
-static void vxlan_sock_hold(struct vxlan_sock *vs)
-{
-	atomic_inc(&vs->refcnt);
-}
-
 void vxlan_sock_release(struct vxlan_sock *vs)
 {
 	struct sock *sk = vs->sock->sk;
@@ -1095,17 +1084,16 @@ void vxlan_sock_release(struct vxlan_sock *vs)
 }
 EXPORT_SYMBOL_GPL(vxlan_sock_release);
 
-/* Callback to update multicast group membership when first VNI on
- * multicast asddress is brought up
- * Done as workqueue because ip_mc_join_group acquires RTNL.
+/* Update multicast group membership when first VNI on
+ * multicast address is brought up
  */
-static void vxlan_igmp_join(struct work_struct *work)
+static int vxlan_igmp_join(struct vxlan_dev *vxlan)
 {
-	struct vxlan_dev *vxlan = container_of(work, struct vxlan_dev, igmp_join);
 	struct vxlan_sock *vs = vxlan->vn_sock;
 	struct sock *sk = vs->sock->sk;
 	union vxlan_addr *ip = &vxlan->default_dst.remote_ip;
 	int ifindex = vxlan->default_dst.remote_ifindex;
+	int ret = -EINVAL;
 
 	lock_sock(sk);
 	if (ip->sa.sa_family == AF_INET) {
@@ -1114,27 +1102,26 @@ static void vxlan_igmp_join(struct work_struct *work)
 			.imr_ifindex		= ifindex,
 		};
 
-		ip_mc_join_group(sk, &mreq);
+		ret = ip_mc_join_group(sk, &mreq);
 #if IS_ENABLED(CONFIG_IPV6)
 	} else {
-		ipv6_stub->ipv6_sock_mc_join(sk, ifindex,
-					     &ip->sin6.sin6_addr);
+		ret = ipv6_stub->ipv6_sock_mc_join(sk, ifindex,
+						   &ip->sin6.sin6_addr);
 #endif
 	}
 	release_sock(sk);
 
-	vxlan_sock_release(vs);
-	dev_put(vxlan->dev);
+	return ret;
 }
 
 /* Inverse of vxlan_igmp_join when last VNI is brought down */
-static void vxlan_igmp_leave(struct work_struct *work)
+static int vxlan_igmp_leave(struct vxlan_dev *vxlan)
 {
-	struct vxlan_dev *vxlan = container_of(work, struct vxlan_dev, igmp_leave);
 	struct vxlan_sock *vs = vxlan->vn_sock;
 	struct sock *sk = vs->sock->sk;
 	union vxlan_addr *ip = &vxlan->default_dst.remote_ip;
 	int ifindex = vxlan->default_dst.remote_ifindex;
+	int ret = -EINVAL;
 
 	lock_sock(sk);
 	if (ip->sa.sa_family == AF_INET) {
@@ -1143,18 +1130,16 @@ static void vxlan_igmp_leave(struct work_struct *work)
 			.imr_ifindex		= ifindex,
 		};
 
-		ip_mc_leave_group(sk, &mreq);
+		ret = ip_mc_leave_group(sk, &mreq);
 #if IS_ENABLED(CONFIG_IPV6)
 	} else {
-		ipv6_stub->ipv6_sock_mc_drop(sk, ifindex,
-					     &ip->sin6.sin6_addr);
+		ret = ipv6_stub->ipv6_sock_mc_drop(sk, ifindex,
+						   &ip->sin6.sin6_addr);
 #endif
 	}
-
 	release_sock(sk);
 
-	vxlan_sock_release(vs);
-	dev_put(vxlan->dev);
+	return ret;
 }
 
 static struct vxlanhdr *vxlan_remcsum(struct sk_buff *skb, struct vxlanhdr *vh,
@@ -1218,7 +1203,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,12 +1224,12 @@ 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
 		 * in reserved fields are to be ignored. The approach here
-		 * maintains compatbility with previous stack code, and also
+		 * maintains compatibility with previous stack code, and also
 		 * is more robust and provides a little more security in
 		 * adding extensions to VXLAN.
 		 */
@@ -1687,7 +1672,8 @@ static void vxlan_build_gbp_hdr(struct vxlanhdr *vxh, u32 vxflags,
 }
 
 #if IS_ENABLED(CONFIG_IPV6)
-static int vxlan6_xmit_skb(struct dst_entry *dst, struct sk_buff *skb,
+static int vxlan6_xmit_skb(struct dst_entry *dst, struct sock *sk,
+			   struct sk_buff *skb,
 			   struct net_device *dev, struct in6_addr *saddr,
 			   struct in6_addr *daddr, __u8 prio, __u8 ttl,
 			   __be16 src_port, __be16 dst_port,
@@ -1713,12 +1699,6 @@ static int vxlan6_xmit_skb(struct dst_entry *dst, struct sk_buff *skb,
 		}
 	}
 
-	skb = iptunnel_handle_offloads(skb, udp_sum, type);
-	if (IS_ERR(skb)) {
-		err = -EINVAL;
-		goto err;
-	}
-
 	skb_scrub_packet(skb, xnet);
 
 	min_headroom = LL_RESERVED_SPACE(dst->dev) + dst->header_len
@@ -1738,6 +1718,12 @@ static int vxlan6_xmit_skb(struct dst_entry *dst, struct sk_buff *skb,
 		goto err;
 	}
 
+	skb = iptunnel_handle_offloads(skb, udp_sum, type);
+	if (IS_ERR(skb)) {
+		err = -EINVAL;
+		goto err;
+	}
+
 	vxh = (struct vxlanhdr *) __skb_push(skb, sizeof(*vxh));
 	vxh->vx_flags = htonl(VXLAN_HF_VNI);
 	vxh->vx_vni = md->vni;
@@ -1763,7 +1749,7 @@ static int vxlan6_xmit_skb(struct dst_entry *dst, struct sk_buff *skb,
 
 	skb_set_inner_protocol(skb, htons(ETH_P_TEB));
 
-	udp_tunnel6_xmit_skb(dst, skb, dev, saddr, daddr, prio,
+	udp_tunnel6_xmit_skb(dst, sk, skb, dev, saddr, daddr, prio,
 			     ttl, src_port, dst_port,
 			     !!(vxflags & VXLAN_F_UDP_ZERO_CSUM6_TX));
 	return 0;
@@ -1773,7 +1759,7 @@ err:
 }
 #endif
 
-int vxlan_xmit_skb(struct rtable *rt, struct sk_buff *skb,
+int vxlan_xmit_skb(struct rtable *rt, struct sock *sk, struct sk_buff *skb,
 		   __be32 src, __be32 dst, __u8 tos, __u8 ttl, __be16 df,
 		   __be16 src_port, __be16 dst_port,
 		   struct vxlan_metadata *md, bool xnet, u32 vxflags)
@@ -1798,10 +1784,6 @@ int vxlan_xmit_skb(struct rtable *rt, struct sk_buff *skb,
 		}
 	}
 
-	skb = iptunnel_handle_offloads(skb, udp_sum, type);
-	if (IS_ERR(skb))
-		return PTR_ERR(skb);
-
 	min_headroom = LL_RESERVED_SPACE(rt->dst.dev) + rt->dst.header_len
 			+ VXLAN_HLEN + sizeof(struct iphdr)
 			+ (skb_vlan_tag_present(skb) ? VLAN_HLEN : 0);
@@ -1817,6 +1799,10 @@ int vxlan_xmit_skb(struct rtable *rt, struct sk_buff *skb,
 	if (WARN_ON(!skb))
 		return -ENOMEM;
 
+	skb = iptunnel_handle_offloads(skb, udp_sum, type);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
 	vxh = (struct vxlanhdr *) __skb_push(skb, sizeof(*vxh));
 	vxh->vx_flags = htonl(VXLAN_HF_VNI);
 	vxh->vx_vni = md->vni;
@@ -1842,7 +1828,7 @@ int vxlan_xmit_skb(struct rtable *rt, struct sk_buff *skb,
 
 	skb_set_inner_protocol(skb, htons(ETH_P_TEB));
 
-	return udp_tunnel_xmit_skb(rt, skb, src, dst, tos,
+	return udp_tunnel_xmit_skb(rt, sk, skb, src, dst, tos,
 				   ttl, df, src_port, dst_port, xnet,
 				   !(vxflags & VXLAN_F_UDP_CSUM));
 }
@@ -1897,6 +1883,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 			   struct vxlan_rdst *rdst, bool did_rsc)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct sock *sk = vxlan->vn_sock->sock->sk;
 	struct rtable *rt = NULL;
 	const struct iphdr *old_iph;
 	struct flowi4 fl4;
@@ -1976,7 +1963,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		md.vni = htonl(vni << 8);
 		md.gbp = skb->mark;
 
-		err = vxlan_xmit_skb(rt, skb, fl4.saddr,
+		err = vxlan_xmit_skb(rt, sk, skb, fl4.saddr,
 				     dst->sin.sin_addr.s_addr, tos, ttl, df,
 				     src_port, dst_port, &md,
 				     !net_eq(vxlan->net, dev_net(vxlan->dev)),
@@ -1990,7 +1977,6 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		iptunnel_xmit_stats(err, &dev->stats, dev->tstats);
 #if IS_ENABLED(CONFIG_IPV6)
 	} else {
-		struct sock *sk = vxlan->vn_sock->sock->sk;
 		struct dst_entry *ndst;
 		struct flowi6 fl6;
 		u32 flags;
@@ -2036,7 +2022,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		md.vni = htonl(vni << 8);
 		md.gbp = skb->mark;
 
-		err = vxlan6_xmit_skb(ndst, skb, dev, &fl6.saddr, &fl6.daddr,
+		err = vxlan6_xmit_skb(ndst, sk, skb, dev, &fl6.saddr, &fl6.daddr,
 				      0, ttl, src_port, dst_port, &md,
 				      !net_eq(vxlan->net, dev_net(vxlan->dev)),
 				      vxlan->flags);
@@ -2175,37 +2161,22 @@ static void vxlan_cleanup(unsigned long arg)
 
 static void vxlan_vs_add_dev(struct vxlan_sock *vs, struct vxlan_dev *vxlan)
 {
+	struct vxlan_net *vn = net_generic(vxlan->net, vxlan_net_id);
 	__u32 vni = vxlan->default_dst.remote_vni;
 
 	vxlan->vn_sock = vs;
+	spin_lock(&vn->sock_lock);
 	hlist_add_head_rcu(&vxlan->hlist, vni_head(vs, vni));
+	spin_unlock(&vn->sock_lock);
 }
 
 /* Setup stats when device is created */
 static int vxlan_init(struct net_device *dev)
 {
-	struct vxlan_dev *vxlan = netdev_priv(dev);
-	struct vxlan_net *vn = net_generic(vxlan->net, vxlan_net_id);
-	struct vxlan_sock *vs;
-	bool ipv6 = vxlan->flags & VXLAN_F_IPV6;
-
 	dev->tstats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats);
 	if (!dev->tstats)
 		return -ENOMEM;
 
-	spin_lock(&vn->sock_lock);
-	vs = vxlan_find_sock(vxlan->net, ipv6 ? AF_INET6 : AF_INET,
-			     vxlan->dst_port, vxlan->flags);
-	if (vs && atomic_add_unless(&vs->refcnt, 1, 0)) {
-		/* If we have a socket with same port already, reuse it */
-		vxlan_vs_add_dev(vs, vxlan);
-	} else {
-		/* otherwise make new socket outside of RTNL */
-		dev_hold(dev);
-		queue_work(vxlan_wq, &vxlan->sock_work);
-	}
-	spin_unlock(&vn->sock_lock);
-
 	return 0;
 }
 
@@ -2223,12 +2194,9 @@ static void vxlan_fdb_delete_default(struct vxlan_dev *vxlan)
 static void vxlan_uninit(struct net_device *dev)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
-	struct vxlan_sock *vs = vxlan->vn_sock;
 
 	vxlan_fdb_delete_default(vxlan);
 
-	if (vs)
-		vxlan_sock_release(vs);
 	free_percpu(dev->tstats);
 }
 
@@ -2236,22 +2204,28 @@ static void vxlan_uninit(struct net_device *dev)
 static int vxlan_open(struct net_device *dev)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
-	struct vxlan_sock *vs = vxlan->vn_sock;
+	struct vxlan_sock *vs;
+	int ret = 0;
 
-	/* socket hasn't been created */
-	if (!vs)
-		return -ENOTCONN;
+	vs = vxlan_sock_add(vxlan->net, vxlan->dst_port, vxlan_rcv, NULL,
+			    false, vxlan->flags);
+	if (IS_ERR(vs))
+		return PTR_ERR(vs);
+
+	vxlan_vs_add_dev(vs, vxlan);
 
 	if (vxlan_addr_multicast(&vxlan->default_dst.remote_ip)) {
-		vxlan_sock_hold(vs);
-		dev_hold(dev);
-		queue_work(vxlan_wq, &vxlan->igmp_join);
+		ret = vxlan_igmp_join(vxlan);
+		if (ret) {
+			vxlan_sock_release(vs);
+			return ret;
+		}
 	}
 
 	if (vxlan->age_interval)
 		mod_timer(&vxlan->age_timer, jiffies + FDB_AGE_INTERVAL);
 
-	return 0;
+	return ret;
 }
 
 /* Purge the forwarding table */
@@ -2279,19 +2253,18 @@ static int vxlan_stop(struct net_device *dev)
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 	struct vxlan_net *vn = net_generic(vxlan->net, vxlan_net_id);
 	struct vxlan_sock *vs = vxlan->vn_sock;
+	int ret = 0;
 
-	if (vs && vxlan_addr_multicast(&vxlan->default_dst.remote_ip) &&
-	    !vxlan_group_used(vn, vxlan)) {
-		vxlan_sock_hold(vs);
-		dev_hold(dev);
-		queue_work(vxlan_wq, &vxlan->igmp_leave);
-	}
+	if (vxlan_addr_multicast(&vxlan->default_dst.remote_ip) &&
+	    !vxlan_group_used(vn, vxlan))
+		ret = vxlan_igmp_leave(vxlan);
 
 	del_timer_sync(&vxlan->age_timer);
 
 	vxlan_flush(vxlan);
+	vxlan_sock_release(vs);
 
-	return 0;
+	return ret;
 }
 
 /* Stub, nothing needs to be done. */
@@ -2402,9 +2375,6 @@ static void vxlan_setup(struct net_device *dev)
 
 	INIT_LIST_HEAD(&vxlan->next);
 	spin_lock_init(&vxlan->hash_lock);
-	INIT_WORK(&vxlan->igmp_join, vxlan_igmp_join);
-	INIT_WORK(&vxlan->igmp_leave, vxlan_igmp_leave);
-	INIT_WORK(&vxlan->sock_work, vxlan_sock_work);
 
 	init_timer_deferrable(&vxlan->age_timer);
 	vxlan->age_timer.function = vxlan_cleanup;
@@ -2516,7 +2486,6 @@ static struct socket *vxlan_create_sock(struct net *net, bool ipv6,
 		    !(flags & VXLAN_F_UDP_ZERO_CSUM6_RX);
 	} else {
 		udp_conf.family = AF_INET;
-		udp_conf.local_ip.s_addr = INADDR_ANY;
 	}
 
 	udp_conf.local_udp_port = port;
@@ -2552,6 +2521,8 @@ static struct vxlan_sock *vxlan_socket_create(struct net *net, __be16 port,
 
 	sock = vxlan_create_sock(net, ipv6, port, flags);
 	if (IS_ERR(sock)) {
+		pr_info("Cannot bind port %d, err=%ld\n", ntohs(port),
+			PTR_ERR(sock));
 		kfree(vs);
 		return ERR_CAST(sock);
 	}
@@ -2591,45 +2562,23 @@ struct vxlan_sock *vxlan_sock_add(struct net *net, __be16 port,
 	struct vxlan_sock *vs;
 	bool ipv6 = flags & VXLAN_F_IPV6;
 
-	vs = vxlan_socket_create(net, port, rcv, data, flags);
-	if (!IS_ERR(vs))
-		return vs;
-
-	if (no_share)	/* Return error if sharing is not allowed. */
-		return vs;
-
-	spin_lock(&vn->sock_lock);
-	vs = vxlan_find_sock(net, ipv6 ? AF_INET6 : AF_INET, port, flags);
-	if (vs && ((vs->rcv != rcv) ||
-		   !atomic_add_unless(&vs->refcnt, 1, 0)))
-			vs = ERR_PTR(-EBUSY);
-	spin_unlock(&vn->sock_lock);
-
-	if (!vs)
-		vs = ERR_PTR(-EINVAL);
+	if (!no_share) {
+		spin_lock(&vn->sock_lock);
+		vs = vxlan_find_sock(net, ipv6 ? AF_INET6 : AF_INET, port,
+				     flags);
+		if (vs && vs->rcv == rcv) {
+			if (!atomic_add_unless(&vs->refcnt, 1, 0))
+				vs = ERR_PTR(-EBUSY);
+			spin_unlock(&vn->sock_lock);
+			return vs;
+		}
+		spin_unlock(&vn->sock_lock);
+	}
 
-	return vs;
+	return vxlan_socket_create(net, port, rcv, data, flags);
 }
 EXPORT_SYMBOL_GPL(vxlan_sock_add);
 
-/* Scheduled at device creation to bind to a socket */
-static void vxlan_sock_work(struct work_struct *work)
-{
-	struct vxlan_dev *vxlan = container_of(work, struct vxlan_dev, sock_work);
-	struct net *net = vxlan->net;
-	struct vxlan_net *vn = net_generic(net, vxlan_net_id);
-	__be16 port = vxlan->dst_port;
-	struct vxlan_sock *nvs;
-
-	nvs = vxlan_sock_add(net, port, vxlan_rcv, NULL, false, vxlan->flags);
-	spin_lock(&vn->sock_lock);
-	if (!IS_ERR(nvs))
-		vxlan_vs_add_dev(nvs, vxlan);
-	spin_unlock(&vn->sock_lock);
-
-	dev_put(vxlan->dev);
-}
-
 static int vxlan_newlink(struct net *src_net, struct net_device *dev,
 			 struct nlattr *tb[], struct nlattr *data[])
 {
@@ -2651,27 +2600,25 @@ static int vxlan_newlink(struct net *src_net, struct net_device *dev,
 	/* Unless IPv6 is explicitly requested, assume IPv4 */
 	dst->remote_ip.sa.sa_family = AF_INET;
 	if (data[IFLA_VXLAN_GROUP]) {
-		dst->remote_ip.sin.sin_addr.s_addr = nla_get_be32(data[IFLA_VXLAN_GROUP]);
+		dst->remote_ip.sin.sin_addr.s_addr = nla_get_in_addr(data[IFLA_VXLAN_GROUP]);
 	} else if (data[IFLA_VXLAN_GROUP6]) {
 		if (!IS_ENABLED(CONFIG_IPV6))
 			return -EPFNOSUPPORT;
 
-		nla_memcpy(&dst->remote_ip.sin6.sin6_addr, data[IFLA_VXLAN_GROUP6],
-			   sizeof(struct in6_addr));
+		dst->remote_ip.sin6.sin6_addr = nla_get_in6_addr(data[IFLA_VXLAN_GROUP6]);
 		dst->remote_ip.sa.sa_family = AF_INET6;
 		use_ipv6 = true;
 	}
 
 	if (data[IFLA_VXLAN_LOCAL]) {
-		vxlan->saddr.sin.sin_addr.s_addr = nla_get_be32(data[IFLA_VXLAN_LOCAL]);
+		vxlan->saddr.sin.sin_addr.s_addr = nla_get_in_addr(data[IFLA_VXLAN_LOCAL]);
 		vxlan->saddr.sa.sa_family = AF_INET;
 	} else if (data[IFLA_VXLAN_LOCAL6]) {
 		if (!IS_ENABLED(CONFIG_IPV6))
 			return -EPFNOSUPPORT;
 
 		/* TODO: respect scope id */
-		nla_memcpy(&vxlan->saddr.sin6.sin6_addr, data[IFLA_VXLAN_LOCAL6],
-			   sizeof(struct in6_addr));
+		vxlan->saddr.sin6.sin6_addr = nla_get_in6_addr(data[IFLA_VXLAN_LOCAL6]);
 		vxlan->saddr.sa.sa_family = AF_INET6;
 		use_ipv6 = true;
 	}
@@ -2856,13 +2803,13 @@ static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 
 	if (!vxlan_addr_any(&dst->remote_ip)) {
 		if (dst->remote_ip.sa.sa_family == AF_INET) {
-			if (nla_put_be32(skb, IFLA_VXLAN_GROUP,
-					 dst->remote_ip.sin.sin_addr.s_addr))
+			if (nla_put_in_addr(skb, IFLA_VXLAN_GROUP,
+					    dst->remote_ip.sin.sin_addr.s_addr))
 				goto nla_put_failure;
 #if IS_ENABLED(CONFIG_IPV6)
 		} else {
-			if (nla_put(skb, IFLA_VXLAN_GROUP6, sizeof(struct in6_addr),
-				    &dst->remote_ip.sin6.sin6_addr))
+			if (nla_put_in6_addr(skb, IFLA_VXLAN_GROUP6,
+					     &dst->remote_ip.sin6.sin6_addr))
 				goto nla_put_failure;
 #endif
 		}
@@ -2873,13 +2820,13 @@ static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 
 	if (!vxlan_addr_any(&vxlan->saddr)) {
 		if (vxlan->saddr.sa.sa_family == AF_INET) {
-			if (nla_put_be32(skb, IFLA_VXLAN_LOCAL,
-					 vxlan->saddr.sin.sin_addr.s_addr))
+			if (nla_put_in_addr(skb, IFLA_VXLAN_LOCAL,
+					    vxlan->saddr.sin.sin_addr.s_addr))
 				goto nla_put_failure;
 #if IS_ENABLED(CONFIG_IPV6)
 		} else {
-			if (nla_put(skb, IFLA_VXLAN_LOCAL6, sizeof(struct in6_addr),
-				    &vxlan->saddr.sin6.sin6_addr))
+			if (nla_put_in6_addr(skb, IFLA_VXLAN_LOCAL6,
+					     &vxlan->saddr.sin6.sin6_addr))
 				goto nla_put_failure;
 #endif
 		}
diff --git a/drivers/net/wan/cosa.c b/drivers/net/wan/cosa.c
index 88d121d43c08..bcfa01add7cc 100644
--- a/drivers/net/wan/cosa.c
+++ b/drivers/net/wan/cosa.c
@@ -579,6 +579,7 @@ static int cosa_probe(int base, int irq, int dma)
 		/* Register the network interface */
 		if (!(chan->netdev = alloc_hdlcdev(chan))) {
 			pr_warn("%s: alloc_hdlcdev failed\n", chan->name);
+			err = -ENOMEM;
 			goto err_hdlcdev;
 		}
 		dev_to_hdlc(chan->netdev)->attach = cosa_net_attach;
diff --git a/drivers/net/wan/lmc/lmc_main.c b/drivers/net/wan/lmc/lmc_main.c
index bea0f313a7a8..317bc79cc8b9 100644
--- a/drivers/net/wan/lmc/lmc_main.c
+++ b/drivers/net/wan/lmc/lmc_main.c
@@ -850,6 +850,7 @@ static int lmc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	dev = alloc_hdlcdev(sc);
 	if (!dev) {
 		printk(KERN_ERR "lmc:alloc_netdev for device failed\n");
+		err = -ENOMEM;
 		goto err_hdlcdev;
 	}
 
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/airo.c
index e71a2ce7a448..d0c97c220026 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/airo.c
@@ -2676,7 +2676,7 @@ static void wifi_setup(struct net_device *dev)
 	dev->addr_len           = ETH_ALEN;
 	dev->tx_queue_len       = 100; 
 
-	memset(dev->broadcast,0xFF, ETH_ALEN);
+	eth_broadcast_addr(dev->broadcast);
 
 	dev->flags              = IFF_BROADCAST|IFF_MULTICAST;
 }
@@ -3211,7 +3211,7 @@ static void airo_print_status(const char *devname, u16 status)
 			airo_print_dbg(devname, "link lost (TSF sync lost)");
 			break;
 		default:
-			airo_print_dbg(devname, "unknow status %x\n", status);
+			airo_print_dbg(devname, "unknown status %x\n", status);
 			break;
 		}
 		break;
@@ -3233,7 +3233,7 @@ static void airo_print_status(const char *devname, u16 status)
 	case STAT_REASSOC:
 		break;
 	default:
-		airo_print_dbg(devname, "unknow status %x\n", status);
+		airo_print_dbg(devname, "unknown status %x\n", status);
 		break;
 	}
 }
@@ -3273,7 +3273,7 @@ static void airo_handle_link(struct airo_info *ai)
 		}
 
 		/* Send event to user space */
-		memset(wrqu.ap_addr.sa_data, '\0', ETH_ALEN);
+		eth_zero_addr(wrqu.ap_addr.sa_data);
 		wrqu.ap_addr.sa_family = ARPHRD_ETHER;
 		wireless_send_event(ai->dev, SIOCGIWAP, &wrqu, NULL);
 	}
diff --git a/drivers/net/wireless/at76c50x-usb.c b/drivers/net/wireless/at76c50x-usb.c
index da92bfa76b7c..49219c508963 100644
--- a/drivers/net/wireless/at76c50x-usb.c
+++ b/drivers/net/wireless/at76c50x-usb.c
@@ -1166,7 +1166,7 @@ static int at76_start_monitor(struct at76_priv *priv)
 	int ret;
 
 	memset(&scan, 0, sizeof(struct at76_req_scan));
-	memset(scan.bssid, 0xff, ETH_ALEN);
+	eth_broadcast_addr(scan.bssid);
 
 	scan.channel = priv->channel;
 	scan.scan_type = SCAN_TYPE_PASSIVE;
@@ -1427,7 +1427,7 @@ static int at76_startup_device(struct at76_priv *priv)
 	at76_wait_completion(priv, CMD_STARTUP);
 
 	/* remove BSSID from previous run */
-	memset(priv->bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->bssid);
 
 	priv->scanning = false;
 
@@ -1973,7 +1973,7 @@ static int at76_hw_scan(struct ieee80211_hw *hw,
 	ieee80211_stop_queues(hw);
 
 	memset(&scan, 0, sizeof(struct at76_req_scan));
-	memset(scan.bssid, 0xFF, ETH_ALEN);
+	eth_broadcast_addr(scan.bssid);
 
 	if (req->n_ssids) {
 		scan.scan_type = SCAN_TYPE_ACTIVE;
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.c b/drivers/net/wireless/ath/ar5523/ar5523.c
index f92050617ae6..5147ebe4cd05 100644
--- a/drivers/net/wireless/ath/ar5523/ar5523.c
+++ b/drivers/net/wireless/ath/ar5523/ar5523.c
@@ -779,8 +779,6 @@ static void ar5523_tx(struct ieee80211_hw *hw,
 		ieee80211_stop_queues(hw);
 	}
 
-	data->skb = skb;
-
 	spin_lock_irqsave(&ar->tx_data_list_lock, flags);
 	list_add_tail(&data->list, &ar->tx_queue_pending);
 	spin_unlock_irqrestore(&ar->tx_data_list_lock, flags);
@@ -817,10 +815,13 @@ static void ar5523_tx_work_locked(struct ar5523 *ar)
 		if (!data)
 			break;
 
-		skb = data->skb;
+		txi = container_of((void *)data, struct ieee80211_tx_info,
+				   driver_data);
 		txqid = 0;
-		txi = IEEE80211_SKB_CB(skb);
+
+		skb = container_of((void *)txi, struct sk_buff, cb);
 		paylen = skb->len;
+
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
 			ar5523_err(ar, "Failed to allocate TX urb\n");
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.h b/drivers/net/wireless/ath/ar5523/ar5523.h
index 00c6fd346d48..9a322a65cdb5 100644
--- a/drivers/net/wireless/ath/ar5523/ar5523.h
+++ b/drivers/net/wireless/ath/ar5523/ar5523.h
@@ -74,7 +74,6 @@ struct ar5523_tx_cmd {
 struct ar5523_tx_data {
 	struct list_head	list;
 	struct ar5523		*ar;
-	struct sk_buff		*skb;
 	struct urb		*urb;
 };
 
diff --git a/drivers/net/wireless/ath/ath.h b/drivers/net/wireless/ath/ath.h
index 1eebe2ea3dfb..7e9481099a8e 100644
--- a/drivers/net/wireless/ath/ath.h
+++ b/drivers/net/wireless/ath/ath.h
@@ -131,6 +131,9 @@ struct ath_ops {
 	void (*enable_write_buffer)(void *);
 	void (*write_flush) (void *);
 	u32 (*rmw)(void *, u32 reg_offset, u32 set, u32 clr);
+	void (*enable_rmw_buffer)(void *);
+	void (*rmw_flush) (void *);
+
 };
 
 struct ath_common;
diff --git a/drivers/net/wireless/ath/ath10k/ce.h b/drivers/net/wireless/ath/ath10k/ce.h
index c18647b87f71..0eddb204d85b 100644
--- a/drivers/net/wireless/ath/ath10k/ce.h
+++ b/drivers/net/wireless/ath/ath10k/ce.h
@@ -39,7 +39,7 @@ struct ath10k_ce_pipe;
 #define CE_DESC_FLAGS_GATHER         (1 << 0)
 #define CE_DESC_FLAGS_BYTE_SWAP      (1 << 1)
 #define CE_DESC_FLAGS_META_DATA_MASK 0xFFFC
-#define CE_DESC_FLAGS_META_DATA_LSB  3
+#define CE_DESC_FLAGS_META_DATA_LSB  2
 
 struct ce_desc {
 	__le32 addr;
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 310e12bc078a..c0e454bb6a8d 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -436,16 +436,16 @@ static int ath10k_download_fw(struct ath10k *ar, enum ath10k_firmware_mode mode)
 
 static void ath10k_core_free_firmware_files(struct ath10k *ar)
 {
-	if (ar->board && !IS_ERR(ar->board))
+	if (!IS_ERR(ar->board))
 		release_firmware(ar->board);
 
-	if (ar->otp && !IS_ERR(ar->otp))
+	if (!IS_ERR(ar->otp))
 		release_firmware(ar->otp);
 
-	if (ar->firmware && !IS_ERR(ar->firmware))
+	if (!IS_ERR(ar->firmware))
 		release_firmware(ar->firmware);
 
-	if (ar->cal_file && !IS_ERR(ar->cal_file))
+	if (!IS_ERR(ar->cal_file))
 		release_firmware(ar->cal_file);
 
 	ar->board = NULL;
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index d60e46fe6d19..f65310c3ba5f 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -159,6 +159,25 @@ struct ath10k_fw_stats_peer {
 	u32 peer_rx_rate; /* 10x only */
 };
 
+struct ath10k_fw_stats_vdev {
+	struct list_head list;
+
+	u32 vdev_id;
+	u32 beacon_snr;
+	u32 data_snr;
+	u32 num_tx_frames[4];
+	u32 num_rx_frames;
+	u32 num_tx_frames_retries[4];
+	u32 num_tx_frames_failures[4];
+	u32 num_rts_fail;
+	u32 num_rts_success;
+	u32 num_rx_err;
+	u32 num_rx_discard;
+	u32 num_tx_not_acked;
+	u32 tx_rate_history[10];
+	u32 beacon_rssi_history[10];
+};
+
 struct ath10k_fw_stats_pdev {
 	struct list_head list;
 
@@ -220,6 +239,7 @@ struct ath10k_fw_stats_pdev {
 
 struct ath10k_fw_stats {
 	struct list_head pdevs;
+	struct list_head vdevs;
 	struct list_head peers;
 };
 
@@ -288,6 +308,7 @@ struct ath10k_vif {
 	bool is_started;
 	bool is_up;
 	bool spectral_enabled;
+	bool ps;
 	u32 aid;
 	u8 bssid[ETH_ALEN];
 
@@ -413,6 +434,12 @@ enum ath10k_fw_features {
 	 */
 	ATH10K_FW_FEATURE_WMI_10_2 = 4,
 
+	/* Some firmware revisions lack proper multi-interface client powersave
+	 * implementation. Enabling PS could result in connection drops,
+	 * traffic stalls, etc.
+	 */
+	ATH10K_FW_FEATURE_MULTI_VIF_PS_SUPPORT = 5,
+
 	/* keep last */
 	ATH10K_FW_FEATURE_COUNT,
 };
diff --git a/drivers/net/wireless/ath/ath10k/debug.c b/drivers/net/wireless/ath/ath10k/debug.c
index d2281e5c2ffe..301081db1ef6 100644
--- a/drivers/net/wireless/ath/ath10k/debug.c
+++ b/drivers/net/wireless/ath/ath10k/debug.c
@@ -243,6 +243,16 @@ static void ath10k_debug_fw_stats_pdevs_free(struct list_head *head)
 	}
 }
 
+static void ath10k_debug_fw_stats_vdevs_free(struct list_head *head)
+{
+	struct ath10k_fw_stats_vdev *i, *tmp;
+
+	list_for_each_entry_safe(i, tmp, head, list) {
+		list_del(&i->list);
+		kfree(i);
+	}
+}
+
 static void ath10k_debug_fw_stats_peers_free(struct list_head *head)
 {
 	struct ath10k_fw_stats_peer *i, *tmp;
@@ -258,6 +268,7 @@ static void ath10k_debug_fw_stats_reset(struct ath10k *ar)
 	spin_lock_bh(&ar->data_lock);
 	ar->debug.fw_stats_done = false;
 	ath10k_debug_fw_stats_pdevs_free(&ar->debug.fw_stats.pdevs);
+	ath10k_debug_fw_stats_vdevs_free(&ar->debug.fw_stats.vdevs);
 	ath10k_debug_fw_stats_peers_free(&ar->debug.fw_stats.peers);
 	spin_unlock_bh(&ar->data_lock);
 }
@@ -273,14 +284,27 @@ static size_t ath10k_debug_fw_stats_num_peers(struct list_head *head)
 	return num;
 }
 
+static size_t ath10k_debug_fw_stats_num_vdevs(struct list_head *head)
+{
+	struct ath10k_fw_stats_vdev *i;
+	size_t num = 0;
+
+	list_for_each_entry(i, head, list)
+		++num;
+
+	return num;
+}
+
 void ath10k_debug_fw_stats_process(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct ath10k_fw_stats stats = {};
 	bool is_start, is_started, is_end;
 	size_t num_peers;
+	size_t num_vdevs;
 	int ret;
 
 	INIT_LIST_HEAD(&stats.pdevs);
+	INIT_LIST_HEAD(&stats.vdevs);
 	INIT_LIST_HEAD(&stats.peers);
 
 	spin_lock_bh(&ar->data_lock);
@@ -308,6 +332,7 @@ void ath10k_debug_fw_stats_process(struct ath10k *ar, struct sk_buff *skb)
 	}
 
 	num_peers = ath10k_debug_fw_stats_num_peers(&ar->debug.fw_stats.peers);
+	num_vdevs = ath10k_debug_fw_stats_num_vdevs(&ar->debug.fw_stats.vdevs);
 	is_start = (list_empty(&ar->debug.fw_stats.pdevs) &&
 		    !list_empty(&stats.pdevs));
 	is_end = (!list_empty(&ar->debug.fw_stats.pdevs) &&
@@ -330,7 +355,13 @@ void ath10k_debug_fw_stats_process(struct ath10k *ar, struct sk_buff *skb)
 			goto free;
 		}
 
+		if (num_vdevs >= BITS_PER_LONG) {
+			ath10k_warn(ar, "dropping fw vdev stats\n");
+			goto free;
+		}
+
 		list_splice_tail_init(&stats.peers, &ar->debug.fw_stats.peers);
+		list_splice_tail_init(&stats.vdevs, &ar->debug.fw_stats.vdevs);
 	}
 
 	complete(&ar->debug.fw_stats_complete);
@@ -340,6 +371,7 @@ free:
 	 * resources if that is not the case.
 	 */
 	ath10k_debug_fw_stats_pdevs_free(&stats.pdevs);
+	ath10k_debug_fw_stats_vdevs_free(&stats.vdevs);
 	ath10k_debug_fw_stats_peers_free(&stats.peers);
 
 unlock:
@@ -363,7 +395,10 @@ static int ath10k_debug_fw_stats_request(struct ath10k *ar)
 
 		reinit_completion(&ar->debug.fw_stats_complete);
 
-		ret = ath10k_wmi_request_stats(ar, WMI_REQUEST_PEER_STAT);
+		ret = ath10k_wmi_request_stats(ar,
+					       WMI_STAT_PDEV |
+					       WMI_STAT_VDEV |
+					       WMI_STAT_PEER);
 		if (ret) {
 			ath10k_warn(ar, "could not request stats (%d)\n", ret);
 			return ret;
@@ -395,8 +430,11 @@ static void ath10k_fw_stats_fill(struct ath10k *ar,
 	unsigned int len = 0;
 	unsigned int buf_len = ATH10K_FW_STATS_BUF_SIZE;
 	const struct ath10k_fw_stats_pdev *pdev;
+	const struct ath10k_fw_stats_vdev *vdev;
 	const struct ath10k_fw_stats_peer *peer;
 	size_t num_peers;
+	size_t num_vdevs;
+	int i;
 
 	spin_lock_bh(&ar->data_lock);
 
@@ -408,6 +446,7 @@ static void ath10k_fw_stats_fill(struct ath10k *ar,
 	}
 
 	num_peers = ath10k_debug_fw_stats_num_peers(&fw_stats->peers);
+	num_vdevs = ath10k_debug_fw_stats_num_vdevs(&fw_stats->vdevs);
 
 	len += scnprintf(buf + len, buf_len - len, "\n");
 	len += scnprintf(buf + len, buf_len - len, "%30s\n",
@@ -531,6 +570,65 @@ static void ath10k_fw_stats_fill(struct ath10k *ar,
 
 	len += scnprintf(buf + len, buf_len - len, "\n");
 	len += scnprintf(buf + len, buf_len - len, "%30s (%zu)\n",
+			 "ath10k VDEV stats", num_vdevs);
+	len += scnprintf(buf + len, buf_len - len, "%30s\n\n",
+				 "=================");
+
+	list_for_each_entry(vdev, &fw_stats->vdevs, list) {
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "vdev id", vdev->vdev_id);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "beacon snr", vdev->beacon_snr);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "data snr", vdev->data_snr);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "num rx frames", vdev->num_rx_frames);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "num rts fail", vdev->num_rts_fail);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "num rts success", vdev->num_rts_success);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "num rx err", vdev->num_rx_err);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "num rx discard", vdev->num_rx_discard);
+		len += scnprintf(buf + len, buf_len - len, "%30s %u\n",
+				 "num tx not acked", vdev->num_tx_not_acked);
+
+		for (i = 0 ; i < ARRAY_SIZE(vdev->num_tx_frames); i++)
+			len += scnprintf(buf + len, buf_len - len,
+					"%25s [%02d] %u\n",
+					 "num tx frames", i,
+					 vdev->num_tx_frames[i]);
+
+		for (i = 0 ; i < ARRAY_SIZE(vdev->num_tx_frames_retries); i++)
+			len += scnprintf(buf + len, buf_len - len,
+					"%25s [%02d] %u\n",
+					 "num tx frames retries", i,
+					 vdev->num_tx_frames_retries[i]);
+
+		for (i = 0 ; i < ARRAY_SIZE(vdev->num_tx_frames_failures); i++)
+			len += scnprintf(buf + len, buf_len - len,
+					"%25s [%02d] %u\n",
+					 "num tx frames failures", i,
+					 vdev->num_tx_frames_failures[i]);
+
+		for (i = 0 ; i < ARRAY_SIZE(vdev->tx_rate_history); i++)
+			len += scnprintf(buf + len, buf_len - len,
+					"%25s [%02d] 0x%08x\n",
+					 "tx rate history", i,
+					 vdev->tx_rate_history[i]);
+
+		for (i = 0 ; i < ARRAY_SIZE(vdev->beacon_rssi_history); i++)
+			len += scnprintf(buf + len, buf_len - len,
+					"%25s [%02d] %u\n",
+					 "beacon rssi history", i,
+					 vdev->beacon_rssi_history[i]);
+
+		len += scnprintf(buf + len, buf_len - len, "\n");
+	}
+
+	len += scnprintf(buf + len, buf_len - len, "\n");
+	len += scnprintf(buf + len, buf_len - len, "%30s (%zu)\n",
 			 "ath10k PEER stats", num_peers);
 	len += scnprintf(buf + len, buf_len - len, "%30s\n\n",
 				 "=================");
@@ -1900,6 +1998,7 @@ int ath10k_debug_create(struct ath10k *ar)
 		return -ENOMEM;
 
 	INIT_LIST_HEAD(&ar->debug.fw_stats.pdevs);
+	INIT_LIST_HEAD(&ar->debug.fw_stats.vdevs);
 	INIT_LIST_HEAD(&ar->debug.fw_stats.peers);
 
 	return 0;
diff --git a/drivers/net/wireless/ath/ath10k/htt_rx.c b/drivers/net/wireless/ath/ath10k/htt_rx.c
index c1da44f65a4d..01a2b384f358 100644
--- a/drivers/net/wireless/ath/ath10k/htt_rx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_rx.c
@@ -176,7 +176,7 @@ static void ath10k_htt_rx_msdu_buff_replenish(struct ath10k_htt *htt)
 	 * automatically balances load wrt to CPU power.
 	 *
 	 * This probably comes at a cost of lower maximum throughput but
-	 * improves the avarage and stability. */
+	 * improves the average and stability. */
 	spin_lock_bh(&htt->rx_ring.lock);
 	num_deficit = htt->rx_ring.fill_level - htt->rx_ring.fill_cnt;
 	num_to_fill = min(ATH10K_HTT_MAX_NUM_REFILL, num_deficit);
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index d6d2f0f00caa..973485bd4121 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -611,7 +611,7 @@ static int ath10k_monitor_vdev_start(struct ath10k *ar, int vdev_id)
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn(ar, "failed to synchronize setup for monitor vdev %i: %d\n",
+		ath10k_warn(ar, "failed to synchronize setup for monitor vdev %i start: %d\n",
 			    vdev_id, ret);
 		return ret;
 	}
@@ -658,7 +658,7 @@ static int ath10k_monitor_vdev_stop(struct ath10k *ar)
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret)
-		ath10k_warn(ar, "failed to synchronise monitor vdev %i: %d\n",
+		ath10k_warn(ar, "failed to synchronize monitor vdev %i stop: %d\n",
 			    ar->monitor_vdev_id, ret);
 
 	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor vdev %i stopped\n",
@@ -927,8 +927,9 @@ static int ath10k_vdev_start_restart(struct ath10k_vif *arvif, bool restart)
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn(ar, "failed to synchronise setup for vdev %i: %d\n",
-			    arg.vdev_id, ret);
+		ath10k_warn(ar,
+			    "failed to synchronize setup for vdev %i restart %d: %d\n",
+			    arg.vdev_id, restart, ret);
 		return ret;
 	}
 
@@ -966,7 +967,7 @@ static int ath10k_vdev_stop(struct ath10k_vif *arvif)
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn(ar, "failed to syncronise setup for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to synchronize setup for vdev %i stop: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -1182,7 +1183,7 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 		if (is_zero_ether_addr(arvif->bssid))
 			return;
 
-		memset(arvif->bssid, 0, ETH_ALEN);
+		eth_zero_addr(arvif->bssid);
 
 		return;
 	}
@@ -1253,6 +1254,20 @@ static int ath10k_mac_vif_recalc_ps_poll_count(struct ath10k_vif *arvif)
 	return 0;
 }
 
+static int ath10k_mac_ps_vif_count(struct ath10k *ar)
+{
+	struct ath10k_vif *arvif;
+	int num = 0;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	list_for_each_entry(arvif, &ar->arvifs, list)
+		if (arvif->ps)
+			num++;
+
+	return num;
+}
+
 static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 {
 	struct ath10k *ar = arvif->ar;
@@ -1262,13 +1277,24 @@ static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 	enum wmi_sta_ps_mode psmode;
 	int ret;
 	int ps_timeout;
+	bool enable_ps;
 
 	lockdep_assert_held(&arvif->ar->conf_mutex);
 
 	if (arvif->vif->type != NL80211_IFTYPE_STATION)
 		return 0;
 
-	if (vif->bss_conf.ps) {
+	enable_ps = arvif->ps;
+
+	if (enable_ps && ath10k_mac_ps_vif_count(ar) > 1 &&
+	    !test_bit(ATH10K_FW_FEATURE_MULTI_VIF_PS_SUPPORT,
+		      ar->fw_features)) {
+		ath10k_warn(ar, "refusing to enable ps on vdev %i: not supported by fw\n",
+			    arvif->vdev_id);
+		enable_ps = false;
+	}
+
+	if (enable_ps) {
 		psmode = WMI_STA_PS_MODE_ENABLED;
 		param = WMI_STA_PS_PARAM_INACTIVITY_TIME;
 
@@ -1386,7 +1412,8 @@ static void ath10k_peer_assoc_h_crypto(struct ath10k *ar,
 	lockdep_assert_held(&ar->conf_mutex);
 
 	bss = cfg80211_get_bss(ar->hw->wiphy, ar->hw->conf.chandef.chan,
-			       info->bssid, NULL, 0, 0, 0);
+			       info->bssid, NULL, 0, IEEE80211_BSS_TYPE_ANY,
+			       IEEE80211_PRIVACY_ANY);
 	if (bss) {
 		const struct cfg80211_bss_ies *ies;
 
@@ -1781,6 +1808,68 @@ static int ath10k_setup_peer_smps(struct ath10k *ar, struct ath10k_vif *arvif,
 					 ath10k_smps_map[smps]);
 }
 
+static int ath10k_mac_vif_recalc_txbf(struct ath10k *ar,
+				      struct ieee80211_vif *vif,
+				      struct ieee80211_sta_vht_cap vht_cap)
+{
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	int ret;
+	u32 param;
+	u32 value;
+
+	if (!(ar->vht_cap_info &
+	      (IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+	       IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE |
+	       IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
+	       IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE)))
+		return 0;
+
+	param = ar->wmi.vdev_param->txbf;
+	value = 0;
+
+	if (WARN_ON(param == WMI_VDEV_PARAM_UNSUPPORTED))
+		return 0;
+
+	/* The following logic is correct. If a remote STA advertises support
+	 * for being a beamformer then we should enable us being a beamformee.
+	 */
+
+	if (ar->vht_cap_info &
+	    (IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+	     IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE)) {
+		if (vht_cap.cap & IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE)
+			value |= WMI_VDEV_PARAM_TXBF_SU_TX_BFEE;
+
+		if (vht_cap.cap & IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE)
+			value |= WMI_VDEV_PARAM_TXBF_MU_TX_BFEE;
+	}
+
+	if (ar->vht_cap_info &
+	    (IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
+	     IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE)) {
+		if (vht_cap.cap & IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE)
+			value |= WMI_VDEV_PARAM_TXBF_SU_TX_BFER;
+
+		if (vht_cap.cap & IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE)
+			value |= WMI_VDEV_PARAM_TXBF_MU_TX_BFER;
+	}
+
+	if (value & WMI_VDEV_PARAM_TXBF_MU_TX_BFEE)
+		value |= WMI_VDEV_PARAM_TXBF_SU_TX_BFEE;
+
+	if (value & WMI_VDEV_PARAM_TXBF_MU_TX_BFER)
+		value |= WMI_VDEV_PARAM_TXBF_SU_TX_BFER;
+
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, param, value);
+	if (ret) {
+		ath10k_warn(ar, "failed to submit vdev param txbf 0x%x: %d\n",
+			    value, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 /* can be called only in mac80211 callbacks due to `key_count` usage */
 static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 			     struct ieee80211_vif *vif,
@@ -1789,6 +1878,7 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 	struct ath10k *ar = hw->priv;
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	struct ieee80211_sta_ht_cap ht_cap;
+	struct ieee80211_sta_vht_cap vht_cap;
 	struct wmi_peer_assoc_complete_arg peer_arg;
 	struct ieee80211_sta *ap_sta;
 	int ret;
@@ -1811,6 +1901,7 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 	/* ap_sta must be accessed only within rcu section which must be left
 	 * before calling ath10k_setup_peer_smps() which might sleep. */
 	ht_cap = ap_sta->ht_cap;
+	vht_cap = ap_sta->vht_cap;
 
 	ret = ath10k_peer_assoc_prepare(ar, vif, ap_sta, &peer_arg);
 	if (ret) {
@@ -1836,6 +1927,13 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 		return;
 	}
 
+	ret = ath10k_mac_vif_recalc_txbf(ar, vif, vht_cap);
+	if (ret) {
+		ath10k_warn(ar, "failed to recalc txbf for vdev %i on bss %pM: %d\n",
+			    arvif->vdev_id, bss_conf->bssid, ret);
+		return;
+	}
+
 	ath10k_dbg(ar, ATH10K_DBG_MAC,
 		   "mac vdev %d up (associated) bssid %pM aid %d\n",
 		   arvif->vdev_id, bss_conf->bssid, bss_conf->aid);
@@ -1853,6 +1951,18 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 	}
 
 	arvif->is_up = true;
+
+	/* Workaround: Some firmware revisions (tested with qca6174
+	 * WLAN.RM.2.0-00073) have buggy powersave state machine and must be
+	 * poked with peer param command.
+	 */
+	ret = ath10k_wmi_peer_set_param(ar, arvif->vdev_id, arvif->bssid,
+					WMI_PEER_DUMMY_VAR, 1);
+	if (ret) {
+		ath10k_warn(ar, "failed to poke peer %pM param for ps workaround on vdev %i: %d\n",
+			    arvif->bssid, arvif->vdev_id, ret);
+		return;
+	}
 }
 
 static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
@@ -1860,6 +1970,7 @@ static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
 {
 	struct ath10k *ar = hw->priv;
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct ieee80211_sta_vht_cap vht_cap = {};
 	int ret;
 
 	lockdep_assert_held(&ar->conf_mutex);
@@ -1874,6 +1985,13 @@ static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
 
 	arvif->def_wep_key_idx = -1;
 
+	ret = ath10k_mac_vif_recalc_txbf(ar, vif, vht_cap);
+	if (ret) {
+		ath10k_warn(ar, "failed to recalc txbf for vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return;
+	}
+
 	arvif->is_up = false;
 }
 
@@ -2554,6 +2672,17 @@ static int ath10k_start_scan(struct ath10k *ar,
 		return -ETIMEDOUT;
 	}
 
+	/* If we failed to start the scan, return error code at
+	 * this point.  This is probably due to some issue in the
+	 * firmware, but no need to wedge the driver due to that...
+	 */
+	spin_lock_bh(&ar->data_lock);
+	if (ar->scan.state == ATH10K_SCAN_IDLE) {
+		spin_unlock_bh(&ar->data_lock);
+		return -EINVAL;
+	}
+	spin_unlock_bh(&ar->data_lock);
+
 	/* Add a 200ms margin to account for event/command processing */
 	ieee80211_queue_delayed_work(ar->hw, &ar->scan.timeout,
 				     msecs_to_jiffies(arg->max_scan_time+200));
@@ -3323,9 +3452,10 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 	list_del(&arvif->list);
 
 	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
-		ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id, vif->addr);
+		ret = ath10k_wmi_peer_delete(arvif->ar, arvif->vdev_id,
+					     vif->addr);
 		if (ret)
-			ath10k_warn(ar, "failed to remove peer for AP vdev %i: %d\n",
+			ath10k_warn(ar, "failed to submit AP self-peer removal on vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 
 		kfree(arvif->u.ap.noa_data);
@@ -3339,6 +3469,21 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 		ath10k_warn(ar, "failed to delete WMI vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 
+	/* Some firmware revisions don't notify host about self-peer removal
+	 * until after associated vdev is deleted.
+	 */
+	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
+		ret = ath10k_wait_for_peer_deleted(ar, arvif->vdev_id,
+						   vif->addr);
+		if (ret)
+			ath10k_warn(ar, "failed to remove AP self-peer on vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+
+		spin_lock_bh(&ar->data_lock);
+		ar->num_peers--;
+		spin_unlock_bh(&ar->data_lock);
+	}
+
 	ath10k_peer_cleanup(ar, arvif->vdev_id);
 
 	mutex_unlock(&ar->conf_mutex);
@@ -3534,7 +3679,9 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 	}
 
 	if (changed & BSS_CHANGED_PS) {
-		ret = ath10k_mac_vif_setup_ps(arvif);
+		arvif->ps = vif->bss_conf.ps;
+
+		ret = ath10k_config_ps(ar);
 		if (ret)
 			ath10k_warn(ar, "failed to setup ps on vdev %i: %d\n",
 				    arvif->vdev_id, ret);
@@ -4857,7 +5004,7 @@ static void ath10k_sta_rc_update(struct ieee80211_hw *hw,
 			bw = WMI_PEER_CHWIDTH_80MHZ;
 			break;
 		case IEEE80211_STA_RX_BW_160:
-			ath10k_warn(ar, "Invalid bandwith %d in rc update for %pM\n",
+			ath10k_warn(ar, "Invalid bandwidth %d in rc update for %pM\n",
 				    sta->bandwidth, sta->addr);
 			bw = WMI_PEER_CHWIDTH_20MHZ;
 			break;
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index e6972b09333e..7681237fe298 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -104,7 +104,7 @@ static const struct ce_attr host_ce_config_wlan[] = {
 	{
 		.flags = CE_ATTR_FLAGS,
 		.src_nentries = 0,
-		.src_sz_max = 512,
+		.src_sz_max = 2048,
 		.dest_nentries = 512,
 	},
 
@@ -174,7 +174,7 @@ static const struct ce_pipe_config target_ce_config_wlan[] = {
 		.pipenum = __cpu_to_le32(1),
 		.pipedir = __cpu_to_le32(PIPEDIR_IN),
 		.nentries = __cpu_to_le32(32),
-		.nbytes_max = __cpu_to_le32(512),
+		.nbytes_max = __cpu_to_le32(2048),
 		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
 		.reserved = __cpu_to_le32(0),
 	},
diff --git a/drivers/net/wireless/ath/ath10k/wmi-ops.h b/drivers/net/wireless/ath/ath10k/wmi-ops.h
index 04dc4b9db04e..c8b64e7a6089 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-ops.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-ops.h
@@ -110,8 +110,7 @@ struct wmi_ops {
 					  bool deliver_cab);
 	struct sk_buff *(*gen_pdev_set_wmm)(struct ath10k *ar,
 					    const struct wmi_wmm_params_all_arg *arg);
-	struct sk_buff *(*gen_request_stats)(struct ath10k *ar,
-					     enum wmi_stats_id stats_id);
+	struct sk_buff *(*gen_request_stats)(struct ath10k *ar, u32 stats_mask);
 	struct sk_buff *(*gen_force_fw_hang)(struct ath10k *ar,
 					     enum wmi_force_fw_hang_type type,
 					     u32 delay_ms);
@@ -816,14 +815,14 @@ ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
 }
 
 static inline int
-ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
+ath10k_wmi_request_stats(struct ath10k *ar, u32 stats_mask)
 {
 	struct sk_buff *skb;
 
 	if (!ar->wmi.ops->gen_request_stats)
 		return -EOPNOTSUPP;
 
-	skb = ar->wmi.ops->gen_request_stats(ar, stats_id);
+	skb = ar->wmi.ops->gen_request_stats(ar, stats_mask);
 	if (IS_ERR(skb))
 		return PTR_ERR(skb);
 
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.c b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
index 71614ba1b145..ee0c5f602e29 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.c
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
@@ -869,16 +869,57 @@ static int ath10k_wmi_tlv_op_pull_rdy_ev(struct ath10k *ar,
 	return 0;
 }
 
+static void ath10k_wmi_tlv_pull_vdev_stats(const struct wmi_tlv_vdev_stats *src,
+					   struct ath10k_fw_stats_vdev *dst)
+{
+	int i;
+
+	dst->vdev_id = __le32_to_cpu(src->vdev_id);
+	dst->beacon_snr = __le32_to_cpu(src->beacon_snr);
+	dst->data_snr = __le32_to_cpu(src->data_snr);
+	dst->num_rx_frames = __le32_to_cpu(src->num_rx_frames);
+	dst->num_rts_fail = __le32_to_cpu(src->num_rts_fail);
+	dst->num_rts_success = __le32_to_cpu(src->num_rts_success);
+	dst->num_rx_err = __le32_to_cpu(src->num_rx_err);
+	dst->num_rx_discard = __le32_to_cpu(src->num_rx_discard);
+	dst->num_tx_not_acked = __le32_to_cpu(src->num_tx_not_acked);
+
+	for (i = 0; i < ARRAY_SIZE(src->num_tx_frames); i++)
+		dst->num_tx_frames[i] =
+			__le32_to_cpu(src->num_tx_frames[i]);
+
+	for (i = 0; i < ARRAY_SIZE(src->num_tx_frames_retries); i++)
+		dst->num_tx_frames_retries[i] =
+			__le32_to_cpu(src->num_tx_frames_retries[i]);
+
+	for (i = 0; i < ARRAY_SIZE(src->num_tx_frames_failures); i++)
+		dst->num_tx_frames_failures[i] =
+			__le32_to_cpu(src->num_tx_frames_failures[i]);
+
+	for (i = 0; i < ARRAY_SIZE(src->tx_rate_history); i++)
+		dst->tx_rate_history[i] =
+			__le32_to_cpu(src->tx_rate_history[i]);
+
+	for (i = 0; i < ARRAY_SIZE(src->beacon_rssi_history); i++)
+		dst->beacon_rssi_history[i] =
+			__le32_to_cpu(src->beacon_rssi_history[i]);
+}
+
 static int ath10k_wmi_tlv_op_pull_fw_stats(struct ath10k *ar,
 					   struct sk_buff *skb,
 					   struct ath10k_fw_stats *stats)
 {
 	const void **tb;
-	const struct wmi_stats_event *ev;
+	const struct wmi_tlv_stats_ev *ev;
 	const void *data;
-	u32 num_pdev_stats, num_vdev_stats, num_peer_stats;
+	u32 num_pdev_stats;
+	u32 num_vdev_stats;
+	u32 num_peer_stats;
+	u32 num_bcnflt_stats;
+	u32 num_chan_stats;
 	size_t data_len;
 	int ret;
+	int i;
 
 	tb = ath10k_wmi_tlv_parse_alloc(ar, skb->data, skb->len, GFP_ATOMIC);
 	if (IS_ERR(tb)) {
@@ -899,8 +940,73 @@ static int ath10k_wmi_tlv_op_pull_fw_stats(struct ath10k *ar,
 	num_pdev_stats = __le32_to_cpu(ev->num_pdev_stats);
 	num_vdev_stats = __le32_to_cpu(ev->num_vdev_stats);
 	num_peer_stats = __le32_to_cpu(ev->num_peer_stats);
+	num_bcnflt_stats = __le32_to_cpu(ev->num_bcnflt_stats);
+	num_chan_stats = __le32_to_cpu(ev->num_chan_stats);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi tlv stats update pdev %i vdev %i peer %i bcnflt %i chan %i\n",
+		   num_pdev_stats, num_vdev_stats, num_peer_stats,
+		   num_bcnflt_stats, num_chan_stats);
+
+	for (i = 0; i < num_pdev_stats; i++) {
+		const struct wmi_pdev_stats *src;
+		struct ath10k_fw_stats_pdev *dst;
+
+		src = data;
+		if (data_len < sizeof(*src))
+			return -EPROTO;
+
+		data += sizeof(*src);
+		data_len -= sizeof(*src);
+
+		dst = kzalloc(sizeof(*dst), GFP_ATOMIC);
+		if (!dst)
+			continue;
+
+		ath10k_wmi_pull_pdev_stats_base(&src->base, dst);
+		ath10k_wmi_pull_pdev_stats_tx(&src->tx, dst);
+		ath10k_wmi_pull_pdev_stats_rx(&src->rx, dst);
+		list_add_tail(&dst->list, &stats->pdevs);
+	}
+
+	for (i = 0; i < num_vdev_stats; i++) {
+		const struct wmi_tlv_vdev_stats *src;
+		struct ath10k_fw_stats_vdev *dst;
+
+		src = data;
+		if (data_len < sizeof(*src))
+			return -EPROTO;
+
+		data += sizeof(*src);
+		data_len -= sizeof(*src);
+
+		dst = kzalloc(sizeof(*dst), GFP_ATOMIC);
+		if (!dst)
+			continue;
 
-	WARN_ON(1); /* FIXME: not implemented yet */
+		ath10k_wmi_tlv_pull_vdev_stats(src, dst);
+		list_add_tail(&dst->list, &stats->vdevs);
+	}
+
+	for (i = 0; i < num_peer_stats; i++) {
+		const struct wmi_10x_peer_stats *src;
+		struct ath10k_fw_stats_peer *dst;
+
+		src = data;
+		if (data_len < sizeof(*src))
+			return -EPROTO;
+
+		data += sizeof(*src);
+		data_len -= sizeof(*src);
+
+		dst = kzalloc(sizeof(*dst), GFP_ATOMIC);
+		if (!dst)
+			continue;
+
+		ath10k_wmi_pull_peer_stats(&src->old, dst);
+		dst->peer_rx_rate = __le32_to_cpu(src->peer_rx_rate);
+		list_add_tail(&dst->list, &stats->peers);
+	}
 
 	kfree(tb);
 	return 0;
@@ -1604,14 +1710,12 @@ ath10k_wmi_tlv_op_gen_vdev_wmm_conf(struct ath10k *ar, u32 vdev_id,
 				    const struct wmi_wmm_params_all_arg *arg)
 {
 	struct wmi_tlv_vdev_set_wmm_cmd *cmd;
-	struct wmi_wmm_params *wmm;
 	struct wmi_tlv *tlv;
 	struct sk_buff *skb;
 	size_t len;
 	void *ptr;
 
-	len = (sizeof(*tlv) + sizeof(*cmd)) +
-	      (4 * (sizeof(*tlv) + sizeof(*wmm)));
+	len = sizeof(*tlv) + sizeof(*cmd);
 	skb = ath10k_wmi_alloc_skb(ar, len);
 	if (!skb)
 		return ERR_PTR(-ENOMEM);
@@ -1623,13 +1727,10 @@ ath10k_wmi_tlv_op_gen_vdev_wmm_conf(struct ath10k *ar, u32 vdev_id,
 	cmd = (void *)tlv->value;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 
-	ptr += sizeof(*tlv);
-	ptr += sizeof(*cmd);
-
-	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_be);
-	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_bk);
-	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_vi);
-	ptr = ath10k_wmi_tlv_put_wmm(ptr, &arg->ac_vo);
+	ath10k_wmi_set_wmm_param(&cmd->vdev_wmm_params[0].params, &arg->ac_be);
+	ath10k_wmi_set_wmm_param(&cmd->vdev_wmm_params[1].params, &arg->ac_bk);
+	ath10k_wmi_set_wmm_param(&cmd->vdev_wmm_params[2].params, &arg->ac_vi);
+	ath10k_wmi_set_wmm_param(&cmd->vdev_wmm_params[3].params, &arg->ac_vo);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv vdev wmm conf\n");
 	return skb;
@@ -2080,8 +2181,7 @@ ath10k_wmi_tlv_op_gen_pdev_set_wmm(struct ath10k *ar,
 }
 
 static struct sk_buff *
-ath10k_wmi_tlv_op_gen_request_stats(struct ath10k *ar,
-				    enum wmi_stats_id stats_id)
+ath10k_wmi_tlv_op_gen_request_stats(struct ath10k *ar, u32 stats_mask)
 {
 	struct wmi_request_stats_cmd *cmd;
 	struct wmi_tlv *tlv;
@@ -2095,7 +2195,7 @@ ath10k_wmi_tlv_op_gen_request_stats(struct ath10k *ar,
 	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_REQUEST_STATS_CMD);
 	tlv->len = __cpu_to_le16(sizeof(*cmd));
 	cmd = (void *)tlv->value;
-	cmd->stats_id = __cpu_to_le32(stats_id);
+	cmd->stats_id = __cpu_to_le32(stats_mask);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv request stats\n");
 	return skb;
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.h b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
index de68fe76eae6..a6c8280cc4b1 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
@@ -1302,8 +1302,14 @@ struct wmi_tlv_pdev_set_wmm_cmd {
 	__le32 dg_type; /* no idea.. */
 } __packed;
 
+struct wmi_tlv_vdev_wmm_params {
+	__le32 dummy;
+	struct wmi_wmm_params params;
+} __packed;
+
 struct wmi_tlv_vdev_set_wmm_cmd {
 	__le32 vdev_id;
+	struct wmi_tlv_vdev_wmm_params vdev_wmm_params[4];
 } __packed;
 
 struct wmi_tlv_phyerr_ev {
@@ -1439,6 +1445,15 @@ struct wmi_tlv_sta_keepalive_cmd {
 	__le32 interval; /* in seconds */
 } __packed;
 
+struct wmi_tlv_stats_ev {
+	__le32 stats_id; /* WMI_STAT_ */
+	__le32 num_pdev_stats;
+	__le32 num_vdev_stats;
+	__le32 num_peer_stats;
+	__le32 num_bcnflt_stats;
+	__le32 num_chan_stats;
+} __packed;
+
 void ath10k_wmi_tlv_attach(struct ath10k *ar);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index aeea1c793943..c7ea77edce24 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -1125,6 +1125,25 @@ static void ath10k_wmi_event_scan_started(struct ath10k *ar)
 	}
 }
 
+static void ath10k_wmi_event_scan_start_failed(struct ath10k *ar)
+{
+	lockdep_assert_held(&ar->data_lock);
+
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
+		ath10k_warn(ar, "received scan start failed event in an invalid scan state: %s (%d)\n",
+			    ath10k_scan_state_str(ar->scan.state),
+			    ar->scan.state);
+		break;
+	case ATH10K_SCAN_STARTING:
+		complete(&ar->scan.started);
+		__ath10k_scan_finish(ar);
+		break;
+	}
+}
+
 static void ath10k_wmi_event_scan_completed(struct ath10k *ar)
 {
 	lockdep_assert_held(&ar->data_lock);
@@ -1292,6 +1311,7 @@ int ath10k_wmi_event_scan(struct ath10k *ar, struct sk_buff *skb)
 		break;
 	case WMI_SCAN_EVENT_START_FAILED:
 		ath10k_warn(ar, "received scan start failure event\n");
+		ath10k_wmi_event_scan_start_failed(ar);
 		break;
 	case WMI_SCAN_EVENT_DEQUEUED:
 	case WMI_SCAN_EVENT_PREEMPTED:
@@ -4954,7 +4974,7 @@ ath10k_wmi_op_gen_pdev_set_wmm(struct ath10k *ar,
 }
 
 static struct sk_buff *
-ath10k_wmi_op_gen_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
+ath10k_wmi_op_gen_request_stats(struct ath10k *ar, u32 stats_mask)
 {
 	struct wmi_request_stats_cmd *cmd;
 	struct sk_buff *skb;
@@ -4964,9 +4984,10 @@ ath10k_wmi_op_gen_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
 		return ERR_PTR(-ENOMEM);
 
 	cmd = (struct wmi_request_stats_cmd *)skb->data;
-	cmd->stats_id = __cpu_to_le32(stats_id);
+	cmd->stats_id = __cpu_to_le32(stats_mask);
 
-	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi request stats %d\n", (int)stats_id);
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi request stats 0x%08x\n",
+		   stats_mask);
 	return skb;
 }
 
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index 20ce3603e64b..adf935bf0580 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -3057,8 +3057,12 @@ struct wmi_pdev_stats_peer {
 } __packed;
 
 enum wmi_stats_id {
-	WMI_REQUEST_PEER_STAT	= 0x01,
-	WMI_REQUEST_AP_STAT	= 0x02
+	WMI_STAT_PEER = BIT(0),
+	WMI_STAT_AP = BIT(1),
+	WMI_STAT_PDEV = BIT(2),
+	WMI_STAT_VDEV = BIT(3),
+	WMI_STAT_BCNFLT = BIT(4),
+	WMI_STAT_VDEV_RATE = BIT(5),
 };
 
 struct wlan_inst_rssi_args {
@@ -3093,7 +3097,7 @@ struct wmi_pdev_suspend_cmd {
 } __packed;
 
 struct wmi_stats_event {
-	__le32 stats_id; /* %WMI_REQUEST_ */
+	__le32 stats_id; /* WMI_STAT_ */
 	/*
 	 * number of pdev stats event structures
 	 * (wmi_pdev_stats) 0 or 1
@@ -3745,6 +3749,11 @@ enum wmi_10x_vdev_param {
 	WMI_10X_VDEV_PARAM_VHT80_RATEMASK,
 };
 
+#define WMI_VDEV_PARAM_TXBF_SU_TX_BFEE BIT(0)
+#define WMI_VDEV_PARAM_TXBF_MU_TX_BFEE BIT(1)
+#define WMI_VDEV_PARAM_TXBF_SU_TX_BFER BIT(2)
+#define WMI_VDEV_PARAM_TXBF_MU_TX_BFER BIT(3)
+
 /* slot time long */
 #define WMI_VDEV_SLOT_TIME_LONG		0x1
 /* slot time short */
@@ -4436,7 +4445,8 @@ enum wmi_peer_param {
 	WMI_PEER_AUTHORIZE  = 0x3,
 	WMI_PEER_CHAN_WIDTH = 0x4,
 	WMI_PEER_NSS        = 0x5,
-	WMI_PEER_USE_4ADDR  = 0x6
+	WMI_PEER_USE_4ADDR  = 0x6,
+	WMI_PEER_DUMMY_VAR  = 0xff, /* dummy parameter for STA PS workaround */
 };
 
 struct wmi_peer_set_param_cmd {
diff --git a/drivers/net/wireless/ath/ath5k/ath5k.h b/drivers/net/wireless/ath/ath5k/ath5k.h
index 1ed7a88aeea9..7ca0d6f930fd 100644
--- a/drivers/net/wireless/ath/ath5k/ath5k.h
+++ b/drivers/net/wireless/ath/ath5k/ath5k.h
@@ -1283,6 +1283,7 @@ struct ath5k_hw {
 #define ATH_STAT_PROMISC	1
 #define ATH_STAT_LEDSOFT	2		/* enable LED gpio status */
 #define ATH_STAT_STARTED	3		/* opened & irqs enabled */
+#define ATH_STAT_RESET		4		/* hw reset */
 
 	unsigned int		filter_flags;	/* HW flags, AR5K_RX_FILTER_* */
 	unsigned int		fif_filter_flags; /* Current FIF_* filter flags */
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index bc9cb356fa69..a6131825c9f6 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -528,7 +528,7 @@ ath5k_update_bssid_mask_and_opmode(struct ath5k_hw *ah,
 	 * together with the BSSID mask when matching addresses.
 	 */
 	iter_data.hw_macaddr = common->macaddr;
-	memset(&iter_data.mask, 0xff, ETH_ALEN);
+	eth_broadcast_addr(iter_data.mask);
 	iter_data.found_active = false;
 	iter_data.need_set_hw_addr = true;
 	iter_data.opmode = NL80211_IFTYPE_UNSPECIFIED;
@@ -1523,6 +1523,9 @@ ath5k_set_current_imask(struct ath5k_hw *ah)
 	enum ath5k_int imask;
 	unsigned long flags;
 
+	if (test_bit(ATH_STAT_RESET, ah->status))
+		return;
+
 	spin_lock_irqsave(&ah->irqlock, flags);
 	imask = ah->imask;
 	if (ah->rx_pending)
@@ -2858,10 +2861,12 @@ ath5k_reset(struct ath5k_hw *ah, struct ieee80211_channel *chan,
 {
 	struct ath_common *common = ath5k_hw_common(ah);
 	int ret, ani_mode;
-	bool fast;
+	bool fast = chan && modparam_fastchanswitch ? 1 : 0;
 
 	ATH5K_DBG(ah, ATH5K_DEBUG_RESET, "resetting\n");
 
+	__set_bit(ATH_STAT_RESET, ah->status);
+
 	ath5k_hw_set_imr(ah, 0);
 	synchronize_irq(ah->irq);
 	ath5k_stop_tasklets(ah);
@@ -2876,11 +2881,29 @@ ath5k_reset(struct ath5k_hw *ah, struct ieee80211_channel *chan,
 	 * so we should also free any remaining
 	 * tx buffers */
 	ath5k_drain_tx_buffs(ah);
+
+	/* Stop PCU */
+	ath5k_hw_stop_rx_pcu(ah);
+
+	/* Stop DMA
+	 *
+	 * Note: If DMA didn't stop continue
+	 * since only a reset will fix it.
+	 */
+	ret = ath5k_hw_dma_stop(ah);
+
+	/* RF Bus grant won't work if we have pending
+	 * frames
+	 */
+	if (ret && fast) {
+		ATH5K_DBG(ah, ATH5K_DEBUG_RESET,
+			  "DMA didn't stop, falling back to normal reset\n");
+		fast = false;
+	}
+
 	if (chan)
 		ah->curchan = chan;
 
-	fast = ((chan != NULL) && modparam_fastchanswitch) ? 1 : 0;
-
 	ret = ath5k_hw_reset(ah, ah->opmode, ah->curchan, fast, skip_pcu);
 	if (ret) {
 		ATH5K_ERR(ah, "can't reset hardware (%d)\n", ret);
@@ -2934,6 +2957,8 @@ ath5k_reset(struct ath5k_hw *ah, struct ieee80211_channel *chan,
 	 */
 /*	ath5k_chan_change(ah, c); */
 
+	__clear_bit(ATH_STAT_RESET, ah->status);
+
 	ath5k_beacon_config(ah);
 	/* intrs are enabled by ath5k_beacon_config */
 
diff --git a/drivers/net/wireless/ath/ath5k/reset.c b/drivers/net/wireless/ath/ath5k/reset.c
index b9b651ea9851..99e62f99a182 100644
--- a/drivers/net/wireless/ath/ath5k/reset.c
+++ b/drivers/net/wireless/ath/ath5k/reset.c
@@ -1169,30 +1169,6 @@ ath5k_hw_reset(struct ath5k_hw *ah, enum nl80211_iftype op_mode,
 	if (ah->ah_version == AR5K_AR5212)
 		ath5k_hw_set_sleep_clock(ah, false);
 
-	/*
-	 * Stop PCU
-	 */
-	ath5k_hw_stop_rx_pcu(ah);
-
-	/*
-	 * Stop DMA
-	 *
-	 * Note: If DMA didn't stop continue
-	 * since only a reset will fix it.
-	 */
-	ret = ath5k_hw_dma_stop(ah);
-
-	/* RF Bus grant won't work if we have pending
-	 * frames */
-	if (ret && fast) {
-		ATH5K_DBG(ah, ATH5K_DEBUG_RESET,
-			"DMA didn't stop, falling back to normal reset\n");
-		fast = false;
-		/* Non fatal, just continue with
-		 * normal reset */
-		ret = 0;
-	}
-
 	mode = channel->hw_value;
 	switch (mode) {
 	case AR5K_MODE_11A:
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index 85da63a67faf..cce4625a53ad 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -686,20 +686,21 @@ ath6kl_add_bss_if_needed(struct ath6kl_vif *vif,
 {
 	struct ath6kl *ar = vif->ar;
 	struct cfg80211_bss *bss;
-	u16 cap_mask, cap_val;
+	u16 cap_val;
+	enum ieee80211_bss_type bss_type;
 	u8 *ie;
 
 	if (nw_type & ADHOC_NETWORK) {
-		cap_mask = WLAN_CAPABILITY_IBSS;
 		cap_val = WLAN_CAPABILITY_IBSS;
+		bss_type = IEEE80211_BSS_TYPE_IBSS;
 	} else {
-		cap_mask = WLAN_CAPABILITY_ESS;
 		cap_val = WLAN_CAPABILITY_ESS;
+		bss_type = IEEE80211_BSS_TYPE_ESS;
 	}
 
 	bss = cfg80211_get_bss(ar->wiphy, chan, bssid,
 			       vif->ssid, vif->ssid_len,
-			       cap_mask, cap_val);
+			       bss_type, IEEE80211_PRIVACY_ANY);
 	if (bss == NULL) {
 		/*
 		 * Since cfg80211 may not yet know about the BSS,
@@ -1495,6 +1496,7 @@ static int ath6kl_cfg80211_set_power_mgmt(struct wiphy *wiphy,
 
 static struct wireless_dev *ath6kl_cfg80211_add_iface(struct wiphy *wiphy,
 						      const char *name,
+						      unsigned char name_assign_type,
 						      enum nl80211_iftype type,
 						      u32 *flags,
 						      struct vif_params *params)
@@ -1513,7 +1515,7 @@ static struct wireless_dev *ath6kl_cfg80211_add_iface(struct wiphy *wiphy,
 		return ERR_PTR(-EINVAL);
 	}
 
-	wdev = ath6kl_interface_add(ar, name, type, if_idx, nw_type);
+	wdev = ath6kl_interface_add(ar, name, name_assign_type, type, if_idx, nw_type);
 	if (!wdev)
 		return ERR_PTR(-ENOMEM);
 
@@ -2033,7 +2035,7 @@ static int ath6kl_wow_sta(struct ath6kl *ar, struct ath6kl_vif *vif)
 	int ret;
 
 	/* Setup unicast pkt pattern */
-	memset(mac_mask, 0xff, ETH_ALEN);
+	eth_broadcast_addr(mac_mask);
 	ret = ath6kl_wmi_add_wow_pattern_cmd(ar->wmi,
 				vif->fw_vif_idx, WOW_LIST_ID,
 				ETH_ALEN, 0, ndev->dev_addr,
@@ -3633,13 +3635,14 @@ void ath6kl_cfg80211_vif_cleanup(struct ath6kl_vif *vif)
 }
 
 struct wireless_dev *ath6kl_interface_add(struct ath6kl *ar, const char *name,
+					  unsigned char name_assign_type,
 					  enum nl80211_iftype type,
 					  u8 fw_vif_idx, u8 nw_type)
 {
 	struct net_device *ndev;
 	struct ath6kl_vif *vif;
 
-	ndev = alloc_netdev(sizeof(*vif), name, NET_NAME_UNKNOWN, ether_setup);
+	ndev = alloc_netdev(sizeof(*vif), name, name_assign_type, ether_setup);
 	if (!ndev)
 		return NULL;
 
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.h b/drivers/net/wireless/ath/ath6kl/cfg80211.h
index b59becd91aea..5aa57a7639bf 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.h
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.h
@@ -25,6 +25,7 @@ enum ath6kl_cfg_suspend_mode {
 };
 
 struct wireless_dev *ath6kl_interface_add(struct ath6kl *ar, const char *name,
+					  unsigned char name_assign_type,
 					  enum nl80211_iftype type,
 					  u8 fw_vif_idx, u8 nw_type);
 void ath6kl_cfg80211_ch_switch_notify(struct ath6kl_vif *vif, int freq,
diff --git a/drivers/net/wireless/ath/ath6kl/core.c b/drivers/net/wireless/ath/ath6kl/core.c
index 0df74b245af4..4ec02cea0f43 100644
--- a/drivers/net/wireless/ath/ath6kl/core.c
+++ b/drivers/net/wireless/ath/ath6kl/core.c
@@ -211,8 +211,8 @@ int ath6kl_core_init(struct ath6kl *ar, enum ath6kl_htc_type htc_type)
 	rtnl_lock();
 
 	/* Add an initial station interface */
-	wdev = ath6kl_interface_add(ar, "wlan%d", NL80211_IFTYPE_STATION, 0,
-				    INFRA_NETWORK);
+	wdev = ath6kl_interface_add(ar, "wlan%d", NET_NAME_ENUM,
+				    NL80211_IFTYPE_STATION, 0, INFRA_NETWORK);
 
 	rtnl_unlock();
 
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index b42ba46b5030..1af3fed5a72c 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -105,7 +105,7 @@ static void ath6kl_sta_cleanup(struct ath6kl *ar, u8 i)
 
 	memset(&ar->ap_stats.sta[sta->aid - 1], 0,
 	       sizeof(struct wmi_per_sta_stat));
-	memset(sta->mac, 0, ETH_ALEN);
+	eth_zero_addr(sta->mac);
 	memset(sta->wpa_ie, 0, ATH6KL_MAX_IE);
 	sta->aid = 0;
 	sta->sta_flags = 0;
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index 473972288a84..ecda613c2d54 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -46,7 +46,8 @@ ath9k_hw-y:=	\
 ath9k_hw-$(CONFIG_ATH9K_WOW) += ar9003_wow.o
 
 ath9k_hw-$(CONFIG_ATH9K_BTCOEX_SUPPORT) += btcoex.o \
-					   ar9003_mci.o
+					   ar9003_mci.o \
+					   ar9003_aic.o
 
 ath9k_hw-$(CONFIG_ATH9K_PCOEM) += ar9003_rtt.o
 
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index ca01d17d130f..25e45e4d1a60 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -107,11 +107,21 @@ static const struct ani_cck_level_entry cck_level_table[] = {
 static void ath9k_hw_update_mibstats(struct ath_hw *ah,
 				     struct ath9k_mib_stats *stats)
 {
-	stats->ackrcv_bad += REG_READ(ah, AR_ACK_FAIL);
-	stats->rts_bad += REG_READ(ah, AR_RTS_FAIL);
-	stats->fcs_bad += REG_READ(ah, AR_FCS_FAIL);
-	stats->rts_good += REG_READ(ah, AR_RTS_OK);
-	stats->beacons += REG_READ(ah, AR_BEACON_CNT);
+	u32 addr[5] = {AR_RTS_OK, AR_RTS_FAIL, AR_ACK_FAIL,
+		       AR_FCS_FAIL, AR_BEACON_CNT};
+	u32 data[5];
+
+	REG_READ_MULTI(ah, &addr[0], &data[0], 5);
+	/* AR_RTS_OK */
+	stats->rts_good += data[0];
+	/* AR_RTS_FAIL */
+	stats->rts_bad += data[1];
+	/* AR_ACK_FAIL */
+	stats->ackrcv_bad += data[2];
+	/* AR_FCS_FAIL */
+	stats->fcs_bad += data[3];
+	/* AR_BEACON_CNT */
+	stats->beacons += data[4];
 }
 
 static void ath9k_ani_restart(struct ath_hw *ah)
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index f273427fdd29..6c23d279525f 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -681,12 +681,13 @@ static void ar5008_hw_set_channel_regs(struct ath_hw *ah,
 			phymode |= AR_PHY_FC_DYN2040_PRI_CH;
 
 	}
+	ENABLE_REGWRITE_BUFFER(ah);
 	REG_WRITE(ah, AR_PHY_TURBO, phymode);
 
+	/* This function do only REG_WRITE, so
+	 * we can include it to REGWRITE_BUFFER. */
 	ath9k_hw_set11nmac2040(ah, chan);
 
-	ENABLE_REGWRITE_BUFFER(ah);
-
 	REG_WRITE(ah, AR_GTXTO, 25 << AR_GTXTO_TIMEOUT_LIMIT_S);
 	REG_WRITE(ah, AR_CST, 0xF << AR_CST_TIMEOUT_LIMIT_S);
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_calib.c b/drivers/net/wireless/ath/ath9k/ar9002_calib.c
index 42190b67c671..50fcd343c41a 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_calib.c
@@ -430,46 +430,43 @@ static void ar9271_hw_pa_cal(struct ath_hw *ah, bool is_reset)
 	u32 regVal;
 	unsigned int i;
 	u32 regList[][2] = {
-		{ 0x786c, 0 },
-		{ 0x7854, 0 },
-		{ 0x7820, 0 },
-		{ 0x7824, 0 },
-		{ 0x7868, 0 },
-		{ 0x783c, 0 },
-		{ 0x7838, 0 } ,
-		{ 0x7828, 0 } ,
+		{ AR9285_AN_TOP3, 0 },
+		{ AR9285_AN_RXTXBB1, 0 },
+		{ AR9285_AN_RF2G1, 0 },
+		{ AR9285_AN_RF2G2, 0 },
+		{ AR9285_AN_TOP2, 0 },
+		{ AR9285_AN_RF2G8, 0 },
+		{ AR9285_AN_RF2G7, 0 },
+		{ AR9285_AN_RF2G3, 0 },
 	};
 
-	for (i = 0; i < ARRAY_SIZE(regList); i++)
-		regList[i][1] = REG_READ(ah, regList[i][0]);
-
-	regVal = REG_READ(ah, 0x7834);
-	regVal &= (~(0x1));
-	REG_WRITE(ah, 0x7834, regVal);
-	regVal = REG_READ(ah, 0x9808);
-	regVal |= (0x1 << 27);
-	REG_WRITE(ah, 0x9808, regVal);
+	REG_READ_ARRAY(ah, regList, ARRAY_SIZE(regList));
 
+	ENABLE_REG_RMW_BUFFER(ah);
+	/* 7834, b1=0 */
+	REG_CLR_BIT(ah, AR9285_AN_RF2G6, 1 << 0);
+	/* 9808, b27=1 */
+	REG_SET_BIT(ah, 0x9808, 1 << 27);
 	/* 786c,b23,1, pwddac=1 */
-	REG_RMW_FIELD(ah, AR9285_AN_TOP3, AR9285_AN_TOP3_PWDDAC, 1);
+	REG_SET_BIT(ah, AR9285_AN_TOP3, AR9285_AN_TOP3_PWDDAC);
 	/* 7854, b5,1, pdrxtxbb=1 */
-	REG_RMW_FIELD(ah, AR9285_AN_RXTXBB1, AR9285_AN_RXTXBB1_PDRXTXBB1, 1);
+	REG_SET_BIT(ah, AR9285_AN_RXTXBB1, AR9285_AN_RXTXBB1_PDRXTXBB1);
 	/* 7854, b7,1, pdv2i=1 */
-	REG_RMW_FIELD(ah, AR9285_AN_RXTXBB1, AR9285_AN_RXTXBB1_PDV2I, 1);
+	REG_SET_BIT(ah, AR9285_AN_RXTXBB1, AR9285_AN_RXTXBB1_PDV2I);
 	/* 7854, b8,1, pddacinterface=1 */
-	REG_RMW_FIELD(ah, AR9285_AN_RXTXBB1, AR9285_AN_RXTXBB1_PDDACIF, 1);
+	REG_SET_BIT(ah, AR9285_AN_RXTXBB1, AR9285_AN_RXTXBB1_PDDACIF);
 	/* 7824,b12,0, offcal=0 */
-	REG_RMW_FIELD(ah, AR9285_AN_RF2G2, AR9285_AN_RF2G2_OFFCAL, 0);
+	REG_CLR_BIT(ah, AR9285_AN_RF2G2, AR9285_AN_RF2G2_OFFCAL);
 	/* 7838, b1,0, pwddb=0 */
-	REG_RMW_FIELD(ah, AR9285_AN_RF2G7, AR9285_AN_RF2G7_PWDDB, 0);
+	REG_CLR_BIT(ah, AR9285_AN_RF2G7, AR9285_AN_RF2G7_PWDDB);
 	/* 7820,b11,0, enpacal=0 */
-	REG_RMW_FIELD(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_ENPACAL, 0);
+	REG_CLR_BIT(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_ENPACAL);
 	/* 7820,b25,1, pdpadrv1=0 */
-	REG_RMW_FIELD(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_PDPADRV1, 0);
+	REG_CLR_BIT(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_PDPADRV1);
 	/* 7820,b24,0, pdpadrv2=0 */
-	REG_RMW_FIELD(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_PDPADRV2, 0);
+	REG_CLR_BIT(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_PDPADRV2);
 	/* 7820,b23,0, pdpaout=0 */
-	REG_RMW_FIELD(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_PDPAOUT, 0);
+	REG_CLR_BIT(ah, AR9285_AN_RF2G1, AR9285_AN_RF2G1_PDPAOUT);
 	/* 783c,b14-16,7, padrvgn2tab_0=7 */
 	REG_RMW_FIELD(ah, AR9285_AN_RF2G8, AR9285_AN_RF2G8_PADRVGN2TAB0, 7);
 	/*
@@ -477,8 +474,9 @@ static void ar9271_hw_pa_cal(struct ath_hw *ah, bool is_reset)
 	 * does not matter since we turn it off
 	 */
 	REG_RMW_FIELD(ah, AR9285_AN_RF2G7, AR9285_AN_RF2G7_PADRVGN2TAB0, 0);
-
+	/* 7828, b0-11, ccom=fff */
 	REG_RMW_FIELD(ah, AR9285_AN_RF2G3, AR9271_AN_RF2G3_CCOMP, 0xfff);
+	REG_RMW_BUFFER_FLUSH(ah);
 
 	/* Set:
 	 * localmode=1,bmode=1,bmoderxtx=1,synthon=1,
@@ -490,15 +488,16 @@ static void ar9271_hw_pa_cal(struct ath_hw *ah, bool is_reset)
 
 	/* find off_6_1; */
 	for (i = 6; i > 0; i--) {
-		regVal = REG_READ(ah, 0x7834);
+		regVal = REG_READ(ah, AR9285_AN_RF2G6);
 		regVal |= (1 << (20 + i));
-		REG_WRITE(ah, 0x7834, regVal);
+		REG_WRITE(ah, AR9285_AN_RF2G6, regVal);
 		udelay(1);
 		/* regVal = REG_READ(ah, 0x7834); */
 		regVal &= (~(0x1 << (20 + i)));
-		regVal |= (MS(REG_READ(ah, 0x7840), AR9285_AN_RXTXBB1_SPARE9)
+		regVal |= (MS(REG_READ(ah, AR9285_AN_RF2G9),
+			      AR9285_AN_RXTXBB1_SPARE9)
 			    << (20 + i));
-		REG_WRITE(ah, 0x7834, regVal);
+		REG_WRITE(ah, AR9285_AN_RF2G6, regVal);
 	}
 
 	regVal = (regVal >> 20) & 0x7f;
@@ -515,15 +514,15 @@ static void ar9271_hw_pa_cal(struct ath_hw *ah, bool is_reset)
 		ah->pacal_info.prev_offset = regVal;
 	}
 
-	ENABLE_REGWRITE_BUFFER(ah);
 
-	regVal = REG_READ(ah, 0x7834);
-	regVal |= 0x1;
-	REG_WRITE(ah, 0x7834, regVal);
-	regVal = REG_READ(ah, 0x9808);
-	regVal &= (~(0x1 << 27));
-	REG_WRITE(ah, 0x9808, regVal);
+	ENABLE_REG_RMW_BUFFER(ah);
+	/* 7834, b1=1 */
+	REG_SET_BIT(ah, AR9285_AN_RF2G6, 1 << 0);
+	/* 9808, b27=0 */
+	REG_CLR_BIT(ah, 0x9808, 1 << 27);
+	REG_RMW_BUFFER_FLUSH(ah);
 
+	ENABLE_REGWRITE_BUFFER(ah);
 	for (i = 0; i < ARRAY_SIZE(regList); i++)
 		REG_WRITE(ah, regList[i][0], regList[i][1]);
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_aic.c b/drivers/net/wireless/ath/ath9k/ar9003_aic.c
new file mode 100644
index 000000000000..1db119d77783
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/ar9003_aic.c
@@ -0,0 +1,599 @@
+/*
+ * Copyright (c) 2015 Qualcomm Atheros Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "hw.h"
+#include "hw-ops.h"
+#include "ar9003_mci.h"
+#include "ar9003_aic.h"
+#include "ar9003_phy.h"
+#include "reg_aic.h"
+
+static const u8 com_att_db_table[ATH_AIC_MAX_COM_ATT_DB_TABLE] = {
+	0, 3, 9, 15, 21, 27
+};
+
+static const u16 aic_lin_table[ATH_AIC_MAX_AIC_LIN_TABLE] = {
+	8191, 7300, 6506, 5799, 5168, 4606, 4105, 3659,
+	3261, 2906, 2590, 2309, 2057, 1834, 1634, 1457,
+	1298, 1157, 1031, 919,	819,  730,  651,  580,
+	517,  461,  411,  366,	326,  291,  259,  231,
+	206,  183,  163,  146,	130,  116,  103,  92,
+	82,   73,   65,	  58,	52,   46,   41,	  37,
+	33,   29,   26,	  23,	21,   18,   16,	  15,
+	13,   12,   10,	  9,	8,    7,    7,	  6,
+	5,    5,    4,	  4,	3
+};
+
+static bool ar9003_hw_is_aic_enabled(struct ath_hw *ah)
+{
+	struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
+
+	/*
+	 * Disable AIC for now, until we have all the
+	 * HW code and the driver-layer support ready.
+	 */
+	return false;
+
+	if (mci_hw->config & ATH_MCI_CONFIG_DISABLE_AIC)
+		return false;
+
+	return true;
+}
+
+static int16_t ar9003_aic_find_valid(struct ath_aic_sram_info *cal_sram,
+				     bool dir, u8 index)
+{
+	int16_t i;
+
+	if (dir) {
+		for (i = index + 1; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+			if (cal_sram[i].valid)
+				break;
+		}
+	} else {
+		for (i = index - 1; i >= 0; i--) {
+			if (cal_sram[i].valid)
+				break;
+		}
+	}
+
+	if ((i >= ATH_AIC_MAX_BT_CHANNEL) || (i < 0))
+		i = -1;
+
+	return i;
+}
+
+/*
+ * type 0: aic_lin_table, 1: com_att_db_table
+ */
+static int16_t ar9003_aic_find_index(u8 type, int16_t value)
+{
+	int16_t i = -1;
+
+	if (type == 0) {
+		for (i = ATH_AIC_MAX_AIC_LIN_TABLE - 1; i >= 0; i--) {
+			if (aic_lin_table[i] >= value)
+				break;
+		}
+	} else if (type == 1) {
+		for (i = 0; i < ATH_AIC_MAX_COM_ATT_DB_TABLE; i++) {
+			if (com_att_db_table[i] > value) {
+				i--;
+				break;
+			}
+		}
+
+		if (i >= ATH_AIC_MAX_COM_ATT_DB_TABLE)
+			i = -1;
+	}
+
+	return i;
+}
+
+static void ar9003_aic_gain_table(struct ath_hw *ah)
+{
+	u32 aic_atten_word[19], i;
+
+	/* Config LNA gain difference */
+	REG_WRITE(ah, AR_PHY_BT_COEX_4, 0x2c200a00);
+	REG_WRITE(ah, AR_PHY_BT_COEX_5, 0x5c4e4438);
+
+	/* Program gain table */
+	aic_atten_word[0] = (0x1 & 0xf) << 14 | (0x1f & 0x1f) << 9 | (0x0 & 0xf) << 5 |
+		(0x1f & 0x1f); /* -01 dB: 4'd1, 5'd31,  00 dB: 4'd0, 5'd31 */
+	aic_atten_word[1] = (0x3 & 0xf) << 14 | (0x1f & 0x1f) << 9 | (0x2 & 0xf) << 5 |
+		(0x1f & 0x1f); /* -03 dB: 4'd3, 5'd31, -02 dB: 4'd2, 5'd31 */
+	aic_atten_word[2] = (0x5 & 0xf) << 14 | (0x1f & 0x1f) << 9 | (0x4 & 0xf) << 5 |
+		(0x1f & 0x1f); /* -05 dB: 4'd5, 5'd31, -04 dB: 4'd4, 5'd31 */
+	aic_atten_word[3] = (0x1 & 0xf) << 14 | (0x1e & 0x1f) << 9 | (0x0 & 0xf) << 5 |
+		(0x1e & 0x1f); /* -07 dB: 4'd1, 5'd30, -06 dB: 4'd0, 5'd30 */
+	aic_atten_word[4] = (0x3 & 0xf) << 14 | (0x1e & 0x1f) << 9 | (0x2 & 0xf) << 5 |
+		(0x1e & 0x1f); /* -09 dB: 4'd3, 5'd30, -08 dB: 4'd2, 5'd30 */
+	aic_atten_word[5] = (0x5 & 0xf) << 14 | (0x1e & 0x1f) << 9 | (0x4 & 0xf) << 5 |
+		(0x1e & 0x1f); /* -11 dB: 4'd5, 5'd30, -10 dB: 4'd4, 5'd30 */
+	aic_atten_word[6] = (0x1 & 0xf) << 14 | (0xf & 0x1f) << 9  | (0x0 & 0xf) << 5 |
+		(0xf & 0x1f);  /* -13 dB: 4'd1, 5'd15, -12 dB: 4'd0, 5'd15 */
+	aic_atten_word[7] = (0x3 & 0xf) << 14 | (0xf & 0x1f) << 9  | (0x2 & 0xf) << 5 |
+		(0xf & 0x1f);  /* -15 dB: 4'd3, 5'd15, -14 dB: 4'd2, 5'd15 */
+	aic_atten_word[8] = (0x5 & 0xf) << 14 | (0xf & 0x1f) << 9  | (0x4 & 0xf) << 5 |
+		(0xf & 0x1f);  /* -17 dB: 4'd5, 5'd15, -16 dB: 4'd4, 5'd15 */
+	aic_atten_word[9] = (0x1 & 0xf) << 14 | (0x7 & 0x1f) << 9  | (0x0 & 0xf) << 5 |
+		(0x7 & 0x1f);  /* -19 dB: 4'd1, 5'd07, -18 dB: 4'd0, 5'd07 */
+	aic_atten_word[10] = (0x3 & 0xf) << 14 | (0x7 & 0x1f) << 9  | (0x2 & 0xf) << 5 |
+		(0x7 & 0x1f);  /* -21 dB: 4'd3, 5'd07, -20 dB: 4'd2, 5'd07 */
+	aic_atten_word[11] = (0x5 & 0xf) << 14 | (0x7 & 0x1f) << 9  | (0x4 & 0xf) << 5 |
+		(0x7 & 0x1f);  /* -23 dB: 4'd5, 5'd07, -22 dB: 4'd4, 5'd07 */
+	aic_atten_word[12] = (0x7 & 0xf) << 14 | (0x7 & 0x1f) << 9  | (0x6 & 0xf) << 5 |
+		(0x7 & 0x1f);  /* -25 dB: 4'd7, 5'd07, -24 dB: 4'd6, 5'd07 */
+	aic_atten_word[13] = (0x3 & 0xf) << 14 | (0x3 & 0x1f) << 9  | (0x2 & 0xf) << 5 |
+		(0x3 & 0x1f);  /* -27 dB: 4'd3, 5'd03, -26 dB: 4'd2, 5'd03 */
+	aic_atten_word[14] = (0x5 & 0xf) << 14 | (0x3 & 0x1f) << 9  | (0x4 & 0xf) << 5 |
+		(0x3 & 0x1f);  /* -29 dB: 4'd5, 5'd03, -28 dB: 4'd4, 5'd03 */
+	aic_atten_word[15] = (0x1 & 0xf) << 14 | (0x1 & 0x1f) << 9  | (0x0 & 0xf) << 5 |
+		(0x1 & 0x1f);  /* -31 dB: 4'd1, 5'd01, -30 dB: 4'd0, 5'd01 */
+	aic_atten_word[16] = (0x3 & 0xf) << 14 | (0x1 & 0x1f) << 9  | (0x2 & 0xf) << 5 |
+		(0x1 & 0x1f);  /* -33 dB: 4'd3, 5'd01, -32 dB: 4'd2, 5'd01 */
+	aic_atten_word[17] = (0x5 & 0xf) << 14 | (0x1 & 0x1f) << 9  | (0x4 & 0xf) << 5 |
+		(0x1 & 0x1f);  /* -35 dB: 4'd5, 5'd01, -34 dB: 4'd4, 5'd01 */
+	aic_atten_word[18] = (0x7 & 0xf) << 14 | (0x1 & 0x1f) << 9  | (0x6 & 0xf) << 5 |
+		(0x1 & 0x1f);  /* -37 dB: 4'd7, 5'd01, -36 dB: 4'd6, 5'd01 */
+
+	/* Write to Gain table with auto increment enabled. */
+	REG_WRITE(ah, (AR_PHY_AIC_SRAM_ADDR_B0 + 0x3000),
+		  (ATH_AIC_SRAM_AUTO_INCREMENT |
+		   ATH_AIC_SRAM_GAIN_TABLE_OFFSET));
+
+	for (i = 0; i < 19; i++) {
+		REG_WRITE(ah, (AR_PHY_AIC_SRAM_DATA_B0 + 0x3000),
+			  aic_atten_word[i]);
+	}
+}
+
+static u8 ar9003_aic_cal_start(struct ath_hw *ah, u8 min_valid_count)
+{
+	struct ath9k_hw_aic *aic = &ah->btcoex_hw.aic;
+	int i;
+
+	/* Write to Gain table with auto increment enabled. */
+	REG_WRITE(ah, (AR_PHY_AIC_SRAM_ADDR_B0 + 0x3000),
+		  (ATH_AIC_SRAM_AUTO_INCREMENT |
+		   ATH_AIC_SRAM_CAL_OFFSET));
+
+	for (i = 0; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+		REG_WRITE(ah, (AR_PHY_AIC_SRAM_DATA_B0 + 0x3000), 0);
+		aic->aic_sram[i] = 0;
+	}
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_0_B0,
+		  (SM(0, AR_PHY_AIC_MON_ENABLE) |
+		   SM(127, AR_PHY_AIC_CAL_MAX_HOP_COUNT) |
+		   SM(min_valid_count, AR_PHY_AIC_CAL_MIN_VALID_COUNT) |
+		   SM(37, AR_PHY_AIC_F_WLAN) |
+		   SM(1, AR_PHY_AIC_CAL_CH_VALID_RESET) |
+		   SM(0, AR_PHY_AIC_CAL_ENABLE) |
+		   SM(0x40, AR_PHY_AIC_BTTX_PWR_THR) |
+		   SM(0, AR_PHY_AIC_ENABLE)));
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_0_B1,
+		  (SM(0, AR_PHY_AIC_MON_ENABLE) |
+		   SM(1, AR_PHY_AIC_CAL_CH_VALID_RESET) |
+		   SM(0, AR_PHY_AIC_CAL_ENABLE) |
+		   SM(0x40, AR_PHY_AIC_BTTX_PWR_THR) |
+		   SM(0, AR_PHY_AIC_ENABLE)));
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_1_B0,
+		  (SM(8, AR_PHY_AIC_CAL_BT_REF_DELAY) |
+		   SM(0, AR_PHY_AIC_BT_IDLE_CFG) |
+		   SM(1, AR_PHY_AIC_STDBY_COND) |
+		   SM(37, AR_PHY_AIC_STDBY_ROT_ATT_DB) |
+		   SM(5, AR_PHY_AIC_STDBY_COM_ATT_DB) |
+		   SM(15, AR_PHY_AIC_RSSI_MAX) |
+		   SM(0, AR_PHY_AIC_RSSI_MIN)));
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_1_B1,
+		  (SM(15, AR_PHY_AIC_RSSI_MAX) |
+		   SM(0, AR_PHY_AIC_RSSI_MIN)));
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_2_B0,
+		  (SM(44, AR_PHY_AIC_RADIO_DELAY) |
+		   SM(8, AR_PHY_AIC_CAL_STEP_SIZE_CORR) |
+		   SM(12, AR_PHY_AIC_CAL_ROT_IDX_CORR) |
+		   SM(2, AR_PHY_AIC_CAL_CONV_CHECK_FACTOR) |
+		   SM(5, AR_PHY_AIC_ROT_IDX_COUNT_MAX) |
+		   SM(0, AR_PHY_AIC_CAL_SYNTH_TOGGLE) |
+		   SM(0, AR_PHY_AIC_CAL_SYNTH_AFTER_BTRX) |
+		   SM(200, AR_PHY_AIC_CAL_SYNTH_SETTLING)));
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_3_B0,
+		  (SM(2, AR_PHY_AIC_MON_MAX_HOP_COUNT) |
+		   SM(1, AR_PHY_AIC_MON_MIN_STALE_COUNT) |
+		   SM(1, AR_PHY_AIC_MON_PWR_EST_LONG) |
+		   SM(2, AR_PHY_AIC_MON_PD_TALLY_SCALING) |
+		   SM(10, AR_PHY_AIC_MON_PERF_THR) |
+		   SM(2, AR_PHY_AIC_CAL_TARGET_MAG_SETTING) |
+		   SM(1, AR_PHY_AIC_CAL_PERF_CHECK_FACTOR) |
+		   SM(1, AR_PHY_AIC_CAL_PWR_EST_LONG)));
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_4_B0,
+		  (SM(2, AR_PHY_AIC_CAL_ROT_ATT_DB_EST_ISO) |
+		   SM(3, AR_PHY_AIC_CAL_COM_ATT_DB_EST_ISO) |
+		   SM(0, AR_PHY_AIC_CAL_ISO_EST_INIT_SETTING) |
+		   SM(2, AR_PHY_AIC_CAL_COM_ATT_DB_BACKOFF) |
+		   SM(1, AR_PHY_AIC_CAL_COM_ATT_DB_FIXED)));
+
+	REG_WRITE(ah, AR_PHY_AIC_CTRL_4_B1,
+		  (SM(2, AR_PHY_AIC_CAL_ROT_ATT_DB_EST_ISO) |
+		   SM(3, AR_PHY_AIC_CAL_COM_ATT_DB_EST_ISO) |
+		   SM(0, AR_PHY_AIC_CAL_ISO_EST_INIT_SETTING) |
+		   SM(2, AR_PHY_AIC_CAL_COM_ATT_DB_BACKOFF) |
+		   SM(1, AR_PHY_AIC_CAL_COM_ATT_DB_FIXED)));
+
+	ar9003_aic_gain_table(ah);
+
+	/* Need to enable AIC reference signal in BT modem. */
+	REG_WRITE(ah, ATH_AIC_BT_JUPITER_CTRL,
+		  (REG_READ(ah, ATH_AIC_BT_JUPITER_CTRL) |
+		   ATH_AIC_BT_AIC_ENABLE));
+
+	aic->aic_cal_start_time = REG_READ(ah, AR_TSF_L32);
+
+	/* Start calibration */
+	REG_CLR_BIT(ah, AR_PHY_AIC_CTRL_0_B1, AR_PHY_AIC_CAL_ENABLE);
+	REG_SET_BIT(ah, AR_PHY_AIC_CTRL_0_B1, AR_PHY_AIC_CAL_CH_VALID_RESET);
+	REG_SET_BIT(ah, AR_PHY_AIC_CTRL_0_B1, AR_PHY_AIC_CAL_ENABLE);
+
+	aic->aic_caled_chan = 0;
+	aic->aic_cal_state = AIC_CAL_STATE_STARTED;
+
+	return aic->aic_cal_state;
+}
+
+static bool ar9003_aic_cal_post_process(struct ath_hw *ah)
+{
+	struct ath9k_hw_aic *aic = &ah->btcoex_hw.aic;
+	struct ath_aic_sram_info cal_sram[ATH_AIC_MAX_BT_CHANNEL];
+	struct ath_aic_out_info aic_sram[ATH_AIC_MAX_BT_CHANNEL];
+	u32 dir_path_gain_idx, quad_path_gain_idx, value;
+	u32 fixed_com_att_db;
+	int8_t dir_path_sign, quad_path_sign;
+	int16_t i;
+	bool ret = true;
+
+	memset(&cal_sram, 0, sizeof(cal_sram));
+	memset(&aic_sram, 0, sizeof(aic_sram));
+
+	for (i = 0; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+		value = aic->aic_sram[i];
+
+		cal_sram[i].valid =
+			MS(value, AR_PHY_AIC_SRAM_VALID);
+		cal_sram[i].rot_quad_att_db =
+			MS(value, AR_PHY_AIC_SRAM_ROT_QUAD_ATT_DB);
+		cal_sram[i].vga_quad_sign =
+			MS(value, AR_PHY_AIC_SRAM_VGA_QUAD_SIGN);
+		cal_sram[i].rot_dir_att_db =
+			MS(value, AR_PHY_AIC_SRAM_ROT_DIR_ATT_DB);
+		cal_sram[i].vga_dir_sign =
+			MS(value, AR_PHY_AIC_SRAM_VGA_DIR_SIGN);
+		cal_sram[i].com_att_6db =
+			MS(value, AR_PHY_AIC_SRAM_COM_ATT_6DB);
+
+		if (cal_sram[i].valid) {
+			dir_path_gain_idx = cal_sram[i].rot_dir_att_db +
+				com_att_db_table[cal_sram[i].com_att_6db];
+			quad_path_gain_idx = cal_sram[i].rot_quad_att_db +
+				com_att_db_table[cal_sram[i].com_att_6db];
+
+			dir_path_sign = (cal_sram[i].vga_dir_sign) ? 1 : -1;
+			quad_path_sign = (cal_sram[i].vga_quad_sign) ? 1 : -1;
+
+			aic_sram[i].dir_path_gain_lin = dir_path_sign *
+				aic_lin_table[dir_path_gain_idx];
+			aic_sram[i].quad_path_gain_lin = quad_path_sign *
+				aic_lin_table[quad_path_gain_idx];
+		}
+	}
+
+	for (i = 0; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+		int16_t start_idx, end_idx;
+
+		if (cal_sram[i].valid)
+			continue;
+
+		start_idx = ar9003_aic_find_valid(cal_sram, 0, i);
+		end_idx = ar9003_aic_find_valid(cal_sram, 1, i);
+
+		if (start_idx < 0) {
+			/* extrapolation */
+			start_idx = end_idx;
+			end_idx = ar9003_aic_find_valid(cal_sram, 1, start_idx);
+
+			if (end_idx < 0) {
+				ret = false;
+				break;
+			}
+
+			aic_sram[i].dir_path_gain_lin =
+				((aic_sram[start_idx].dir_path_gain_lin -
+				  aic_sram[end_idx].dir_path_gain_lin) *
+				 (start_idx - i) + ((end_idx - i) >> 1)) /
+				(end_idx - i) +
+				aic_sram[start_idx].dir_path_gain_lin;
+			aic_sram[i].quad_path_gain_lin =
+				((aic_sram[start_idx].quad_path_gain_lin -
+				  aic_sram[end_idx].quad_path_gain_lin) *
+				 (start_idx - i) + ((end_idx - i) >> 1)) /
+				(end_idx - i) +
+				aic_sram[start_idx].quad_path_gain_lin;
+		}
+
+		if (end_idx < 0) {
+			/* extrapolation */
+			end_idx = ar9003_aic_find_valid(cal_sram, 0, start_idx);
+
+			if (end_idx < 0) {
+				ret = false;
+				break;
+			}
+
+			aic_sram[i].dir_path_gain_lin =
+				((aic_sram[start_idx].dir_path_gain_lin -
+				  aic_sram[end_idx].dir_path_gain_lin) *
+				 (i - start_idx) + ((start_idx - end_idx) >> 1)) /
+				(start_idx - end_idx) +
+				aic_sram[start_idx].dir_path_gain_lin;
+			aic_sram[i].quad_path_gain_lin =
+				((aic_sram[start_idx].quad_path_gain_lin -
+				  aic_sram[end_idx].quad_path_gain_lin) *
+				 (i - start_idx) + ((start_idx - end_idx) >> 1)) /
+				(start_idx - end_idx) +
+				aic_sram[start_idx].quad_path_gain_lin;
+
+		} else if (start_idx >= 0){
+			/* interpolation */
+			aic_sram[i].dir_path_gain_lin =
+				(((end_idx - i) * aic_sram[start_idx].dir_path_gain_lin) +
+				 ((i - start_idx) * aic_sram[end_idx].dir_path_gain_lin) +
+				 ((end_idx - start_idx) >> 1)) /
+				(end_idx - start_idx);
+			aic_sram[i].quad_path_gain_lin =
+				(((end_idx - i) * aic_sram[start_idx].quad_path_gain_lin) +
+				 ((i - start_idx) * aic_sram[end_idx].quad_path_gain_lin) +
+				 ((end_idx - start_idx) >> 1))/
+				(end_idx - start_idx);
+		}
+	}
+
+	/* From dir/quad_path_gain_lin to sram. */
+	i = ar9003_aic_find_valid(cal_sram, 1, 0);
+	if (i < 0) {
+		i = 0;
+		ret = false;
+	}
+	fixed_com_att_db = com_att_db_table[cal_sram[i].com_att_6db];
+
+	for (i = 0; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+		int16_t rot_dir_path_att_db, rot_quad_path_att_db;
+
+		aic_sram[i].sram.vga_dir_sign =
+			(aic_sram[i].dir_path_gain_lin >= 0) ? 1 : 0;
+		aic_sram[i].sram.vga_quad_sign=
+			(aic_sram[i].quad_path_gain_lin >= 0) ? 1 : 0;
+
+		rot_dir_path_att_db =
+			ar9003_aic_find_index(0, abs(aic_sram[i].dir_path_gain_lin)) -
+			fixed_com_att_db;
+		rot_quad_path_att_db =
+			ar9003_aic_find_index(0, abs(aic_sram[i].quad_path_gain_lin)) -
+			fixed_com_att_db;
+
+		aic_sram[i].sram.com_att_6db =
+			ar9003_aic_find_index(1, fixed_com_att_db);
+
+		aic_sram[i].sram.valid = 1;
+
+		aic_sram[i].sram.rot_dir_att_db =
+			min(max(rot_dir_path_att_db,
+				(int16_t)ATH_AIC_MIN_ROT_DIR_ATT_DB),
+			    ATH_AIC_MAX_ROT_DIR_ATT_DB);
+		aic_sram[i].sram.rot_quad_att_db =
+			min(max(rot_quad_path_att_db,
+				(int16_t)ATH_AIC_MIN_ROT_QUAD_ATT_DB),
+			    ATH_AIC_MAX_ROT_QUAD_ATT_DB);
+	}
+
+	for (i = 0; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+		aic->aic_sram[i] = (SM(aic_sram[i].sram.vga_dir_sign,
+				       AR_PHY_AIC_SRAM_VGA_DIR_SIGN) |
+				    SM(aic_sram[i].sram.vga_quad_sign,
+				       AR_PHY_AIC_SRAM_VGA_QUAD_SIGN) |
+				    SM(aic_sram[i].sram.com_att_6db,
+				       AR_PHY_AIC_SRAM_COM_ATT_6DB) |
+				    SM(aic_sram[i].sram.valid,
+				       AR_PHY_AIC_SRAM_VALID) |
+				    SM(aic_sram[i].sram.rot_dir_att_db,
+				       AR_PHY_AIC_SRAM_ROT_DIR_ATT_DB) |
+				    SM(aic_sram[i].sram.rot_quad_att_db,
+				       AR_PHY_AIC_SRAM_ROT_QUAD_ATT_DB));
+	}
+
+	return ret;
+}
+
+static void ar9003_aic_cal_done(struct ath_hw *ah)
+{
+	struct ath9k_hw_aic *aic = &ah->btcoex_hw.aic;
+
+	/* Disable AIC reference signal in BT modem. */
+	REG_WRITE(ah, ATH_AIC_BT_JUPITER_CTRL,
+		  (REG_READ(ah, ATH_AIC_BT_JUPITER_CTRL) &
+		   ~ATH_AIC_BT_AIC_ENABLE));
+
+	if (ar9003_aic_cal_post_process(ah))
+		aic->aic_cal_state = AIC_CAL_STATE_DONE;
+	else
+		aic->aic_cal_state = AIC_CAL_STATE_ERROR;
+}
+
+static u8 ar9003_aic_cal_continue(struct ath_hw *ah, bool cal_once)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
+	struct ath9k_hw_aic *aic = &ah->btcoex_hw.aic;
+	int i, num_chan;
+
+	num_chan = MS(mci_hw->config, ATH_MCI_CONFIG_AIC_CAL_NUM_CHAN);
+
+	if (!num_chan) {
+		aic->aic_cal_state = AIC_CAL_STATE_ERROR;
+		return aic->aic_cal_state;
+	}
+
+	if (cal_once) {
+		for (i = 0; i < 10000; i++) {
+			if ((REG_READ(ah, AR_PHY_AIC_CTRL_0_B1) &
+			     AR_PHY_AIC_CAL_ENABLE) == 0)
+				break;
+
+			udelay(100);
+		}
+	}
+
+	/*
+	 * Use AR_PHY_AIC_CAL_ENABLE bit instead of AR_PHY_AIC_CAL_DONE.
+	 * Sometimes CAL_DONE bit is not asserted.
+	 */
+	if ((REG_READ(ah, AR_PHY_AIC_CTRL_0_B1) &
+	     AR_PHY_AIC_CAL_ENABLE) != 0) {
+		ath_dbg(common, MCI, "AIC cal is not done after 40ms");
+		goto exit;
+	}
+
+	REG_WRITE(ah, AR_PHY_AIC_SRAM_ADDR_B1,
+		  (ATH_AIC_SRAM_CAL_OFFSET | ATH_AIC_SRAM_AUTO_INCREMENT));
+
+	for (i = 0; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+		u32 value;
+
+		value = REG_READ(ah, AR_PHY_AIC_SRAM_DATA_B1);
+
+		if (value & 0x01) {
+			if (aic->aic_sram[i] == 0)
+				aic->aic_caled_chan++;
+
+			aic->aic_sram[i] = value;
+
+			if (!cal_once)
+				break;
+		}
+	}
+
+	if ((aic->aic_caled_chan >= num_chan) || cal_once) {
+		ar9003_aic_cal_done(ah);
+	} else {
+		/* Start calibration */
+		REG_CLR_BIT(ah, AR_PHY_AIC_CTRL_0_B1, AR_PHY_AIC_CAL_ENABLE);
+		REG_SET_BIT(ah, AR_PHY_AIC_CTRL_0_B1,
+			    AR_PHY_AIC_CAL_CH_VALID_RESET);
+		REG_SET_BIT(ah, AR_PHY_AIC_CTRL_0_B1, AR_PHY_AIC_CAL_ENABLE);
+	}
+exit:
+	return aic->aic_cal_state;
+
+}
+
+u8 ar9003_aic_calibration(struct ath_hw *ah)
+{
+	struct ath9k_hw_aic *aic = &ah->btcoex_hw.aic;
+	u8 cal_ret = AIC_CAL_STATE_ERROR;
+
+	switch (aic->aic_cal_state) {
+	case AIC_CAL_STATE_IDLE:
+		cal_ret = ar9003_aic_cal_start(ah, 1);
+		break;
+	case AIC_CAL_STATE_STARTED:
+		cal_ret = ar9003_aic_cal_continue(ah, false);
+		break;
+	case AIC_CAL_STATE_DONE:
+		cal_ret = AIC_CAL_STATE_DONE;
+		break;
+	default:
+		break;
+	}
+
+	return cal_ret;
+}
+
+u8 ar9003_aic_start_normal(struct ath_hw *ah)
+{
+	struct ath9k_hw_aic *aic = &ah->btcoex_hw.aic;
+	int16_t i;
+
+	if (aic->aic_cal_state != AIC_CAL_STATE_DONE)
+		return 1;
+
+	ar9003_aic_gain_table(ah);
+
+	REG_WRITE(ah, AR_PHY_AIC_SRAM_ADDR_B1, ATH_AIC_SRAM_AUTO_INCREMENT);
+
+	for (i = 0; i < ATH_AIC_MAX_BT_CHANNEL; i++) {
+		REG_WRITE(ah, AR_PHY_AIC_SRAM_DATA_B1, aic->aic_sram[i]);
+	}
+
+	/* FIXME: Replace these with proper register names */
+	REG_WRITE(ah, 0xa6b0, 0x80);
+	REG_WRITE(ah, 0xa6b4, 0x5b2df0);
+	REG_WRITE(ah, 0xa6b8, 0x10762cc8);
+	REG_WRITE(ah, 0xa6bc, 0x1219a4b);
+	REG_WRITE(ah, 0xa6c0, 0x1e01);
+	REG_WRITE(ah, 0xb6b4, 0xf0);
+	REG_WRITE(ah, 0xb6c0, 0x1e01);
+	REG_WRITE(ah, 0xb6b0, 0x81);
+	REG_WRITE(ah, AR_PHY_65NM_CH1_RXTX4, 0x40000000);
+
+	aic->aic_enabled = true;
+
+	return 0;
+}
+
+u8 ar9003_aic_cal_reset(struct ath_hw *ah)
+{
+	struct ath9k_hw_aic *aic = &ah->btcoex_hw.aic;
+
+	aic->aic_cal_state = AIC_CAL_STATE_IDLE;
+	return aic->aic_cal_state;
+}
+
+u8 ar9003_aic_calibration_single(struct ath_hw *ah)
+{
+	struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
+	u8 cal_ret;
+	int num_chan;
+
+	num_chan = MS(mci_hw->config, ATH_MCI_CONFIG_AIC_CAL_NUM_CHAN);
+
+	(void) ar9003_aic_cal_start(ah, num_chan);
+	cal_ret = ar9003_aic_cal_continue(ah, true);
+
+	return cal_ret;
+}
+
+void ar9003_hw_attach_aic_ops(struct ath_hw *ah)
+{
+	struct ath_hw_private_ops *priv_ops = ath9k_hw_private_ops(ah);
+
+	priv_ops->is_aic_enabled = ar9003_hw_is_aic_enabled;
+}
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_aic.h b/drivers/net/wireless/ath/ath9k/ar9003_aic.h
new file mode 100644
index 000000000000..86f40644be43
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/ar9003_aic.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2015 Qualcomm Atheros Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef AR9003_AIC_H
+#define AR9003_AIC_H
+
+#define ATH_AIC_MAX_COM_ATT_DB_TABLE    6
+#define ATH_AIC_MAX_AIC_LIN_TABLE       69
+#define ATH_AIC_MIN_ROT_DIR_ATT_DB      0
+#define ATH_AIC_MIN_ROT_QUAD_ATT_DB     0
+#define ATH_AIC_MAX_ROT_DIR_ATT_DB      37
+#define ATH_AIC_MAX_ROT_QUAD_ATT_DB     37
+#define ATH_AIC_SRAM_AUTO_INCREMENT     0x80000000
+#define ATH_AIC_SRAM_GAIN_TABLE_OFFSET  0x280
+#define ATH_AIC_SRAM_CAL_OFFSET         0x140
+#define ATH_AIC_SRAM_OFFSET             0x00
+#define ATH_AIC_MEAS_MAG_THRESH         20
+#define ATH_AIC_BT_JUPITER_CTRL         0x66820
+#define ATH_AIC_BT_AIC_ENABLE           0x02
+
+enum aic_cal_state {
+	AIC_CAL_STATE_IDLE = 0,
+	AIC_CAL_STATE_STARTED,
+	AIC_CAL_STATE_DONE,
+	AIC_CAL_STATE_ERROR
+};
+
+struct ath_aic_sram_info {
+	bool valid:1;
+	bool vga_quad_sign:1;
+	bool vga_dir_sign:1;
+	u8 rot_quad_att_db;
+	u8 rot_dir_att_db;
+	u8 com_att_6db;
+};
+
+struct ath_aic_out_info {
+	int16_t dir_path_gain_lin;
+	int16_t quad_path_gain_lin;
+	struct ath_aic_sram_info sram;
+};
+
+u8 ar9003_aic_calibration(struct ath_hw *ah);
+u8 ar9003_aic_start_normal(struct ath_hw *ah);
+u8 ar9003_aic_cal_reset(struct ath_hw *ah);
+u8 ar9003_aic_calibration_single(struct ath_hw *ah);
+
+#endif /* AR9003_AIC_H */
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_hw.c b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
index 4335ccbe7d7e..79fd3b2dcbde 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
@@ -195,16 +195,16 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 		INIT_INI_ARRAY(&ah->iniCckfirJapan2484,
 			       ar9485_1_1_baseband_core_txfir_coeff_japan_2484);
 
-		if (ah->config.no_pll_pwrsave) {
+		if (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) {
 			INIT_INI_ARRAY(&ah->iniPcieSerdes,
-				       ar9485_1_1_pcie_phy_clkreq_disable_L1);
+				       ar9485_1_1_pll_on_cdr_on_clkreq_disable_L1);
 			INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-				       ar9485_1_1_pcie_phy_clkreq_disable_L1);
+				       ar9485_1_1_pll_on_cdr_on_clkreq_disable_L1);
 		} else {
 			INIT_INI_ARRAY(&ah->iniPcieSerdes,
-				       ar9485_1_1_pll_on_cdr_on_clkreq_disable_L1);
+				       ar9485_1_1_pcie_phy_clkreq_disable_L1);
 			INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-				       ar9485_1_1_pll_on_cdr_on_clkreq_disable_L1);
+				       ar9485_1_1_pcie_phy_clkreq_disable_L1);
 		}
 	} else if (AR_SREV_9462_21(ah)) {
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
@@ -231,10 +231,20 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 			       ar9462_2p1_modes_fast_clock);
 		INIT_INI_ARRAY(&ah->iniCckfirJapan2484,
 			       ar9462_2p1_baseband_core_txfir_coeff_japan_2484);
-		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-			       ar9462_2p1_pciephy_clkreq_disable_L1);
-		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-			       ar9462_2p1_pciephy_clkreq_disable_L1);
+
+		/* Awake -> Sleep Setting */
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D3)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdes,
+				       ar9462_2p1_pciephy_clkreq_disable_L1);
+		}
+
+		/* Sleep -> Awake Setting */
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D0)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
+				       ar9462_2p1_pciephy_clkreq_disable_L1);
+		}
 	} else if (AR_SREV_9462_20(ah)) {
 
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE], ar9462_2p0_mac_core);
@@ -262,11 +272,18 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 				ar9462_2p0_common_rx_gain);
 
 		/* Awake -> Sleep Setting */
-		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-			       ar9462_2p0_pciephy_clkreq_disable_L1);
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D3)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdes,
+				       ar9462_2p0_pciephy_clkreq_disable_L1);
+		}
+
 		/* Sleep -> Awake Setting */
-		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-			       ar9462_2p0_pciephy_clkreq_disable_L1);
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D0)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
+				       ar9462_2p0_pciephy_clkreq_disable_L1);
+		}
 
 		/* Fast clock modal settings */
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
@@ -456,10 +473,19 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			       ar9565_1p1_Modes_lowest_ob_db_tx_gain_table);
 
-		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-			       ar9565_1p1_pciephy_clkreq_disable_L1);
-		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-			       ar9565_1p1_pciephy_clkreq_disable_L1);
+		/* Awake -> Sleep Setting */
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D3)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdes,
+				       ar9565_1p1_pciephy_clkreq_disable_L1);
+		}
+
+		/* Sleep -> Awake Setting */
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D0)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
+				       ar9565_1p1_pciephy_clkreq_disable_L1);
+		}
 
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
 				ar9565_1p1_modes_fast_clock);
@@ -491,10 +517,19 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
 			       ar9565_1p0_Modes_lowest_ob_db_tx_gain_table);
 
-		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-			       ar9565_1p0_pciephy_clkreq_disable_L1);
-		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-			       ar9565_1p0_pciephy_clkreq_disable_L1);
+		/* Awake -> Sleep Setting */
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D3)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdes,
+				       ar9565_1p0_pciephy_clkreq_disable_L1);
+		}
+
+		/* Sleep -> Awake Setting */
+		if ((ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_CONTROL) &&
+		    (ah->config.pll_pwrsave & AR_PCIE_PLL_PWRSAVE_ON_D0)) {
+			INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
+				       ar9565_1p0_pciephy_clkreq_disable_L1);
+		}
 
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
 				ar9565_1p0_modes_fast_clock);
@@ -1130,6 +1165,12 @@ void ar9003_hw_attach_ops(struct ath_hw *ah)
 	struct ath_hw_ops *ops = ath9k_hw_ops(ah);
 
 	ar9003_hw_init_mode_regs(ah);
+
+	if (AR_SREV_9003_PCOEM(ah)) {
+		WARN_ON(!ah->iniPcieSerdes.ia_array);
+		WARN_ON(!ah->iniPcieSerdesLowPower.ia_array);
+	}
+
 	priv_ops->init_mode_gain_regs = ar9003_hw_init_mode_gain_regs;
 	priv_ops->init_hang_checks = ar9003_hw_init_hang_checks;
 	priv_ops->detect_mac_hang = ar9003_hw_detect_mac_hang;
@@ -1139,4 +1180,5 @@ void ar9003_hw_attach_ops(struct ath_hw *ah)
 	ar9003_hw_attach_phy_ops(ah);
 	ar9003_hw_attach_calib_ops(ah);
 	ar9003_hw_attach_mac_ops(ah);
+	ar9003_hw_attach_aic_ops(ah);
 }
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.c b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
index 7b94a6c7db3d..af5ee416a560 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
@@ -19,6 +19,7 @@
 #include "hw-ops.h"
 #include "ar9003_phy.h"
 #include "ar9003_mci.h"
+#include "ar9003_aic.h"
 
 static void ar9003_mci_reset_req_wakeup(struct ath_hw *ah)
 {
@@ -284,12 +285,12 @@ static void ar9003_mci_prep_interface(struct ath_hw *ah)
 		  AR_MCI_INTERRUPT_RX_MSG_CONT_RST);
 	REG_WRITE(ah, AR_MCI_INTERRUPT_RAW, AR_MCI_INTERRUPT_BT_PRI);
 
-	if (mci->is_2g) {
+	if (mci->is_2g && MCI_ANT_ARCH_PA_LNA_SHARED(mci)) {
 		ar9003_mci_send_lna_transfer(ah, true);
 		udelay(5);
 	}
 
-	if ((mci->is_2g && !mci->update_2g5g)) {
+	if (mci->is_2g && !mci->update_2g5g && MCI_ANT_ARCH_PA_LNA_SHARED(mci)) {
 		if (ar9003_mci_wait_for_interrupt(ah,
 					AR_MCI_INTERRUPT_RX_MSG_RAW,
 					AR_MCI_INTERRUPT_RX_MSG_LNA_INFO,
@@ -593,7 +594,7 @@ static u32 ar9003_mci_wait_for_gpm(struct ath_hw *ah, u8 gpm_type,
 		if (!time_out)
 			break;
 
-		offset = ar9003_mci_get_next_gpm_offset(ah, false, &more_data);
+		offset = ar9003_mci_get_next_gpm_offset(ah, &more_data);
 
 		if (offset == MCI_GPM_INVALID)
 			continue;
@@ -657,7 +658,7 @@ static u32 ar9003_mci_wait_for_gpm(struct ath_hw *ah, u8 gpm_type,
 		time_out = 0;
 
 	while (more_data == MCI_GPM_MORE) {
-		offset = ar9003_mci_get_next_gpm_offset(ah, false, &more_data);
+		offset = ar9003_mci_get_next_gpm_offset(ah, &more_data);
 		if (offset == MCI_GPM_INVALID)
 			break;
 
@@ -771,8 +772,14 @@ exit:
 
 static void ar9003_mci_mute_bt(struct ath_hw *ah)
 {
+	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+
 	/* disable all MCI messages */
 	REG_WRITE(ah, AR_MCI_MSG_ATTRIBUTES_TABLE, 0xffff0000);
+	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS0, 0xffffffff);
+	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS1, 0xffffffff);
+	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS2, 0xffffffff);
+	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS3, 0xffffffff);
 	REG_SET_BIT(ah, AR_MCI_TX_CTRL, AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
 
 	/* wait pending HW messages to flush out */
@@ -783,9 +790,10 @@ static void ar9003_mci_mute_bt(struct ath_hw *ah)
 	 * 1. reset not after resuming from full sleep
 	 * 2. before reset MCI RX, to quiet BT and avoid MCI RX misalignment
 	 */
-	ar9003_mci_send_lna_take(ah, true);
-
-	udelay(5);
+	if (MCI_ANT_ARCH_PA_LNA_SHARED(mci)) {
+		ar9003_mci_send_lna_take(ah, true);
+		udelay(5);
+	}
 
 	ar9003_mci_send_sys_sleeping(ah, true);
 }
@@ -821,6 +829,80 @@ static void ar9003_mci_osla_setup(struct ath_hw *ah, bool enable)
 		      AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN, 1);
 }
 
+static void ar9003_mci_stat_setup(struct ath_hw *ah)
+{
+	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+
+	if (!AR_SREV_9565(ah))
+		return;
+
+	if (mci->config & ATH_MCI_CONFIG_MCI_STAT_DBG) {
+		REG_RMW_FIELD(ah, AR_MCI_DBG_CNT_CTRL,
+			      AR_MCI_DBG_CNT_CTRL_ENABLE, 1);
+		REG_RMW_FIELD(ah, AR_MCI_DBG_CNT_CTRL,
+			      AR_MCI_DBG_CNT_CTRL_BT_LINKID,
+			      MCI_STAT_ALL_BT_LINKID);
+	} else {
+		REG_RMW_FIELD(ah, AR_MCI_DBG_CNT_CTRL,
+			      AR_MCI_DBG_CNT_CTRL_ENABLE, 0);
+	}
+}
+
+static void ar9003_mci_set_btcoex_ctrl_9565_1ANT(struct ath_hw *ah)
+{
+	u32 regval;
+
+	regval = SM(1, AR_BTCOEX_CTRL_AR9462_MODE) |
+		 SM(1, AR_BTCOEX_CTRL_WBTIMER_EN) |
+		 SM(1, AR_BTCOEX_CTRL_PA_SHARED) |
+		 SM(1, AR_BTCOEX_CTRL_LNA_SHARED) |
+		 SM(1, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
+		 SM(1, AR_BTCOEX_CTRL_RX_CHAIN_MASK) |
+		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_ACK) |
+		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_BCN) |
+		 SM(0, AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
+
+	REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2,
+		      AR_BTCOEX_CTRL2_TX_CHAIN_MASK, 0x1);
+	REG_WRITE(ah, AR_BTCOEX_CTRL, regval);
+}
+
+static void ar9003_mci_set_btcoex_ctrl_9565_2ANT(struct ath_hw *ah)
+{
+	u32 regval;
+
+	regval = SM(1, AR_BTCOEX_CTRL_AR9462_MODE) |
+		 SM(1, AR_BTCOEX_CTRL_WBTIMER_EN) |
+		 SM(0, AR_BTCOEX_CTRL_PA_SHARED) |
+		 SM(0, AR_BTCOEX_CTRL_LNA_SHARED) |
+		 SM(2, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
+		 SM(1, AR_BTCOEX_CTRL_RX_CHAIN_MASK) |
+		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_ACK) |
+		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_BCN) |
+		 SM(0, AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
+
+	REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2,
+		      AR_BTCOEX_CTRL2_TX_CHAIN_MASK, 0x0);
+	REG_WRITE(ah, AR_BTCOEX_CTRL, regval);
+}
+
+static void ar9003_mci_set_btcoex_ctrl_9462(struct ath_hw *ah)
+{
+	u32 regval;
+
+        regval = SM(1, AR_BTCOEX_CTRL_AR9462_MODE) |
+		 SM(1, AR_BTCOEX_CTRL_WBTIMER_EN) |
+		 SM(1, AR_BTCOEX_CTRL_PA_SHARED) |
+		 SM(1, AR_BTCOEX_CTRL_LNA_SHARED) |
+		 SM(2, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
+		 SM(3, AR_BTCOEX_CTRL_RX_CHAIN_MASK) |
+		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_ACK) |
+		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_BCN) |
+		 SM(0, AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
+
+	REG_WRITE(ah, AR_BTCOEX_CTRL, regval);
+}
+
 int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 		     bool is_full_sleep)
 {
@@ -831,11 +913,6 @@ int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 	ath_dbg(common, MCI, "MCI Reset (full_sleep = %d, is_2g = %d)\n",
 		is_full_sleep, is_2g);
 
-	if (!mci->gpm_addr && !mci->sched_addr) {
-		ath_err(common, "MCI GPM and schedule buffers are not allocated\n");
-		return -ENOMEM;
-	}
-
 	if (REG_READ(ah, AR_BTCOEX_CTRL) == 0xdeadbeef) {
 		ath_err(common, "BTCOEX control register is dead\n");
 		return -EINVAL;
@@ -850,26 +927,17 @@ int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 	* To avoid MCI state machine be affected by incoming remote MCI msgs,
 	* MCI mode will be enabled later, right before reset the MCI TX and RX.
 	*/
-
-	regval = SM(1, AR_BTCOEX_CTRL_AR9462_MODE) |
-		 SM(1, AR_BTCOEX_CTRL_WBTIMER_EN) |
-		 SM(1, AR_BTCOEX_CTRL_PA_SHARED) |
-		 SM(1, AR_BTCOEX_CTRL_LNA_SHARED) |
-		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_ACK) |
-		 SM(0, AR_BTCOEX_CTRL_1_CHAIN_BCN) |
-		 SM(0, AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
 	if (AR_SREV_9565(ah)) {
-		regval |= SM(1, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
-			  SM(1, AR_BTCOEX_CTRL_RX_CHAIN_MASK);
-		REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2,
-			      AR_BTCOEX_CTRL2_TX_CHAIN_MASK, 0x1);
+		u8 ant = MS(mci->config, ATH_MCI_CONFIG_ANT_ARCH);
+
+		if (ant == ATH_MCI_ANT_ARCH_1_ANT_PA_LNA_SHARED)
+			ar9003_mci_set_btcoex_ctrl_9565_1ANT(ah);
+		else
+			ar9003_mci_set_btcoex_ctrl_9565_2ANT(ah);
 	} else {
-		regval |= SM(2, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
-			  SM(3, AR_BTCOEX_CTRL_RX_CHAIN_MASK);
+		ar9003_mci_set_btcoex_ctrl_9462(ah);
 	}
 
-	REG_WRITE(ah, AR_BTCOEX_CTRL, regval);
-
 	if (is_2g && !(mci->config & ATH_MCI_CONFIG_DISABLE_OSLA))
 		ar9003_mci_osla_setup(ah, true);
 	else
@@ -926,26 +994,32 @@ int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 	regval &= ~SM(1, AR_MCI_COMMAND2_RESET_RX);
 	REG_WRITE(ah, AR_MCI_COMMAND2, regval);
 
-	ar9003_mci_get_next_gpm_offset(ah, true, NULL);
+	/* Init GPM offset after MCI Reset Rx */
+	ar9003_mci_state(ah, MCI_STATE_INIT_GPM_OFFSET);
 
 	REG_WRITE(ah, AR_MCI_MSG_ATTRIBUTES_TABLE,
 		  (SM(0xe801, AR_MCI_MSG_ATTRIBUTES_TABLE_INVALID_HDR) |
 		   SM(0x0000, AR_MCI_MSG_ATTRIBUTES_TABLE_CHECKSUM)));
 
-	REG_CLR_BIT(ah, AR_MCI_TX_CTRL,
-		    AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
+	if (MCI_ANT_ARCH_PA_LNA_SHARED(mci))
+		REG_CLR_BIT(ah, AR_MCI_TX_CTRL,
+			    AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
+	else
+		REG_SET_BIT(ah, AR_MCI_TX_CTRL,
+			    AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
 
 	ar9003_mci_observation_set_up(ah);
 
 	mci->ready = true;
 	ar9003_mci_prep_interface(ah);
+	ar9003_mci_stat_setup(ah);
 
-	if (AR_SREV_9565(ah))
-		REG_RMW_FIELD(ah, AR_MCI_DBG_CNT_CTRL,
-			      AR_MCI_DBG_CNT_CTRL_ENABLE, 0);
 	if (en_int)
 		ar9003_mci_enable_interrupt(ah);
 
+	if (ath9k_hw_is_aic_enabled(ah))
+		ar9003_aic_start_normal(ah);
+
 	return 0;
 }
 
@@ -1218,6 +1292,14 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type)
 		}
 		value &= AR_BTCOEX_CTRL_MCI_MODE_EN;
 		break;
+	case MCI_STATE_INIT_GPM_OFFSET:
+		value = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
+
+		if (value < mci->gpm_len)
+			mci->gpm_idx = value;
+		else
+			mci->gpm_idx = 0;
+		break;
 	case MCI_STATE_LAST_SCHD_MSG_OFFSET:
 		value = MS(REG_READ(ah, AR_MCI_RX_STATUS),
 				    AR_MCI_RX_LAST_SCHD_MSG_INDEX);
@@ -1284,6 +1366,22 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type)
 		value = (!mci->unhalt_bt_gpm && mci->need_flush_btinfo) ? 1 : 0;
 		mci->need_flush_btinfo = false;
 		break;
+	case MCI_STATE_AIC_CAL:
+		if (ath9k_hw_is_aic_enabled(ah))
+			value = ar9003_aic_calibration(ah);
+		break;
+	case MCI_STATE_AIC_START:
+		if (ath9k_hw_is_aic_enabled(ah))
+			ar9003_aic_start_normal(ah);
+		break;
+	case MCI_STATE_AIC_CAL_RESET:
+		if (ath9k_hw_is_aic_enabled(ah))
+			value = ar9003_aic_cal_reset(ah);
+		break;
+	case MCI_STATE_AIC_CAL_SINGLE:
+		if (ath9k_hw_is_aic_enabled(ah))
+			value = ar9003_aic_calibration_single(ah);
+		break;
 	default:
 		break;
 	}
@@ -1364,21 +1462,11 @@ void ar9003_mci_check_gpm_offset(struct ath_hw *ah)
 	mci->gpm_idx = 0;
 }
 
-u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, bool first, u32 *more)
+u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, u32 *more)
 {
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 	u32 offset, more_gpm = 0, gpm_ptr;
 
-	if (first) {
-		gpm_ptr = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
-
-		if (gpm_ptr >= mci->gpm_len)
-			gpm_ptr = 0;
-
-		mci->gpm_idx = gpm_ptr;
-		return gpm_ptr;
-	}
-
 	/*
 	 * This could be useful to avoid new GPM message interrupt which
 	 * may lead to spurious interrupt after power sleep, or multiple
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.h b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
index 66d7ab9f920d..e288611c12d5 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
@@ -92,14 +92,36 @@ enum mci_gpm_coex_bt_update_flags_op {
 #define ATH_MCI_CONFIG_CLK_DIV              0x00003000
 #define ATH_MCI_CONFIG_CLK_DIV_S            12
 #define ATH_MCI_CONFIG_DISABLE_TUNING       0x00004000
+#define ATH_MCI_CONFIG_DISABLE_AIC          0x00008000
+#define ATH_MCI_CONFIG_AIC_CAL_NUM_CHAN     0x007f0000
+#define ATH_MCI_CONFIG_AIC_CAL_NUM_CHAN_S   16
+#define ATH_MCI_CONFIG_NO_QUIET_ACK         0x00800000
+#define ATH_MCI_CONFIG_NO_QUIET_ACK_S       23
+#define ATH_MCI_CONFIG_ANT_ARCH             0x07000000
+#define ATH_MCI_CONFIG_ANT_ARCH_S           24
+#define ATH_MCI_CONFIG_FORCE_QUIET_ACK      0x08000000
+#define ATH_MCI_CONFIG_FORCE_QUIET_ACK_S    27
+#define ATH_MCI_CONFIG_FORCE_2CHAIN_ACK     0x10000000
+#define ATH_MCI_CONFIG_MCI_STAT_DBG         0x20000000
 #define ATH_MCI_CONFIG_MCI_WEIGHT_DBG       0x40000000
 #define ATH_MCI_CONFIG_DISABLE_MCI          0x80000000
 
 #define ATH_MCI_CONFIG_MCI_OBS_MASK     (ATH_MCI_CONFIG_MCI_OBS_MCI  | \
 					 ATH_MCI_CONFIG_MCI_OBS_TXRX | \
 					 ATH_MCI_CONFIG_MCI_OBS_BT)
+
 #define ATH_MCI_CONFIG_MCI_OBS_GPIO     0x0000002F
 
+#define ATH_MCI_ANT_ARCH_1_ANT_PA_LNA_NON_SHARED 0x00
+#define ATH_MCI_ANT_ARCH_1_ANT_PA_LNA_SHARED     0x01
+#define ATH_MCI_ANT_ARCH_2_ANT_PA_LNA_NON_SHARED 0x02
+#define ATH_MCI_ANT_ARCH_2_ANT_PA_LNA_SHARED     0x03
+#define ATH_MCI_ANT_ARCH_3_ANT                   0x04
+
+#define MCI_ANT_ARCH_PA_LNA_SHARED(mci)					\
+	((MS(mci->config, ATH_MCI_CONFIG_ANT_ARCH) == ATH_MCI_ANT_ARCH_1_ANT_PA_LNA_SHARED) || \
+	 (MS(mci->config, ATH_MCI_CONFIG_ANT_ARCH) == ATH_MCI_ANT_ARCH_2_ANT_PA_LNA_SHARED))
+
 enum mci_message_header {		/* length of payload */
 	MCI_LNA_CTRL     = 0x10,        /* len = 0 */
 	MCI_CONT_NACK    = 0x20,        /* len = 0 */
@@ -188,20 +210,55 @@ enum mci_bt_state {
 	MCI_BT_CAL
 };
 
+enum mci_ps_state {
+	MCI_PS_DISABLE,
+	MCI_PS_ENABLE,
+	MCI_PS_ENABLE_OFF,
+	MCI_PS_ENABLE_ON
+};
+
 /* Type of state query */
 enum mci_state_type {
 	MCI_STATE_ENABLE,
+	MCI_STATE_INIT_GPM_OFFSET,
+	MCI_STATE_CHECK_GPM_OFFSET,
+	MCI_STATE_NEXT_GPM_OFFSET,
+	MCI_STATE_LAST_GPM_OFFSET,
+	MCI_STATE_BT,
+	MCI_STATE_SET_BT_SLEEP,
 	MCI_STATE_SET_BT_AWAKE,
+	MCI_STATE_SET_BT_CAL_START,
+	MCI_STATE_SET_BT_CAL,
 	MCI_STATE_LAST_SCHD_MSG_OFFSET,
 	MCI_STATE_REMOTE_SLEEP,
+	MCI_STATE_CONT_STATUS,
 	MCI_STATE_RESET_REQ_WAKE,
 	MCI_STATE_SEND_WLAN_COEX_VERSION,
+	MCI_STATE_SET_BT_COEX_VERSION,
+	MCI_STATE_SEND_WLAN_CHANNELS,
 	MCI_STATE_SEND_VERSION_QUERY,
 	MCI_STATE_SEND_STATUS_QUERY,
+	MCI_STATE_NEED_FLUSH_BT_INFO,
+	MCI_STATE_SET_CONCUR_TX_PRI,
 	MCI_STATE_RECOVER_RX,
 	MCI_STATE_NEED_FTP_STOMP,
+	MCI_STATE_NEED_TUNING,
+	MCI_STATE_NEED_STAT_DEBUG,
+	MCI_STATE_SHARED_CHAIN_CONCUR_TX,
+	MCI_STATE_AIC_CAL,
+	MCI_STATE_AIC_START,
+	MCI_STATE_AIC_CAL_RESET,
+	MCI_STATE_AIC_CAL_SINGLE,
+	MCI_STATE_IS_AR9462,
+	MCI_STATE_IS_AR9565_1ANT,
+	MCI_STATE_IS_AR9565_2ANT,
+	MCI_STATE_WLAN_WEAK_SIGNAL,
+	MCI_STATE_SET_WLAN_PS_STATE,
+	MCI_STATE_GET_WLAN_PS_STATE,
 	MCI_STATE_DEBUG,
-	MCI_STATE_NEED_FLUSH_BT_INFO,
+	MCI_STATE_STAT_DEBUG,
+	MCI_STATE_ALLOW_FCS,
+	MCI_STATE_SET_2G_CONTENTION,
 	MCI_STATE_MAX
 };
 
@@ -255,7 +312,7 @@ int ar9003_mci_setup(struct ath_hw *ah, u32 gpm_addr, void *gpm_buf,
 void ar9003_mci_cleanup(struct ath_hw *ah);
 void ar9003_mci_get_interrupt(struct ath_hw *ah, u32 *raw_intr,
 			      u32 *rx_msg_intr);
-u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, bool first, u32 *more);
+u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, u32 *more);
 void ar9003_mci_set_bt_version(struct ath_hw *ah, u8 major, u8 minor);
 void ar9003_mci_send_wlan_channels(struct ath_hw *ah);
 /*
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.h b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
index c311b2bfdb00..fc595b92ac56 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
@@ -640,16 +640,6 @@
 #define AR_PHY_BB_THERM_ADC_4_LATEST_VOLT_VALUE		0x0000ff00
 #define AR_PHY_BB_THERM_ADC_4_LATEST_VOLT_VALUE_S	8
 
-/* AIC Registers */
-#define AR_PHY_AIC_CTRL_0_B0	(AR_SM_BASE + 0x4b0)
-#define AR_PHY_AIC_CTRL_1_B0	(AR_SM_BASE + 0x4b4)
-#define AR_PHY_AIC_CTRL_2_B0	(AR_SM_BASE + 0x4b8)
-#define AR_PHY_AIC_CTRL_3_B0	(AR_SM_BASE + 0x4bc)
-#define AR_PHY_AIC_STAT_0_B0	(AR_SM_BASE + 0x4c4))
-#define AR_PHY_AIC_STAT_1_B0	(AR_SM_BASE + 0x4c8))
-#define AR_PHY_AIC_CTRL_4_B0	(AR_SM_BASE + 0x4c0)
-#define AR_PHY_AIC_STAT_2_B0	(AR_SM_BASE + 0x4cc)
-
 #define AR_PHY_65NM_CH0_TXRF3       0x16048
 #define AR_PHY_65NM_CH0_TXRF3_CAPDIV2G		0x0000001e
 #define AR_PHY_65NM_CH0_TXRF3_CAPDIV2G_S	1
@@ -989,21 +979,6 @@
 #define AR_PHY_TX_IQCAL_STATUS_B1   (AR_SM1_BASE + 0x48c)
 #define AR_PHY_TX_IQCAL_CORR_COEFF_B1(_i)    (AR_SM1_BASE + 0x450 + ((_i) << 2))
 
-/* SM 1 AIC Registers */
-
-#define AR_PHY_AIC_CTRL_0_B1	(AR_SM1_BASE + 0x4b0)
-#define AR_PHY_AIC_CTRL_1_B1	(AR_SM1_BASE + 0x4b4)
-#define AR_PHY_AIC_CTRL_2_B1	(AR_SM1_BASE + 0x4b8)
-#define AR_PHY_AIC_STAT_0_B1	(AR_SM1_BASE + (AR_SREV_9462_10(ah) ? \
-					0x4c0 : 0x4c4))
-#define AR_PHY_AIC_STAT_1_B1	(AR_SM1_BASE + (AR_SREV_9462_10(ah) ? \
-					0x4c4 : 0x4c8))
-#define AR_PHY_AIC_CTRL_4_B1	(AR_SM1_BASE + 0x4c0)
-#define AR_PHY_AIC_STAT_2_B1	(AR_SM1_BASE + 0x4cc)
-
-#define AR_PHY_AIC_SRAM_ADDR_B1	(AR_SM1_BASE + 0x5f0)
-#define AR_PHY_AIC_SRAM_DATA_B1	(AR_SM1_BASE + 0x5f4)
-
 #define AR_PHY_RTT_TABLE_SW_INTF_B(i)	(0x384 + ((i) ? \
 					AR_SM1_BASE : AR_SM_BASE))
 #define AR_PHY_RTT_TABLE_SW_INTF_1_B(i)	(0x388 + ((i) ? \
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_rtt.c b/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
index 934418872e8e..e4d11fa7fe8c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_rtt.c
@@ -106,7 +106,7 @@ void ar9003_hw_rtt_load_hist(struct ath_hw *ah)
 	int chain, i;
 
 	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
-		if (!(ah->rxchainmask & (1 << chain)))
+		if (!(ah->caps.rx_chainmask & (1 << chain)))
 			continue;
 		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++) {
 			ar9003_hw_rtt_load_hist_entry(ah, chain, i,
@@ -171,7 +171,7 @@ void ar9003_hw_rtt_fill_hist(struct ath_hw *ah)
 	int chain, i;
 
 	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
-		if (!(ah->rxchainmask & (1 << chain)))
+		if (!(ah->caps.rx_chainmask & (1 << chain)))
 			continue;
 		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++) {
 			ah->caldata->rtt_table[chain][i] =
@@ -193,7 +193,7 @@ void ar9003_hw_rtt_clear_hist(struct ath_hw *ah)
 	int chain, i;
 
 	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
-		if (!(ah->rxchainmask & (1 << chain)))
+		if (!(ah->caps.rx_chainmask & (1 << chain)))
 			continue;
 		for (i = 0; i < MAX_RTT_TABLE_ENTRY; i++)
 			ar9003_hw_rtt_load_hist_entry(ah, chain, i, 0);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_wow.c b/drivers/net/wireless/ath/ath9k/ar9003_wow.c
index 86bfc9604dca..bea41df9fbd7 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_wow.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_wow.c
@@ -20,11 +20,25 @@
 #include "reg_wow.h"
 #include "hw-ops.h"
 
+static void ath9k_hw_set_sta_powersave(struct ath_hw *ah)
+{
+	if (!ath9k_hw_mci_is_enabled(ah))
+		goto set;
+	/*
+	 * If MCI is being used, set PWR_SAV only when MCI's
+	 * PS state is disabled.
+	 */
+	if (ar9003_mci_state(ah, MCI_STATE_GET_WLAN_PS_STATE) != MCI_PS_DISABLE)
+		return;
+set:
+	REG_SET_BIT(ah, AR_STA_ID1, AR_STA_ID1_PWR_SAV);
+}
+
 static void ath9k_hw_set_powermode_wow_sleep(struct ath_hw *ah)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
 
-	REG_SET_BIT(ah, AR_STA_ID1, AR_STA_ID1_PWR_SAV);
+	ath9k_hw_set_sta_powersave(ah);
 
 	/* set rx disable bit */
 	REG_WRITE(ah, AR_CR, AR_CR_RXD);
@@ -44,6 +58,9 @@ static void ath9k_hw_set_powermode_wow_sleep(struct ath_hw *ah)
 			REG_CLR_BIT(ah, AR_DIRECT_CONNECT, AR_DC_TSF2_ENABLE);
 	}
 
+	if (ath9k_hw_mci_is_enabled(ah))
+		REG_WRITE(ah, AR_RTC_KEEP_AWAKE, 0x2);
+
 	REG_WRITE(ah, AR_RTC_FORCE_WAKE, AR_RTC_FORCE_WAKE_ON_INT);
 }
 
@@ -74,8 +91,6 @@ static void ath9k_wow_create_keep_alive_pattern(struct ath_hw *ah)
 	for (i = 0; i < KAL_NUM_DESC_WORDS; i++)
 		REG_WRITE(ah, (AR_WOW_KA_DESC_WORD2 + i * 4), ctl[i]);
 
-	REG_WRITE(ah, (AR_WOW_KA_DESC_WORD2 + i * 4), ctl[i]);
-
 	data_word[0] = (KAL_FRAME_TYPE << 2) | (KAL_FRAME_SUB_TYPE << 4) |
 		       (KAL_TO_DS << 8) | (KAL_DURATION_ID << 16);
 	data_word[1] = (ap_mac_addr[3] << 24) | (ap_mac_addr[2] << 16) |
@@ -88,9 +103,11 @@ static void ath9k_wow_create_keep_alive_pattern(struct ath_hw *ah)
 		       (ap_mac_addr[1] << 8) | (ap_mac_addr[0]);
 	data_word[5] = (ap_mac_addr[5] << 8) | (ap_mac_addr[4]);
 
-	if (AR_SREV_9462_20(ah)) {
-		/* AR9462 2.0 has an extra descriptor word (time based
-		 * discard) compared to other chips */
+	if (AR_SREV_9462_20_OR_LATER(ah) || AR_SREV_9565(ah)) {
+		/*
+		 * AR9462 2.0 and AR9565 have an extra descriptor word
+		 * (time based discard) compared to other chips.
+		 */
 		REG_WRITE(ah, (AR_WOW_KA_DESC_WORD2 + (12 * 4)), 0);
 		wow_ka_data_word0 = AR_WOW_TXBUF(13);
 	} else {
@@ -99,7 +116,6 @@ static void ath9k_wow_create_keep_alive_pattern(struct ath_hw *ah)
 
 	for (i = 0; i < KAL_NUM_DATA_WORDS; i++)
 		REG_WRITE(ah, (wow_ka_data_word0 + i*4), data_word[i]);
-
 }
 
 int ath9k_hw_wow_apply_pattern(struct ath_hw *ah, u8 *user_pattern,
@@ -170,18 +186,17 @@ u32 ath9k_hw_wow_wakeup(struct ath_hw *ah)
 	u32 val = 0, rval;
 
 	/*
-	 * read the WoW status register to know
-	 * the wakeup reason
+	 * Read the WoW status register to know
+	 * the wakeup reason.
 	 */
 	rval = REG_READ(ah, AR_WOW_PATTERN);
 	val = AR_WOW_STATUS(rval);
 
 	/*
-	 * mask only the WoW events that we have enabled. Sometimes
+	 * Mask only the WoW events that we have enabled. Sometimes
 	 * we have spurious WoW events from the AR_WOW_PATTERN
 	 * register. This mask will clean it up.
 	 */
-
 	val &= ah->wow.wow_event_mask;
 
 	if (val) {
@@ -195,6 +210,15 @@ u32 ath9k_hw_wow_wakeup(struct ath_hw *ah)
 			wow_status |= AH_WOW_BEACON_MISS;
 	}
 
+	rval = REG_READ(ah, AR_MAC_PCU_WOW4);
+	val = AR_WOW_STATUS2(rval);
+	val &= ah->wow.wow_event_mask2;
+
+	if (val) {
+		if (AR_WOW2_PATTERN_FOUND(val))
+			wow_status |= AH_WOW_USER_PATTERN_EN;
+	}
+
 	/*
 	 * set and clear WOW_PME_CLEAR registers for the chip to
 	 * generate next wow signal.
@@ -206,10 +230,12 @@ u32 ath9k_hw_wow_wakeup(struct ath_hw *ah)
 		AR_PMCTRL_PWR_STATE_D1D3);
 
 	/*
-	 * clear all events
+	 * Clear all events.
 	 */
 	REG_WRITE(ah, AR_WOW_PATTERN,
 		  AR_WOW_CLEAR_EVENTS(REG_READ(ah, AR_WOW_PATTERN)));
+	REG_WRITE(ah, AR_MAC_PCU_WOW4,
+		  AR_WOW_CLEAR_EVENTS2(REG_READ(ah, AR_MAC_PCU_WOW4)));
 
 	/*
 	 * restore the beacon threshold to init value
@@ -226,7 +252,15 @@ u32 ath9k_hw_wow_wakeup(struct ath_hw *ah)
 	if (ah->is_pciexpress)
 		ath9k_hw_configpcipowersave(ah, false);
 
+	if (AR_SREV_9462(ah) || AR_SREV_9565(ah) || AR_SREV_9485(ah)) {
+		u32 dc = REG_READ(ah, AR_DIRECT_CONNECT);
+
+		if (!(dc & AR_DC_TSF2_ENABLE))
+			ath9k_hw_gen_timer_start_tsf2(ah);
+	}
+
 	ah->wow.wow_event_mask = 0;
+	ah->wow.wow_event_mask2 = 0;
 
 	return wow_status;
 }
@@ -408,6 +442,9 @@ void ath9k_hw_wow_enable(struct ath_hw *ah, u32 pattern_enable)
 
 	ath9k_hw_wow_set_arwr_reg(ah);
 
+	if (ath9k_hw_mci_is_enabled(ah))
+		REG_WRITE(ah, AR_RTC_KEEP_AWAKE, 0x2);
+
 	/* HW WoW */
 	REG_CLR_BIT(ah, AR_PCU_MISC_MODE3, BIT(5));
 
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 0f8e9464e4ab..a7a81b3969ce 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -184,12 +184,12 @@ struct ath_frame_info {
 	struct ath_buf *bf;
 	u16 framelen;
 	s8 txq;
-	enum ath9k_key_type keytype;
 	u8 keyix;
 	u8 rtscts_rate;
 	u8 retries : 7;
 	u8 baw_tracked : 1;
 	u8 tx_power;
+	enum ath9k_key_type keytype:2;
 };
 
 struct ath_rxbuf {
@@ -645,6 +645,7 @@ void ath9k_calculate_iter_data(struct ath_softc *sc,
 			       struct ath9k_vif_iter_data *iter_data);
 void ath9k_calculate_summary_state(struct ath_softc *sc,
 				   struct ath_chanctx *ctx);
+void ath9k_set_txpower(struct ath_softc *sc, struct ieee80211_vif *vif);
 
 /*******************/
 /* Beacon Handling */
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/btcoex.c b/drivers/net/wireless/ath/ath9k/btcoex.c
index 3dfc2c7f1f07..5a084d94ed90 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.c
+++ b/drivers/net/wireless/ath/ath9k/btcoex.c
@@ -103,7 +103,9 @@ void ath9k_hw_btcoex_init_scheme(struct ath_hw *ah)
 		return;
 	}
 
-	if (AR_SREV_9300_20_OR_LATER(ah)) {
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI) {
+		btcoex_hw->scheme = ATH_BTCOEX_CFG_MCI;
+	} else if (AR_SREV_9300_20_OR_LATER(ah)) {
 		btcoex_hw->scheme = ATH_BTCOEX_CFG_3WIRE;
 		btcoex_hw->btactive_gpio = ATH_BTACTIVE_GPIO_9300;
 		btcoex_hw->wlanactive_gpio = ATH_WLANACTIVE_GPIO_9300;
@@ -307,6 +309,18 @@ static void ath9k_hw_btcoex_enable_mci(struct ath_hw *ah)
 	btcoex->enabled = true;
 }
 
+static void ath9k_hw_btcoex_disable_mci(struct ath_hw *ah)
+{
+	struct ath_btcoex_hw *btcoex_hw = &ah->btcoex_hw;
+	int i;
+
+	ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_NONE);
+
+	for (i = 0; i < AR9300_NUM_BT_WEIGHTS; i++)
+		REG_WRITE(ah, AR_MCI_COEX_WL_WEIGHTS(i),
+			  btcoex_hw->wlan_weight[i]);
+}
+
 void ath9k_hw_btcoex_enable(struct ath_hw *ah)
 {
 	struct ath_btcoex_hw *btcoex_hw = &ah->btcoex_hw;
@@ -318,17 +332,18 @@ void ath9k_hw_btcoex_enable(struct ath_hw *ah)
 		ath9k_hw_btcoex_enable_2wire(ah);
 		break;
 	case ATH_BTCOEX_CFG_3WIRE:
-		if (AR_SREV_9462(ah) || AR_SREV_9565(ah)) {
-			ath9k_hw_btcoex_enable_mci(ah);
-			return;
-		}
 		ath9k_hw_btcoex_enable_3wire(ah);
 		break;
+	case ATH_BTCOEX_CFG_MCI:
+		ath9k_hw_btcoex_enable_mci(ah);
+		break;
 	}
 
-	REG_RMW(ah, AR_GPIO_PDPU,
-		(0x2 << (btcoex_hw->btactive_gpio * 2)),
-		(0x3 << (btcoex_hw->btactive_gpio * 2)));
+	if (ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_MCI) {
+		REG_RMW(ah, AR_GPIO_PDPU,
+			(0x2 << (btcoex_hw->btactive_gpio * 2)),
+			(0x3 << (btcoex_hw->btactive_gpio * 2)));
+	}
 
 	ah->btcoex_hw.enabled = true;
 }
@@ -340,14 +355,14 @@ void ath9k_hw_btcoex_disable(struct ath_hw *ah)
 	int i;
 
 	btcoex_hw->enabled = false;
-	if (AR_SREV_9462(ah) || AR_SREV_9565(ah)) {
-		ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_NONE);
-		for (i = 0; i < AR9300_NUM_BT_WEIGHTS; i++)
-			REG_WRITE(ah, AR_MCI_COEX_WL_WEIGHTS(i),
-				  btcoex_hw->wlan_weight[i]);
+
+	if (ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_MCI) {
+		ath9k_hw_btcoex_disable_mci(ah);
 		return;
 	}
-	ath9k_hw_set_gpio(ah, btcoex_hw->wlanactive_gpio, 0);
+
+	if (!AR_SREV_9300_20_OR_LATER(ah))
+		ath9k_hw_set_gpio(ah, btcoex_hw->wlanactive_gpio, 0);
 
 	ath9k_hw_cfg_output(ah, btcoex_hw->wlanactive_gpio,
 			AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.h b/drivers/net/wireless/ath/ath9k/btcoex.h
index 6de26ea5d5fa..cd2f0a2373cb 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.h
+++ b/drivers/net/wireless/ath/ath9k/btcoex.h
@@ -44,6 +44,9 @@
 
 #define AR9300_NUM_BT_WEIGHTS   4
 #define AR9300_NUM_WLAN_WEIGHTS 4
+
+#define ATH_AIC_MAX_BT_CHANNEL  79
+
 /* Defines the BT AR_BT_COEX_WGHT used */
 enum ath_stomp_type {
 	ATH_BTCOEX_STOMP_ALL,
@@ -58,6 +61,7 @@ enum ath_btcoex_scheme {
 	ATH_BTCOEX_CFG_NONE,
 	ATH_BTCOEX_CFG_2WIRE,
 	ATH_BTCOEX_CFG_3WIRE,
+	ATH_BTCOEX_CFG_MCI,
 };
 
 struct ath9k_hw_mci {
@@ -92,9 +96,18 @@ struct ath9k_hw_mci {
 	u32 last_recovery;
 };
 
+struct ath9k_hw_aic {
+	bool aic_enabled;
+	u8 aic_cal_state;
+	u8 aic_caled_chan;
+	u32 aic_sram[ATH_AIC_MAX_BT_CHANNEL];
+	u32 aic_cal_start_time;
+};
+
 struct ath_btcoex_hw {
 	enum ath_btcoex_scheme scheme;
 	struct ath9k_hw_mci mci;
+	struct ath9k_hw_aic aic;
 	bool enabled;
 	u8 wlanactive_gpio;
 	u8 btactive_gpio;
diff --git a/drivers/net/wireless/ath/ath9k/calib.c b/drivers/net/wireless/ath/ath9k/calib.c
index e200a6e3aca5..3e2e24e4843f 100644
--- a/drivers/net/wireless/ath/ath9k/calib.c
+++ b/drivers/net/wireless/ath/ath9k/calib.c
@@ -238,7 +238,6 @@ int ath9k_hw_loadnf(struct ath_hw *ah, struct ath9k_channel *chan)
 {
 	struct ath9k_nfcal_hist *h = NULL;
 	unsigned i, j;
-	int32_t val;
 	u8 chainmask = (ah->rxchainmask << 3) | ah->rxchainmask;
 	struct ath_common *common = ath9k_hw_common(ah);
 	s16 default_nf = ath9k_hw_get_default_nf(ah, chan);
@@ -246,6 +245,7 @@ int ath9k_hw_loadnf(struct ath_hw *ah, struct ath9k_channel *chan)
 	if (ah->caldata)
 		h = ah->caldata->nfCalHist;
 
+	ENABLE_REG_RMW_BUFFER(ah);
 	for (i = 0; i < NUM_NF_READINGS; i++) {
 		if (chainmask & (1 << i)) {
 			s16 nfval;
@@ -258,10 +258,8 @@ int ath9k_hw_loadnf(struct ath_hw *ah, struct ath9k_channel *chan)
 			else
 				nfval = default_nf;
 
-			val = REG_READ(ah, ah->nf_regs[i]);
-			val &= 0xFFFFFE00;
-			val |= (((u32) nfval << 1) & 0x1ff);
-			REG_WRITE(ah, ah->nf_regs[i], val);
+			REG_RMW(ah, ah->nf_regs[i],
+				(((u32) nfval << 1) & 0x1ff), 0x1ff);
 		}
 	}
 
@@ -274,6 +272,7 @@ int ath9k_hw_loadnf(struct ath_hw *ah, struct ath9k_channel *chan)
 	REG_CLR_BIT(ah, AR_PHY_AGC_CONTROL,
 		    AR_PHY_AGC_CONTROL_NO_UPDATE_NF);
 	REG_SET_BIT(ah, AR_PHY_AGC_CONTROL, AR_PHY_AGC_CONTROL_NF);
+	REG_RMW_BUFFER_FLUSH(ah);
 
 	/*
 	 * Wait for load to complete, should be fast, a few 10s of us.
@@ -309,19 +308,17 @@ int ath9k_hw_loadnf(struct ath_hw *ah, struct ath9k_channel *chan)
 	 * by the median we just loaded.  This will be initial (and max) value
 	 * of next noise floor calibration the baseband does.
 	 */
-	ENABLE_REGWRITE_BUFFER(ah);
+	ENABLE_REG_RMW_BUFFER(ah);
 	for (i = 0; i < NUM_NF_READINGS; i++) {
 		if (chainmask & (1 << i)) {
 			if ((i >= AR5416_MAX_CHAINS) && !IS_CHAN_HT40(chan))
 				continue;
 
-			val = REG_READ(ah, ah->nf_regs[i]);
-			val &= 0xFFFFFE00;
-			val |= (((u32) (-50) << 1) & 0x1ff);
-			REG_WRITE(ah, ah->nf_regs[i], val);
+			REG_RMW(ah, ah->nf_regs[i],
+					(((u32) (-50) << 1) & 0x1ff), 0x1ff);
 		}
 	}
-	REGWRITE_BUFFER_FLUSH(ah);
+	REG_RMW_BUFFER_FLUSH(ah);
 
 	return 0;
 }
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/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 50a2e0ac3b8b..dbf8f4959642 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -1156,7 +1156,10 @@ static ssize_t write_file_tpc(struct file *file, const char __user *user_buf,
 
 	if (tpc_enabled != ah->tpc_enabled) {
 		ah->tpc_enabled = tpc_enabled;
-		ath9k_hw_set_txpowerlimit(ah, sc->cur_chan->txpower, false);
+
+		mutex_lock(&sc->mutex);
+		ath9k_set_txpower(sc, NULL);
+		mutex_unlock(&sc->mutex);
 	}
 
 	return count;
diff --git a/drivers/net/wireless/ath/ath9k/dfs.c b/drivers/net/wireless/ath/ath9k/dfs.c
index 726271c7c330..e98a9eaba7ff 100644
--- a/drivers/net/wireless/ath/ath9k/dfs.c
+++ b/drivers/net/wireless/ath/ath9k/dfs.c
@@ -126,8 +126,19 @@ ath9k_postprocess_radar_event(struct ath_softc *sc,
 	DFS_STAT_INC(sc, pulses_detected);
 	return true;
 }
-#undef PRI_CH_RADAR_FOUND
-#undef EXT_CH_RADAR_FOUND
+
+static void
+ath9k_dfs_process_radar_pulse(struct ath_softc *sc, struct pulse_event *pe)
+{
+	struct dfs_pattern_detector *pd = sc->dfs_detector;
+	DFS_STAT_INC(sc, pulses_processed);
+	if (pd == NULL)
+		return;
+	if (!pd->add_pulse(pd, pe))
+		return;
+	DFS_STAT_INC(sc, radar_detected);
+	ieee80211_radar_detected(sc->hw);
+}
 
 /*
  * DFS: check PHY-error for radar pulse and feed the detector
@@ -176,18 +187,21 @@ void ath9k_dfs_process_phyerr(struct ath_softc *sc, void *data,
 	ard.pulse_length_pri = vdata_end[-3];
 	pe.freq = ah->curchan->channel;
 	pe.ts = mactime;
-	if (ath9k_postprocess_radar_event(sc, &ard, &pe)) {
-		struct dfs_pattern_detector *pd = sc->dfs_detector;
-		ath_dbg(common, DFS,
-			"ath9k_dfs_process_phyerr: channel=%d, ts=%llu, "
-			"width=%d, rssi=%d, delta_ts=%llu\n",
-			pe.freq, pe.ts, pe.width, pe.rssi,
-			pe.ts - sc->dfs_prev_pulse_ts);
-		sc->dfs_prev_pulse_ts = pe.ts;
-		DFS_STAT_INC(sc, pulses_processed);
-		if (pd != NULL && pd->add_pulse(pd, &pe)) {
-			DFS_STAT_INC(sc, radar_detected);
-			ieee80211_radar_detected(sc->hw);
-		}
+	if (!ath9k_postprocess_radar_event(sc, &ard, &pe))
+		return;
+
+	ath_dbg(common, DFS,
+		"ath9k_dfs_process_phyerr: type=%d, freq=%d, ts=%llu, "
+		"width=%d, rssi=%d, delta_ts=%llu\n",
+		ard.pulse_bw_info, pe.freq, pe.ts, pe.width, pe.rssi,
+		pe.ts - sc->dfs_prev_pulse_ts);
+	sc->dfs_prev_pulse_ts = pe.ts;
+	if (ard.pulse_bw_info & PRI_CH_RADAR_FOUND)
+		ath9k_dfs_process_radar_pulse(sc, &pe);
+	if (ard.pulse_bw_info & EXT_CH_RADAR_FOUND) {
+		pe.freq += IS_CHAN_HT40PLUS(ah->curchan) ? 20 : -20;
+		ath9k_dfs_process_radar_pulse(sc, &pe);
 	}
 }
+#undef PRI_CH_RADAR_FOUND
+#undef EXT_CH_RADAR_FOUND
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.c b/drivers/net/wireless/ath/ath9k/eeprom.c
index 971d770722cf..cc81482c934d 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom.c
@@ -27,12 +27,7 @@ void ath9k_hw_analog_shift_regwrite(struct ath_hw *ah, u32 reg, u32 val)
 void ath9k_hw_analog_shift_rmw(struct ath_hw *ah, u32 reg, u32 mask,
 			       u32 shift, u32 val)
 {
-	u32 regVal;
-
-	regVal = REG_READ(ah, reg) & ~mask;
-	regVal |= (val << shift) & mask;
-
-	REG_WRITE(ah, reg, regVal);
+	REG_RMW(ah, reg, ((val << shift) & mask), mask);
 
 	if (ah->config.analog_shiftreg)
 		udelay(100);
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_4k.c b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
index e5a78d4fd66e..4773da6dc6f2 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_4k.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
@@ -389,6 +389,7 @@ static void ath9k_hw_set_4k_power_cal_table(struct ath_hw *ah,
 		}
 	}
 
+	ENABLE_REG_RMW_BUFFER(ah);
 	REG_RMW_FIELD(ah, AR_PHY_TPCRG1, AR_PHY_TPCRG1_NUM_PD_GAIN,
 		      (numXpdGain - 1) & 0x3);
 	REG_RMW_FIELD(ah, AR_PHY_TPCRG1, AR_PHY_TPCRG1_PD_GAIN_1,
@@ -396,6 +397,7 @@ static void ath9k_hw_set_4k_power_cal_table(struct ath_hw *ah,
 	REG_RMW_FIELD(ah, AR_PHY_TPCRG1, AR_PHY_TPCRG1_PD_GAIN_2,
 		      xpdGainValues[1]);
 	REG_RMW_FIELD(ah, AR_PHY_TPCRG1, AR_PHY_TPCRG1_PD_GAIN_3, 0);
+	REG_RMW_BUFFER_FLUSH(ah);
 
 	for (i = 0; i < AR5416_EEP4K_MAX_CHAINS; i++) {
 		regChainOffset = i * 0x1000;
@@ -770,15 +772,14 @@ static void ath9k_hw_4k_set_gain(struct ath_hw *ah,
 				 struct ar5416_eeprom_4k *eep,
 				 u8 txRxAttenLocal)
 {
-	REG_WRITE(ah, AR_PHY_SWITCH_CHAIN_0,
-		  pModal->antCtrlChain[0]);
+	ENABLE_REG_RMW_BUFFER(ah);
+	REG_RMW(ah, AR_PHY_SWITCH_CHAIN_0,
+		pModal->antCtrlChain[0], 0);
 
-	REG_WRITE(ah, AR_PHY_TIMING_CTRL4(0),
-		  (REG_READ(ah, AR_PHY_TIMING_CTRL4(0)) &
-		   ~(AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF |
-		     AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF)) |
-		  SM(pModal->iqCalICh[0], AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF) |
-		  SM(pModal->iqCalQCh[0], AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF));
+	REG_RMW(ah, AR_PHY_TIMING_CTRL4(0),
+		SM(pModal->iqCalICh[0], AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF) |
+		SM(pModal->iqCalQCh[0], AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF),
+		AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF | AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF);
 
 	if ((eep->baseEepHeader.version & AR5416_EEP_VER_MINOR_MASK) >=
 	    AR5416_EEP_MINOR_VER_3) {
@@ -817,6 +818,7 @@ static void ath9k_hw_4k_set_gain(struct ath_hw *ah,
 		      AR9280_PHY_RXGAIN_TXRX_ATTEN, txRxAttenLocal);
 	REG_RMW_FIELD(ah, AR_PHY_RXGAIN + 0x1000,
 		      AR9280_PHY_RXGAIN_TXRX_MARGIN, pModal->rxTxMarginCh[0]);
+	REG_RMW_BUFFER_FLUSH(ah);
 }
 
 /*
@@ -928,6 +930,7 @@ static void ath9k_hw_4k_set_board_values(struct ath_hw *ah,
 		}
 	}
 
+	ENABLE_REG_RMW_BUFFER(ah);
 	if (AR_SREV_9271(ah)) {
 		ath9k_hw_analog_shift_rmw(ah,
 					  AR9285_AN_RF2G3,
@@ -1032,18 +1035,19 @@ static void ath9k_hw_4k_set_board_values(struct ath_hw *ah,
 					  AR9285_AN_RF2G4_DB2_4_S,
 					  db2[4]);
 	}
+	REG_RMW_BUFFER_FLUSH(ah);
 
-
+	ENABLE_REG_RMW_BUFFER(ah);
 	REG_RMW_FIELD(ah, AR_PHY_SETTLING, AR_PHY_SETTLING_SWITCH,
 		      pModal->switchSettling);
 	REG_RMW_FIELD(ah, AR_PHY_DESIRED_SZ, AR_PHY_DESIRED_SZ_ADC,
 		      pModal->adcDesiredSize);
 
-	REG_WRITE(ah, AR_PHY_RF_CTL4,
-		  SM(pModal->txEndToXpaOff, AR_PHY_RF_CTL4_TX_END_XPAA_OFF) |
-		  SM(pModal->txEndToXpaOff, AR_PHY_RF_CTL4_TX_END_XPAB_OFF) |
-		  SM(pModal->txFrameToXpaOn, AR_PHY_RF_CTL4_FRAME_XPAA_ON)  |
-		  SM(pModal->txFrameToXpaOn, AR_PHY_RF_CTL4_FRAME_XPAB_ON));
+	REG_RMW(ah, AR_PHY_RF_CTL4,
+		SM(pModal->txEndToXpaOff, AR_PHY_RF_CTL4_TX_END_XPAA_OFF) |
+		SM(pModal->txEndToXpaOff, AR_PHY_RF_CTL4_TX_END_XPAB_OFF) |
+		SM(pModal->txFrameToXpaOn, AR_PHY_RF_CTL4_FRAME_XPAA_ON)  |
+		SM(pModal->txFrameToXpaOn, AR_PHY_RF_CTL4_FRAME_XPAB_ON), 0);
 
 	REG_RMW_FIELD(ah, AR_PHY_RF_CTL3, AR_PHY_TX_END_TO_A2_RX_ON,
 		      pModal->txEndToRxOn);
@@ -1072,6 +1076,8 @@ static void ath9k_hw_4k_set_board_values(struct ath_hw *ah,
 				      pModal->swSettleHt40);
 	}
 
+	REG_RMW_BUFFER_FLUSH(ah);
+
 	bb_desired_scale = (pModal->bb_scale_smrt_antenna &
 			EEP_4K_BB_DESIRED_SCALE_MASK);
 	if ((pBase->txGainType == 0) && (bb_desired_scale != 0)) {
@@ -1080,6 +1086,7 @@ static void ath9k_hw_4k_set_board_values(struct ath_hw *ah,
 		mask = BIT(0)|BIT(5)|BIT(10)|BIT(15)|BIT(20)|BIT(25);
 		pwrctrl = mask * bb_desired_scale;
 		clr = mask * 0x1f;
+		ENABLE_REG_RMW_BUFFER(ah);
 		REG_RMW(ah, AR_PHY_TX_PWRCTRL8, pwrctrl, clr);
 		REG_RMW(ah, AR_PHY_TX_PWRCTRL10, pwrctrl, clr);
 		REG_RMW(ah, AR_PHY_CH0_TX_PWRCTRL12, pwrctrl, clr);
@@ -1094,6 +1101,7 @@ static void ath9k_hw_4k_set_board_values(struct ath_hw *ah,
 		clr = mask * 0x1f;
 		REG_RMW(ah, AR_PHY_CH0_TX_PWRCTRL11, pwrctrl, clr);
 		REG_RMW(ah, AR_PHY_CH0_TX_PWRCTRL13, pwrctrl, clr);
+		REG_RMW_BUFFER_FLUSH(ah);
 	}
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_def.c b/drivers/net/wireless/ath/ath9k/eeprom_def.c
index 098059039351..056f516bf017 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_def.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_def.c
@@ -466,6 +466,7 @@ static void ath9k_hw_def_set_gain(struct ath_hw *ah,
 				  struct ar5416_eeprom_def *eep,
 				  u8 txRxAttenLocal, int regChainOffset, int i)
 {
+	ENABLE_REG_RMW_BUFFER(ah);
 	if (AR5416_VER_MASK >= AR5416_EEP_MINOR_VER_3) {
 		txRxAttenLocal = pModal->txRxAttenCh[i];
 
@@ -483,16 +484,12 @@ static void ath9k_hw_def_set_gain(struct ath_hw *ah,
 			      AR_PHY_GAIN_2GHZ_XATTEN2_DB,
 			      pModal->xatten2Db[i]);
 		} else {
-			REG_WRITE(ah, AR_PHY_GAIN_2GHZ + regChainOffset,
-			  (REG_READ(ah, AR_PHY_GAIN_2GHZ + regChainOffset) &
-			   ~AR_PHY_GAIN_2GHZ_BSW_MARGIN)
-			  | SM(pModal-> bswMargin[i],
-			       AR_PHY_GAIN_2GHZ_BSW_MARGIN));
-			REG_WRITE(ah, AR_PHY_GAIN_2GHZ + regChainOffset,
-			  (REG_READ(ah, AR_PHY_GAIN_2GHZ + regChainOffset) &
-			   ~AR_PHY_GAIN_2GHZ_BSW_ATTEN)
-			  | SM(pModal->bswAtten[i],
-			       AR_PHY_GAIN_2GHZ_BSW_ATTEN));
+			REG_RMW(ah, AR_PHY_GAIN_2GHZ + regChainOffset,
+				SM(pModal-> bswMargin[i], AR_PHY_GAIN_2GHZ_BSW_MARGIN),
+				AR_PHY_GAIN_2GHZ_BSW_MARGIN);
+			REG_RMW(ah, AR_PHY_GAIN_2GHZ + regChainOffset,
+				SM(pModal->bswAtten[i], AR_PHY_GAIN_2GHZ_BSW_ATTEN),
+				AR_PHY_GAIN_2GHZ_BSW_ATTEN);
 		}
 	}
 
@@ -504,17 +501,14 @@ static void ath9k_hw_def_set_gain(struct ath_hw *ah,
 		      AR_PHY_RXGAIN + regChainOffset,
 		      AR9280_PHY_RXGAIN_TXRX_MARGIN, pModal->rxTxMarginCh[i]);
 	} else {
-		REG_WRITE(ah,
-			  AR_PHY_RXGAIN + regChainOffset,
-			  (REG_READ(ah, AR_PHY_RXGAIN + regChainOffset) &
-			   ~AR_PHY_RXGAIN_TXRX_ATTEN)
-			  | SM(txRxAttenLocal, AR_PHY_RXGAIN_TXRX_ATTEN));
-		REG_WRITE(ah,
-			  AR_PHY_GAIN_2GHZ + regChainOffset,
-			  (REG_READ(ah, AR_PHY_GAIN_2GHZ + regChainOffset) &
-			   ~AR_PHY_GAIN_2GHZ_RXTX_MARGIN) |
-			  SM(pModal->rxTxMarginCh[i], AR_PHY_GAIN_2GHZ_RXTX_MARGIN));
+		REG_RMW(ah, AR_PHY_RXGAIN + regChainOffset,
+			SM(txRxAttenLocal, AR_PHY_RXGAIN_TXRX_ATTEN),
+			AR_PHY_RXGAIN_TXRX_ATTEN);
+		REG_RMW(ah, AR_PHY_GAIN_2GHZ + regChainOffset,
+			SM(pModal->rxTxMarginCh[i], AR_PHY_GAIN_2GHZ_RXTX_MARGIN),
+			AR_PHY_GAIN_2GHZ_RXTX_MARGIN);
 	}
+	REG_RMW_BUFFER_FLUSH(ah);
 }
 
 static void ath9k_hw_def_set_board_values(struct ath_hw *ah,
diff --git a/drivers/net/wireless/ath/ath9k/gpio.c b/drivers/net/wireless/ath/ath9k/gpio.c
index da344b27326c..284706798c71 100644
--- a/drivers/net/wireless/ath/ath9k/gpio.c
+++ b/drivers/net/wireless/ath/ath9k/gpio.c
@@ -69,9 +69,15 @@ void ath_fill_led_pin(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
 
-	if (AR_SREV_9100(ah) || (ah->led_pin >= 0))
+	if (AR_SREV_9100(ah))
 		return;
 
+	if (ah->led_pin >= 0) {
+		if (!((1 << ah->led_pin) & AR_GPIO_OE_OUT_MASK))
+			ath9k_hw_request_gpio(ah, ah->led_pin, "ath9k-led");
+		return;
+	}
+
 	if (AR_SREV_9287(ah))
 		ah->led_pin = ATH_LED_PIN_9287;
 	else if (AR_SREV_9485(sc->sc_ah))
@@ -202,17 +208,16 @@ static void ath_btcoex_period_timer(unsigned long data)
 	}
 	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
-	ath9k_mci_update_rssi(sc);
-
 	ath9k_ps_wakeup(sc);
+	spin_lock_bh(&btcoex->btcoex_lock);
 
-	if (!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI))
-		ath_detect_bt_priority(sc);
-
-	if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI) {
+		ath9k_mci_update_rssi(sc);
 		ath_mci_ftp_adjust(sc);
+	}
 
-	spin_lock_bh(&btcoex->btcoex_lock);
+	if (!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI))
+		ath_detect_bt_priority(sc);
 
 	stomp_type = btcoex->bt_stomp_type;
 	timer_period = btcoex->btcoex_no_stomp;
@@ -252,9 +257,6 @@ static void ath_btcoex_no_stomp_timer(unsigned long arg)
 	struct ath_softc *sc = (struct ath_softc *)arg;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_btcoex *btcoex = &sc->btcoex;
-	struct ath_common *common = ath9k_hw_common(ah);
-
-	ath_dbg(common, BTCOEX, "no stomp timer running\n");
 
 	ath9k_ps_wakeup(sc);
 	spin_lock_bh(&btcoex->btcoex_lock);
@@ -271,7 +273,7 @@ static void ath_btcoex_no_stomp_timer(unsigned long arg)
 	ath9k_ps_restore(sc);
 }
 
-static int ath_init_btcoex_timer(struct ath_softc *sc)
+static void ath_init_btcoex_timer(struct ath_softc *sc)
 {
 	struct ath_btcoex *btcoex = &sc->btcoex;
 
@@ -280,6 +282,7 @@ static int ath_init_btcoex_timer(struct ath_softc *sc)
 		btcoex->btcoex_period / 100;
 	btcoex->btscan_no_stomp = (100 - ATH_BTCOEX_BTSCAN_DUTY_CYCLE) *
 				   btcoex->btcoex_period / 100;
+	btcoex->bt_stomp_type = ATH_BTCOEX_STOMP_LOW;
 
 	setup_timer(&btcoex->period_timer, ath_btcoex_period_timer,
 			(unsigned long) sc);
@@ -287,8 +290,6 @@ static int ath_init_btcoex_timer(struct ath_softc *sc)
 			(unsigned long) sc);
 
 	spin_lock_init(&btcoex->btcoex_lock);
-
-	return 0;
 }
 
 /*
@@ -299,6 +300,10 @@ void ath9k_btcoex_timer_resume(struct ath_softc *sc)
 	struct ath_btcoex *btcoex = &sc->btcoex;
 	struct ath_hw *ah = sc->sc_ah;
 
+	if (ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_3WIRE &&
+	    ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_MCI)
+		return;
+
 	ath_dbg(ath9k_hw_common(ah), BTCOEX, "Starting btcoex timers\n");
 
 	/* make sure duty cycle timer is also stopped when resuming */
@@ -312,13 +317,19 @@ void ath9k_btcoex_timer_resume(struct ath_softc *sc)
 	mod_timer(&btcoex->period_timer, jiffies);
 }
 
-
 /*
  * Pause btcoex timer and bt duty cycle timer
  */
 void ath9k_btcoex_timer_pause(struct ath_softc *sc)
 {
 	struct ath_btcoex *btcoex = &sc->btcoex;
+	struct ath_hw *ah = sc->sc_ah;
+
+	if (ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_3WIRE &&
+	    ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_MCI)
+		return;
+
+	ath_dbg(ath9k_hw_common(ah), BTCOEX, "Stopping btcoex timers\n");
 
 	del_timer_sync(&btcoex->period_timer);
 	del_timer_sync(&btcoex->no_stomp_timer);
@@ -356,33 +367,33 @@ void ath9k_start_btcoex(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
 
-	if ((ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_NONE) &&
-	    !ah->btcoex_hw.enabled) {
-		if (!(sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_MCI))
-			ath9k_hw_btcoex_set_weight(ah, AR_BT_COEX_WGHT,
-						   AR_STOMP_LOW_WLAN_WGHT, 0);
-		else
-			ath9k_hw_btcoex_set_weight(ah, 0, 0,
-						   ATH_BTCOEX_STOMP_NONE);
-		ath9k_hw_btcoex_enable(ah);
+	if (ah->btcoex_hw.enabled ||
+	    ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_NONE)
+		return;
 
-		if (ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_3WIRE)
-			ath9k_btcoex_timer_resume(sc);
-	}
+	if (!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI))
+		ath9k_hw_btcoex_set_weight(ah, AR_BT_COEX_WGHT,
+					   AR_STOMP_LOW_WLAN_WGHT, 0);
+	else
+		ath9k_hw_btcoex_set_weight(ah, 0, 0,
+					   ATH_BTCOEX_STOMP_NONE);
+	ath9k_hw_btcoex_enable(ah);
+	ath9k_btcoex_timer_resume(sc);
 }
 
 void ath9k_stop_btcoex(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
 
-	if (ah->btcoex_hw.enabled &&
-	    ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_NONE) {
-		if (ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_3WIRE)
-			ath9k_btcoex_timer_pause(sc);
-		ath9k_hw_btcoex_disable(ah);
-		if (AR_SREV_9462(ah) || AR_SREV_9565(ah))
-			ath_mci_flush_profile(&sc->btcoex.mci);
-	}
+	if (!ah->btcoex_hw.enabled ||
+	    ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_NONE)
+		return;
+
+	ath9k_btcoex_timer_pause(sc);
+	ath9k_hw_btcoex_disable(ah);
+
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
+		ath_mci_flush_profile(&sc->btcoex.mci);
 }
 
 void ath9k_deinit_btcoex(struct ath_softc *sc)
@@ -409,22 +420,20 @@ int ath9k_init_btcoex(struct ath_softc *sc)
 		break;
 	case ATH_BTCOEX_CFG_3WIRE:
 		ath9k_hw_btcoex_init_3wire(sc->sc_ah);
-		r = ath_init_btcoex_timer(sc);
-		if (r)
-			return -1;
+		ath_init_btcoex_timer(sc);
 		txq = sc->tx.txq_map[IEEE80211_AC_BE];
 		ath9k_hw_init_btcoex_hw(sc->sc_ah, txq->axq_qnum);
-		sc->btcoex.bt_stomp_type = ATH_BTCOEX_STOMP_LOW;
-		if (ath9k_hw_mci_is_enabled(ah)) {
-			sc->btcoex.duty_cycle = ATH_BTCOEX_DEF_DUTY_CYCLE;
-			INIT_LIST_HEAD(&sc->btcoex.mci.info);
+		break;
+	case ATH_BTCOEX_CFG_MCI:
+		ath_init_btcoex_timer(sc);
 
-			r = ath_mci_setup(sc);
-			if (r)
-				return r;
+		sc->btcoex.duty_cycle = ATH_BTCOEX_DEF_DUTY_CYCLE;
+		INIT_LIST_HEAD(&sc->btcoex.mci.info);
+		ath9k_hw_btcoex_init_mci(ah);
 
-			ath9k_hw_btcoex_init_mci(ah);
-		}
+		r = ath_mci_setup(sc);
+		if (r)
+			return r;
 
 		break;
 	default:
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index 8e7153b186ed..10c02f5cbc5e 100644
--- a/drivers/net/wireless/ath/ath9k/hif_usb.c
+++ b/drivers/net/wireless/ath/ath9k/hif_usb.c
@@ -40,6 +40,7 @@ static struct usb_device_id ath9k_hif_usb_ids[] = {
 	{ USB_DEVICE(0x0cf3, 0xb003) }, /* Ubiquiti WifiStation Ext */
 	{ USB_DEVICE(0x0cf3, 0xb002) }, /* Ubiquiti WifiStation */
 	{ USB_DEVICE(0x057c, 0x8403) }, /* AVM FRITZ!WLAN 11N v2 USB */
+	{ USB_DEVICE(0x0471, 0x209e) }, /* Philips (or NXP) PTA01 */
 
 	{ USB_DEVICE(0x0cf3, 0x7015),
 	  .driver_info = AR9287_USB },  /* Atheros */
diff --git a/drivers/net/wireless/ath/ath9k/htc.h b/drivers/net/wireless/ath/ath9k/htc.h
index 300d3671d0ef..e82a0d4ce23f 100644
--- a/drivers/net/wireless/ath/ath9k/htc.h
+++ b/drivers/net/wireless/ath/ath9k/htc.h
@@ -444,6 +444,10 @@ static inline void ath9k_htc_stop_btcoex(struct ath9k_htc_priv *priv)
 #define OP_BT_SCAN                 BIT(4)
 #define OP_TSF_RESET               BIT(6)
 
+enum htc_op_flags {
+	HTC_FWFLAG_NO_RMW,
+};
+
 struct ath9k_htc_priv {
 	struct device *dev;
 	struct ieee80211_hw *hw;
@@ -482,6 +486,7 @@ struct ath9k_htc_priv {
 	bool reconfig_beacon;
 	unsigned int rxfilter;
 	unsigned long op_flags;
+	unsigned long fw_flags;
 
 	struct ath9k_hw_cal_data caldata;
 	struct ath_spec_scan_priv spec_priv;
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index fd229409f676..d7beefe60683 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -376,17 +376,139 @@ static void ath9k_regwrite_flush(void *hw_priv)
 	mutex_unlock(&priv->wmi->multi_write_mutex);
 }
 
-static u32 ath9k_reg_rmw(void *hw_priv, u32 reg_offset, u32 set, u32 clr)
+static void ath9k_reg_rmw_buffer(void *hw_priv,
+				 u32 reg_offset, u32 set, u32 clr)
+{
+	struct ath_hw *ah = (struct ath_hw *) hw_priv;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
+	u32 rsp_status;
+	int r;
+
+	mutex_lock(&priv->wmi->multi_rmw_mutex);
+
+	/* Store the register/value */
+	priv->wmi->multi_rmw[priv->wmi->multi_rmw_idx].reg =
+		cpu_to_be32(reg_offset);
+	priv->wmi->multi_rmw[priv->wmi->multi_rmw_idx].set =
+		cpu_to_be32(set);
+	priv->wmi->multi_rmw[priv->wmi->multi_rmw_idx].clr =
+		cpu_to_be32(clr);
+
+	priv->wmi->multi_rmw_idx++;
+
+	/* If the buffer is full, send it out. */
+	if (priv->wmi->multi_rmw_idx == MAX_RMW_CMD_NUMBER) {
+		r = ath9k_wmi_cmd(priv->wmi, WMI_REG_RMW_CMDID,
+			  (u8 *) &priv->wmi->multi_rmw,
+			  sizeof(struct register_write) * priv->wmi->multi_rmw_idx,
+			  (u8 *) &rsp_status, sizeof(rsp_status),
+			  100);
+		if (unlikely(r)) {
+			ath_dbg(common, WMI,
+				"REGISTER RMW FAILED, multi len: %d\n",
+				priv->wmi->multi_rmw_idx);
+		}
+		priv->wmi->multi_rmw_idx = 0;
+	}
+
+	mutex_unlock(&priv->wmi->multi_rmw_mutex);
+}
+
+static void ath9k_reg_rmw_flush(void *hw_priv)
 {
-	u32 val;
+	struct ath_hw *ah = (struct ath_hw *) hw_priv;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
+	u32 rsp_status;
+	int r;
+
+	if (test_bit(HTC_FWFLAG_NO_RMW, &priv->fw_flags))
+		return;
+
+	atomic_dec(&priv->wmi->m_rmw_cnt);
 
-	val = ath9k_regread(hw_priv, reg_offset);
-	val &= ~clr;
-	val |= set;
-	ath9k_regwrite(hw_priv, val, reg_offset);
+	mutex_lock(&priv->wmi->multi_rmw_mutex);
+
+	if (priv->wmi->multi_rmw_idx) {
+		r = ath9k_wmi_cmd(priv->wmi, WMI_REG_RMW_CMDID,
+			  (u8 *) &priv->wmi->multi_rmw,
+			  sizeof(struct register_rmw) * priv->wmi->multi_rmw_idx,
+			  (u8 *) &rsp_status, sizeof(rsp_status),
+			  100);
+		if (unlikely(r)) {
+			ath_dbg(common, WMI,
+				"REGISTER RMW FAILED, multi len: %d\n",
+				priv->wmi->multi_rmw_idx);
+		}
+		priv->wmi->multi_rmw_idx = 0;
+	}
+
+	mutex_unlock(&priv->wmi->multi_rmw_mutex);
+}
+
+static void ath9k_enable_rmw_buffer(void *hw_priv)
+{
+	struct ath_hw *ah = (struct ath_hw *) hw_priv;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
+
+	if (test_bit(HTC_FWFLAG_NO_RMW, &priv->fw_flags))
+		return;
+
+	atomic_inc(&priv->wmi->m_rmw_cnt);
+}
+
+static u32 ath9k_reg_rmw_single(void *hw_priv,
+				 u32 reg_offset, u32 set, u32 clr)
+{
+	struct ath_hw *ah = (struct ath_hw *) hw_priv;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
+	struct register_rmw buf, buf_ret;
+	int ret;
+	u32 val = 0;
+
+	buf.reg = cpu_to_be32(reg_offset);
+	buf.set = cpu_to_be32(set);
+	buf.clr = cpu_to_be32(clr);
+
+	ret = ath9k_wmi_cmd(priv->wmi, WMI_REG_RMW_CMDID,
+			  (u8 *) &buf, sizeof(buf),
+			  (u8 *) &buf_ret, sizeof(buf_ret),
+			  100);
+	if (unlikely(ret)) {
+		ath_dbg(common, WMI, "REGISTER RMW FAILED:(0x%04x, %d)\n",
+			reg_offset, ret);
+	}
 	return val;
 }
 
+static u32 ath9k_reg_rmw(void *hw_priv, u32 reg_offset, u32 set, u32 clr)
+{
+	struct ath_hw *ah = (struct ath_hw *) hw_priv;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
+
+	if (test_bit(HTC_FWFLAG_NO_RMW, &priv->fw_flags)) {
+		u32 val;
+
+		val = REG_READ(ah, reg_offset);
+		val &= ~clr;
+		val |= set;
+		REG_WRITE(ah, reg_offset, val);
+
+		return 0;
+	}
+
+	if (atomic_read(&priv->wmi->m_rmw_cnt))
+		ath9k_reg_rmw_buffer(hw_priv, reg_offset, set, clr);
+	else
+		ath9k_reg_rmw_single(hw_priv, reg_offset, set, clr);
+
+	return 0;
+}
+
 static void ath_usb_read_cachesize(struct ath_common *common, int *csz)
 {
 	*csz = L1_CACHE_BYTES >> 2;
@@ -501,6 +623,8 @@ static int ath9k_init_priv(struct ath9k_htc_priv *priv,
 	ah->reg_ops.write = ath9k_regwrite;
 	ah->reg_ops.enable_write_buffer = ath9k_enable_regwrite_buffer;
 	ah->reg_ops.write_flush = ath9k_regwrite_flush;
+	ah->reg_ops.enable_rmw_buffer = ath9k_enable_rmw_buffer;
+	ah->reg_ops.rmw_flush = ath9k_reg_rmw_flush;
 	ah->reg_ops.rmw = ath9k_reg_rmw;
 	priv->ah = ah;
 
@@ -686,6 +810,12 @@ static int ath9k_init_firmware_version(struct ath9k_htc_priv *priv)
 		return -EINVAL;
 	}
 
+	if (priv->fw_version_major == 1 && priv->fw_version_minor < 4)
+		set_bit(HTC_FWFLAG_NO_RMW, &priv->fw_flags);
+
+	dev_info(priv->dev, "FW RMW support: %s\n",
+		test_bit(HTC_FWFLAG_NO_RMW, &priv->fw_flags) ? "Off" : "On");
+
 	return 0;
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index 92d5a6c5a225..564923c0df87 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -149,7 +149,7 @@ static void ath9k_htc_set_mac_bssid_mask(struct ath9k_htc_priv *priv,
 	 * when matching addresses.
 	 */
 	iter_data.hw_macaddr = NULL;
-	memset(&iter_data.mask, 0xff, ETH_ALEN);
+	eth_broadcast_addr(iter_data.mask);
 
 	if (vif)
 		ath9k_htc_bssid_iter(&iter_data, vif->addr, vif);
diff --git a/drivers/net/wireless/ath/ath9k/hw-ops.h b/drivers/net/wireless/ath/ath9k/hw-ops.h
index 88769b64b20b..232339b05540 100644
--- a/drivers/net/wireless/ath/ath9k/hw-ops.h
+++ b/drivers/net/wireless/ath/ath9k/hw-ops.h
@@ -108,6 +108,14 @@ static inline void ath9k_hw_set_bt_ant_diversity(struct ath_hw *ah, bool enable)
 		ath9k_hw_ops(ah)->set_bt_ant_diversity(ah, enable);
 }
 
+static inline bool ath9k_hw_is_aic_enabled(struct ath_hw *ah)
+{
+	if (ath9k_hw_private_ops(ah)->is_aic_enabled)
+		return ath9k_hw_private_ops(ah)->is_aic_enabled(ah);
+
+	return false;
+}
+
 #endif
 
 /* Private hardware call ops */
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 60aa8d71e753..5e15e8e10ed3 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -20,6 +20,7 @@
 #include <linux/time.h>
 #include <linux/bitops.h>
 #include <linux/etherdevice.h>
+#include <linux/gpio.h>
 #include <asm/unaligned.h>
 
 #include "hw.h"
@@ -121,6 +122,36 @@ void ath9k_hw_write_array(struct ath_hw *ah, const struct ar5416IniArray *array,
 	REGWRITE_BUFFER_FLUSH(ah);
 }
 
+void ath9k_hw_read_array(struct ath_hw *ah, u32 array[][2], int size)
+{
+	u32 *tmp_reg_list, *tmp_data;
+	int i;
+
+	tmp_reg_list = kmalloc(size * sizeof(u32), GFP_KERNEL);
+	if (!tmp_reg_list) {
+		dev_err(ah->dev, "%s: tmp_reg_list: alloc filed\n", __func__);
+		return;
+	}
+
+	tmp_data = kmalloc(size * sizeof(u32), GFP_KERNEL);
+	if (!tmp_data) {
+		dev_err(ah->dev, "%s tmp_data: alloc filed\n", __func__);
+		goto error_tmp_data;
+	}
+
+	for (i = 0; i < size; i++)
+		tmp_reg_list[i] = array[i][0];
+
+	REG_READ_MULTI(ah, tmp_reg_list, tmp_data, size);
+
+	for (i = 0; i < size; i++)
+		array[i][1] = tmp_data[i];
+
+	kfree(tmp_data);
+error_tmp_data:
+	kfree(tmp_reg_list);
+}
+
 u32 ath9k_hw_reverse_bits(u32 val, u32 n)
 {
 	u32 retval;
@@ -366,6 +397,9 @@ static void ath9k_hw_init_config(struct ath_hw *ah)
 		ah->config.rimt_first = 700;
 	}
 
+	if (AR_SREV_9462(ah) || AR_SREV_9565(ah))
+		ah->config.pll_pwrsave = 7;
+
 	/*
 	 * We need this for PCI devices only (Cardbus, PCI, miniPCI)
 	 * _and_ if on non-uniprocessor systems (Multiprocessor/HT).
@@ -424,7 +458,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))
@@ -1197,6 +1231,7 @@ static void ath9k_hw_set_operating_mode(struct ath_hw *ah, int opmode)
 	u32 mask = AR_STA_ID1_STA_AP | AR_STA_ID1_ADHOC;
 	u32 set = AR_STA_ID1_KSRCH_MODE;
 
+	ENABLE_REG_RMW_BUFFER(ah);
 	switch (opmode) {
 	case NL80211_IFTYPE_ADHOC:
 		if (!AR_SREV_9340_13(ah)) {
@@ -1218,6 +1253,7 @@ static void ath9k_hw_set_operating_mode(struct ath_hw *ah, int opmode)
 		break;
 	}
 	REG_RMW(ah, AR_STA_ID1, set, mask);
+	REG_RMW_BUFFER_FLUSH(ah);
 }
 
 void ath9k_hw_get_delta_slope_vals(struct ath_hw *ah, u32 coef_scaled,
@@ -1930,6 +1966,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (!ath9k_hw_mci_is_enabled(ah))
 		REG_WRITE(ah, AR_OBS, 8);
 
+	ENABLE_REG_RMW_BUFFER(ah);
 	if (ah->config.rx_intr_mitigation) {
 		REG_RMW_FIELD(ah, AR_RIMT, AR_RIMT_LAST, ah->config.rimt_last);
 		REG_RMW_FIELD(ah, AR_RIMT, AR_RIMT_FIRST, ah->config.rimt_first);
@@ -1939,6 +1976,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 		REG_RMW_FIELD(ah, AR_TIMT, AR_TIMT_LAST, 300);
 		REG_RMW_FIELD(ah, AR_TIMT, AR_TIMT_FIRST, 750);
 	}
+	REG_RMW_BUFFER_FLUSH(ah);
 
 	ath9k_hw_init_bb(ah, chan);
 
@@ -2674,11 +2712,23 @@ void ath9k_hw_set_gpio(struct ath_hw *ah, u32 gpio, u32 val)
 	if (AR_SREV_9271(ah))
 		val = ~val;
 
-	REG_RMW(ah, AR_GPIO_IN_OUT, ((val & 1) << gpio),
-		AR_GPIO_BIT(gpio));
+	if ((1 << gpio) & AR_GPIO_OE_OUT_MASK)
+		REG_RMW(ah, AR_GPIO_IN_OUT, ((val & 1) << gpio),
+			AR_GPIO_BIT(gpio));
+	else
+		gpio_set_value(gpio, val & 1);
 }
 EXPORT_SYMBOL(ath9k_hw_set_gpio);
 
+void ath9k_hw_request_gpio(struct ath_hw *ah, u32 gpio, const char *label)
+{
+	if (gpio >= ah->caps.num_gpio_pins)
+		return;
+
+	gpio_request_one(gpio, GPIOF_DIR_OUT | GPIOF_INIT_LOW, label);
+}
+EXPORT_SYMBOL(ath9k_hw_request_gpio);
+
 void ath9k_hw_setantenna(struct ath_hw *ah, u32 antenna)
 {
 	REG_WRITE(ah, AR_DEF_ANTENNA, (antenna & 0x7));
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index e82e570de330..c1d2d0340feb 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -27,6 +27,7 @@
 #include "eeprom.h"
 #include "calib.h"
 #include "reg.h"
+#include "reg_mci.h"
 #include "phy.h"
 #include "btcoex.h"
 #include "dynack.h"
@@ -99,6 +100,18 @@
 			(_ah)->reg_ops.write_flush((_ah));	\
 	} while (0)
 
+#define ENABLE_REG_RMW_BUFFER(_ah)					\
+	do {								\
+		if ((_ah)->reg_ops.enable_rmw_buffer)	\
+			(_ah)->reg_ops.enable_rmw_buffer((_ah)); \
+	} while (0)
+
+#define REG_RMW_BUFFER_FLUSH(_ah)					\
+	do {								\
+		if ((_ah)->reg_ops.rmw_flush)		\
+			(_ah)->reg_ops.rmw_flush((_ah));	\
+	} while (0)
+
 #define PR_EEP(_s, _val)						\
 	do {								\
 		len += scnprintf(buf + len, size - len, "%20s : %10d\n",\
@@ -125,6 +138,8 @@
 
 #define REG_WRITE_ARRAY(iniarray, column, regWr) \
 	ath9k_hw_write_array(ah, iniarray, column, &(regWr))
+#define REG_READ_ARRAY(ah, array, size) \
+	ath9k_hw_read_array(ah, array, size)
 
 #define AR_GPIO_OUTPUT_MUX_AS_OUTPUT             0
 #define AR_GPIO_OUTPUT_MUX_AS_PCIE_ATTENTION_LED 1
@@ -308,6 +323,12 @@ enum ath9k_hw_hang_checks {
 	HW_MAC_HANG               = BIT(5),
 };
 
+#define AR_PCIE_PLL_PWRSAVE_CONTROL BIT(0)
+#define AR_PCIE_PLL_PWRSAVE_ON_D3   BIT(1)
+#define AR_PCIE_PLL_PWRSAVE_ON_D0   BIT(2)
+#define AR_PCIE_CDR_PWRSAVE_ON_D3   BIT(3)
+#define AR_PCIE_CDR_PWRSAVE_ON_D0   BIT(4)
+
 struct ath9k_ops_config {
 	int dma_beacon_response_time;
 	int sw_beacon_response_time;
@@ -334,7 +355,7 @@ struct ath9k_ops_config {
 	u32 ant_ctrl_comm2g_switch_enable;
 	bool xatten_margin_cfg;
 	bool alt_mingainidx;
-	bool no_pll_pwrsave;
+	u8 pll_pwrsave;
 	bool tx_gain_buffalo;
 	bool led_active_high;
 };
@@ -646,6 +667,10 @@ struct ath_hw_private_ops {
 
 	/* ANI */
 	void (*ani_cache_ini_regs)(struct ath_hw *ah);
+
+#ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
+	bool (*is_aic_enabled)(struct ath_hw *ah);
+#endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 };
 
 /**
@@ -999,6 +1024,7 @@ u32 ath9k_hw_gpio_get(struct ath_hw *ah, u32 gpio);
 void ath9k_hw_cfg_output(struct ath_hw *ah, u32 gpio,
 			 u32 ah_signal_type);
 void ath9k_hw_set_gpio(struct ath_hw *ah, u32 gpio, u32 val);
+void ath9k_hw_request_gpio(struct ath_hw *ah, u32 gpio, const char *label);
 void ath9k_hw_setantenna(struct ath_hw *ah, u32 antenna);
 
 /* General Operation */
@@ -1007,6 +1033,7 @@ void ath9k_hw_synth_delay(struct ath_hw *ah, struct ath9k_channel *chan,
 bool ath9k_hw_wait(struct ath_hw *ah, u32 reg, u32 mask, u32 val, u32 timeout);
 void ath9k_hw_write_array(struct ath_hw *ah, const struct ar5416IniArray *array,
 			  int column, unsigned int *writecnt);
+void ath9k_hw_read_array(struct ath_hw *ah, u32 array[][2], int size);
 u32 ath9k_hw_reverse_bits(u32 val, u32 n);
 u16 ath9k_hw_computetxtime(struct ath_hw *ah,
 			   u8 phy, int kbps,
@@ -1116,6 +1143,7 @@ void ath9k_hw_set_cts_timeout(struct ath_hw *ah, u32 us);
 void ath9k_hw_setslottime(struct ath_hw *ah, u32 us);
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
+void ar9003_hw_attach_aic_ops(struct ath_hw *ah);
 static inline bool ath9k_hw_btcoex_is_enabled(struct ath_hw *ah)
 {
 	return ah->btcoex_hw.enabled;
@@ -1133,6 +1161,9 @@ ath9k_hw_get_btcoex_scheme(struct ath_hw *ah)
 	return ah->btcoex_hw.scheme;
 }
 #else
+static inline void ar9003_hw_attach_aic_ops(struct ath_hw *ah)
+{
+}
 static inline bool ath9k_hw_btcoex_is_enabled(struct ath_hw *ah)
 {
 	return false;
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index 6c6e88495394..f8d11efa7b0f 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -141,6 +141,16 @@ static unsigned int ath9k_ioread32(void *hw_priv, u32 reg_offset)
 	return val;
 }
 
+static void ath9k_multi_ioread32(void *hw_priv, u32 *addr,
+                                u32 *val, u16 count)
+{
+	int i;
+
+	for (i = 0; i < count; i++)
+		val[i] = ath9k_ioread32(hw_priv, addr[i]);
+}
+
+
 static unsigned int __ath9k_reg_rmw(struct ath_softc *sc, u32 reg_offset,
 				    u32 set, u32 clr)
 {
@@ -437,8 +447,15 @@ static void ath9k_init_pcoem_platform(struct ath_softc *sc)
 		ath_info(common, "Enable WAR for ASPM D3/L1\n");
 	}
 
+	/*
+	 * The default value of pll_pwrsave is 1.
+	 * For certain AR9485 cards, it is set to 0.
+	 * For AR9462, AR9565 it's set to 7.
+	 */
+	ah->config.pll_pwrsave = 1;
+
 	if (sc->driver_data & ATH9K_PCI_NO_PLL_PWRSAVE) {
-		ah->config.no_pll_pwrsave = true;
+		ah->config.pll_pwrsave = 0;
 		ath_info(common, "Disable PLL PowerSave\n");
 	}
 
@@ -530,6 +547,7 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	ah->hw = sc->hw;
 	ah->hw_version.devid = devid;
 	ah->reg_ops.read = ath9k_ioread32;
+	ah->reg_ops.multi_read = ath9k_multi_ioread32;
 	ah->reg_ops.write = ath9k_iowrite32;
 	ah->reg_ops.rmw = ath9k_reg_rmw;
 	pCap = &ah->caps;
@@ -763,7 +781,8 @@ static const struct ieee80211_iface_combination if_comb[] = {
 		.num_different_channels = 1,
 		.beacon_int_infra_match = true,
 		.radar_detect_widths =	BIT(NL80211_CHAN_WIDTH_20_NOHT) |
-					BIT(NL80211_CHAN_WIDTH_20),
+					BIT(NL80211_CHAN_WIDTH_20) |
+					BIT(NL80211_CHAN_WIDTH_40),
 	}
 #endif
 };
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 9ede991b8d76..b0badef71ce7 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -994,7 +994,7 @@ void ath9k_calculate_iter_data(struct ath_softc *sc,
 	 * BSSID mask when matching addresses.
 	 */
 	memset(iter_data, 0, sizeof(*iter_data));
-	memset(&iter_data->mask, 0xff, ETH_ALEN);
+	eth_broadcast_addr(iter_data->mask);
 	iter_data->slottime = ATH9K_SLOT_TIME_9;
 
 	list_for_each_entry(avp, &ctx->vifs, list)
@@ -1139,7 +1139,7 @@ void ath9k_calculate_summary_state(struct ath_softc *sc,
 			ctx->primary_sta = iter_data.primary_sta;
 		} else {
 			ctx->primary_sta = NULL;
-			memset(common->curbssid, 0, ETH_ALEN);
+			eth_zero_addr(common->curbssid);
 			common->curaid = 0;
 			ath9k_hw_write_associd(sc->sc_ah);
 			if (ath9k_hw_mci_is_enabled(sc->sc_ah))
@@ -1172,6 +1172,38 @@ void ath9k_calculate_summary_state(struct ath_softc *sc,
 	ath9k_ps_restore(sc);
 }
 
+static void ath9k_tpc_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	int *power = (int *)data;
+
+	if (*power < vif->bss_conf.txpower)
+		*power = vif->bss_conf.txpower;
+}
+
+/* Called with sc->mutex held. */
+void ath9k_set_txpower(struct ath_softc *sc, struct ieee80211_vif *vif)
+{
+	int power;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_regulatory *reg = ath9k_hw_regulatory(ah);
+
+	ath9k_ps_wakeup(sc);
+	if (ah->tpc_enabled) {
+		power = (vif) ? vif->bss_conf.txpower : -1;
+		ieee80211_iterate_active_interfaces_atomic(
+				sc->hw, IEEE80211_IFACE_ITER_RESUME_ALL,
+				ath9k_tpc_vif_iter, &power);
+		if (power == -1)
+			power = sc->hw->conf.power_level;
+	} else {
+		power = sc->hw->conf.power_level;
+	}
+	sc->cur_chan->txpower = 2 * power;
+	ath9k_hw_set_txpowerlimit(ah, sc->cur_chan->txpower, false);
+	sc->cur_chan->cur_txpower = reg->max_power_level;
+	ath9k_ps_restore(sc);
+}
+
 static void ath9k_assign_hw_queues(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif)
 {
@@ -1225,6 +1257,8 @@ static int ath9k_add_interface(struct ieee80211_hw *hw,
 
 	ath9k_assign_hw_queues(hw, vif);
 
+	ath9k_set_txpower(sc, vif);
+
 	an->sc = sc;
 	an->sta = NULL;
 	an->vif = vif;
@@ -1265,6 +1299,8 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
 	ath9k_assign_hw_queues(hw, vif);
 	ath9k_calculate_summary_state(sc, avp->chanctx);
 
+	ath9k_set_txpower(sc, vif);
+
 	mutex_unlock(&sc->mutex);
 	return 0;
 }
@@ -1294,6 +1330,8 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
 
 	ath9k_calculate_summary_state(sc, avp->chanctx);
 
+	ath9k_set_txpower(sc, NULL);
+
 	mutex_unlock(&sc->mutex);
 }
 
@@ -1397,14 +1435,6 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 		ath_chanctx_set_channel(sc, ctx, &hw->conf.chandef);
 	}
 
-	if (changed & IEEE80211_CONF_CHANGE_POWER) {
-		ath_dbg(common, CONFIG, "Set power: %d\n", conf->power_level);
-		sc->cur_chan->txpower = 2 * conf->power_level;
-		ath9k_cmn_update_txpow(ah, sc->cur_chan->cur_txpower,
-				       sc->cur_chan->txpower,
-				       &sc->cur_chan->cur_txpower);
-	}
-
 	mutex_unlock(&sc->mutex);
 	ath9k_ps_restore(sc);
 
@@ -1764,6 +1794,12 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 	if (changed & CHECK_ANI)
 		ath_check_ani(sc);
 
+	if (changed & BSS_CHANGED_TXPOWER) {
+		ath_dbg(common, CONFIG, "vif %pM power %d dbm power_type %d\n",
+			vif->addr, bss_conf->txpower, bss_conf->txpower_type);
+		ath9k_set_txpower(sc, vif);
+	}
+
 	mutex_unlock(&sc->mutex);
 	ath9k_ps_restore(sc);
 
diff --git a/drivers/net/wireless/ath/ath9k/mci.c b/drivers/net/wireless/ath/ath9k/mci.c
index 3f7a11edb82a..66596b95273f 100644
--- a/drivers/net/wireless/ath/ath9k/mci.c
+++ b/drivers/net/wireless/ath/ath9k/mci.c
@@ -495,7 +495,7 @@ void ath_mci_intr(struct ath_softc *sc)
 	ar9003_mci_get_interrupt(sc->sc_ah, &mci_int, &mci_int_rxmsg);
 
 	if (ar9003_mci_state(ah, MCI_STATE_ENABLE) == 0) {
-		ar9003_mci_get_next_gpm_offset(ah, true, NULL);
+		ar9003_mci_state(ah, MCI_STATE_INIT_GPM_OFFSET);
 		return;
 	}
 
@@ -559,8 +559,7 @@ void ath_mci_intr(struct ath_softc *sc)
 				return;
 
 			pgpm = mci->gpm_buf.bf_addr;
-			offset = ar9003_mci_get_next_gpm_offset(ah, false,
-								&more_data);
+			offset = ar9003_mci_get_next_gpm_offset(ah, &more_data);
 
 			if (offset == MCI_GPM_INVALID)
 				break;
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index 9587ec655680..caba54ddad25 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -958,6 +958,8 @@
 
 #define AR_SREV_9550(_ah) \
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9550))
+#define AR_SREV_9550_OR_LATER(_ah) \
+	(((_ah)->hw_version.macVersion >= AR_SREV_VERSION_9550))
 
 #define AR_SREV_9580(_ah) \
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9580) && \
@@ -1128,6 +1130,8 @@ enum {
 
 #define AR_GPIO_OE_OUT                           (AR_SREV_9340(ah) ? 0x4030 : \
 						  (AR_SREV_9300_20_OR_LATER(ah) ? 0x4050 : 0x404c))
+#define AR_GPIO_OE_OUT_MASK			 (AR_SREV_9550_OR_LATER(ah) ? \
+						  0x0000000F : 0xFFFFFFFF)
 #define AR_GPIO_OE_OUT_DRV                       0x3
 #define AR_GPIO_OE_OUT_DRV_NO                    0x0
 #define AR_GPIO_OE_OUT_DRV_LOW                   0x1
@@ -2044,279 +2048,4 @@ enum {
 #define AR_PHY_AGC_CONTROL_YCOK_MAX		0x000003c0
 #define AR_PHY_AGC_CONTROL_YCOK_MAX_S		6
 
-/* MCI Registers */
-
-#define AR_MCI_COMMAND0				0x1800
-#define AR_MCI_COMMAND0_HEADER			0xFF
-#define AR_MCI_COMMAND0_HEADER_S		0
-#define AR_MCI_COMMAND0_LEN			0x1f00
-#define AR_MCI_COMMAND0_LEN_S			8
-#define AR_MCI_COMMAND0_DISABLE_TIMESTAMP	0x2000
-#define AR_MCI_COMMAND0_DISABLE_TIMESTAMP_S	13
-
-#define AR_MCI_COMMAND1				0x1804
-
-#define AR_MCI_COMMAND2				0x1808
-#define AR_MCI_COMMAND2_RESET_TX		0x01
-#define AR_MCI_COMMAND2_RESET_TX_S		0
-#define AR_MCI_COMMAND2_RESET_RX		0x02
-#define AR_MCI_COMMAND2_RESET_RX_S		1
-#define AR_MCI_COMMAND2_RESET_RX_NUM_CYCLES     0x3FC
-#define AR_MCI_COMMAND2_RESET_RX_NUM_CYCLES_S   2
-#define AR_MCI_COMMAND2_RESET_REQ_WAKEUP        0x400
-#define AR_MCI_COMMAND2_RESET_REQ_WAKEUP_S      10
-
-#define AR_MCI_RX_CTRL				0x180c
-
-#define AR_MCI_TX_CTRL				0x1810
-/* 0 = no division, 1 = divide by 2, 2 = divide by 4, 3 = divide by 8 */
-#define AR_MCI_TX_CTRL_CLK_DIV			0x03
-#define AR_MCI_TX_CTRL_CLK_DIV_S		0
-#define AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE	0x04
-#define AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE_S	2
-#define AR_MCI_TX_CTRL_GAIN_UPDATE_FREQ		0xFFFFF8
-#define AR_MCI_TX_CTRL_GAIN_UPDATE_FREQ_S	3
-#define AR_MCI_TX_CTRL_GAIN_UPDATE_NUM		0xF000000
-#define AR_MCI_TX_CTRL_GAIN_UPDATE_NUM_S	24
-
-#define AR_MCI_MSG_ATTRIBUTES_TABLE			0x1814
-#define AR_MCI_MSG_ATTRIBUTES_TABLE_CHECKSUM		0xFFFF
-#define AR_MCI_MSG_ATTRIBUTES_TABLE_CHECKSUM_S		0
-#define AR_MCI_MSG_ATTRIBUTES_TABLE_INVALID_HDR		0xFFFF0000
-#define AR_MCI_MSG_ATTRIBUTES_TABLE_INVALID_HDR_S	16
-
-#define AR_MCI_SCHD_TABLE_0				0x1818
-#define AR_MCI_SCHD_TABLE_1				0x181c
-#define AR_MCI_GPM_0					0x1820
-#define AR_MCI_GPM_1					0x1824
-#define AR_MCI_GPM_WRITE_PTR				0xFFFF0000
-#define AR_MCI_GPM_WRITE_PTR_S				16
-#define AR_MCI_GPM_BUF_LEN				0x0000FFFF
-#define AR_MCI_GPM_BUF_LEN_S				0
-
-#define AR_MCI_INTERRUPT_RAW				0x1828
-#define AR_MCI_INTERRUPT_EN				0x182c
-#define AR_MCI_INTERRUPT_SW_MSG_DONE			0x00000001
-#define AR_MCI_INTERRUPT_SW_MSG_DONE_S			0
-#define AR_MCI_INTERRUPT_CPU_INT_MSG			0x00000002
-#define AR_MCI_INTERRUPT_CPU_INT_MSG_S			1
-#define AR_MCI_INTERRUPT_RX_CKSUM_FAIL			0x00000004
-#define AR_MCI_INTERRUPT_RX_CKSUM_FAIL_S		2
-#define AR_MCI_INTERRUPT_RX_INVALID_HDR			0x00000008
-#define AR_MCI_INTERRUPT_RX_INVALID_HDR_S		3
-#define AR_MCI_INTERRUPT_RX_HW_MSG_FAIL			0x00000010
-#define AR_MCI_INTERRUPT_RX_HW_MSG_FAIL_S		4
-#define AR_MCI_INTERRUPT_RX_SW_MSG_FAIL			0x00000020
-#define AR_MCI_INTERRUPT_RX_SW_MSG_FAIL_S		5
-#define AR_MCI_INTERRUPT_TX_HW_MSG_FAIL			0x00000080
-#define AR_MCI_INTERRUPT_TX_HW_MSG_FAIL_S		7
-#define AR_MCI_INTERRUPT_TX_SW_MSG_FAIL			0x00000100
-#define AR_MCI_INTERRUPT_TX_SW_MSG_FAIL_S		8
-#define AR_MCI_INTERRUPT_RX_MSG				0x00000200
-#define AR_MCI_INTERRUPT_RX_MSG_S			9
-#define AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE		0x00000400
-#define AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE_S		10
-#define AR_MCI_INTERRUPT_BT_PRI				0x07fff800
-#define AR_MCI_INTERRUPT_BT_PRI_S			11
-#define AR_MCI_INTERRUPT_BT_PRI_THRESH			0x08000000
-#define AR_MCI_INTERRUPT_BT_PRI_THRESH_S		27
-#define AR_MCI_INTERRUPT_BT_FREQ			0x10000000
-#define AR_MCI_INTERRUPT_BT_FREQ_S			28
-#define AR_MCI_INTERRUPT_BT_STOMP			0x20000000
-#define AR_MCI_INTERRUPT_BT_STOMP_S			29
-#define AR_MCI_INTERRUPT_BB_AIC_IRQ			0x40000000
-#define AR_MCI_INTERRUPT_BB_AIC_IRQ_S			30
-#define AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT		0x80000000
-#define AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT_S		31
-
-#define AR_MCI_INTERRUPT_DEFAULT    (AR_MCI_INTERRUPT_SW_MSG_DONE	  | \
-				     AR_MCI_INTERRUPT_RX_INVALID_HDR	  | \
-				     AR_MCI_INTERRUPT_RX_HW_MSG_FAIL	  | \
-				     AR_MCI_INTERRUPT_RX_SW_MSG_FAIL	  | \
-				     AR_MCI_INTERRUPT_TX_HW_MSG_FAIL	  | \
-				     AR_MCI_INTERRUPT_TX_SW_MSG_FAIL	  | \
-				     AR_MCI_INTERRUPT_RX_MSG		  | \
-				     AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE | \
-				     AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT)
-
-#define AR_MCI_INTERRUPT_MSG_FAIL_MASK (AR_MCI_INTERRUPT_RX_HW_MSG_FAIL | \
-					AR_MCI_INTERRUPT_RX_SW_MSG_FAIL | \
-					AR_MCI_INTERRUPT_TX_HW_MSG_FAIL | \
-					AR_MCI_INTERRUPT_TX_SW_MSG_FAIL)
-
-#define AR_MCI_REMOTE_CPU_INT				0x1830
-#define AR_MCI_REMOTE_CPU_INT_EN			0x1834
-#define AR_MCI_INTERRUPT_RX_MSG_RAW			0x1838
-#define AR_MCI_INTERRUPT_RX_MSG_EN			0x183c
-#define AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET		0x00000001
-#define AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET_S		0
-#define AR_MCI_INTERRUPT_RX_MSG_LNA_CONTROL		0x00000002
-#define AR_MCI_INTERRUPT_RX_MSG_LNA_CONTROL_S		1
-#define AR_MCI_INTERRUPT_RX_MSG_CONT_NACK		0x00000004
-#define AR_MCI_INTERRUPT_RX_MSG_CONT_NACK_S		2
-#define AR_MCI_INTERRUPT_RX_MSG_CONT_INFO		0x00000008
-#define AR_MCI_INTERRUPT_RX_MSG_CONT_INFO_S		3
-#define AR_MCI_INTERRUPT_RX_MSG_CONT_RST		0x00000010
-#define AR_MCI_INTERRUPT_RX_MSG_CONT_RST_S		4
-#define AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO		0x00000020
-#define AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO_S		5
-#define AR_MCI_INTERRUPT_RX_MSG_CPU_INT			0x00000040
-#define AR_MCI_INTERRUPT_RX_MSG_CPU_INT_S		6
-#define AR_MCI_INTERRUPT_RX_MSG_GPM			0x00000100
-#define AR_MCI_INTERRUPT_RX_MSG_GPM_S			8
-#define AR_MCI_INTERRUPT_RX_MSG_LNA_INFO		0x00000200
-#define AR_MCI_INTERRUPT_RX_MSG_LNA_INFO_S		9
-#define AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING		0x00000400
-#define AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING_S		10
-#define AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING		0x00000800
-#define AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING_S		11
-#define AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE		0x00001000
-#define AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE_S		12
-#define AR_MCI_INTERRUPT_RX_HW_MSG_MASK	 (AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO  | \
-					  AR_MCI_INTERRUPT_RX_MSG_LNA_CONTROL| \
-					  AR_MCI_INTERRUPT_RX_MSG_LNA_INFO   | \
-					  AR_MCI_INTERRUPT_RX_MSG_CONT_NACK  | \
-					  AR_MCI_INTERRUPT_RX_MSG_CONT_INFO  | \
-					  AR_MCI_INTERRUPT_RX_MSG_CONT_RST)
-
-#define AR_MCI_INTERRUPT_RX_MSG_DEFAULT (AR_MCI_INTERRUPT_RX_MSG_GPM	 | \
-					 AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET| \
-					 AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING  | \
-					 AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING| \
-					 AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE)
-
-#define AR_MCI_CPU_INT					0x1840
-
-#define AR_MCI_RX_STATUS			0x1844
-#define AR_MCI_RX_LAST_SCHD_MSG_INDEX		0x00000F00
-#define AR_MCI_RX_LAST_SCHD_MSG_INDEX_S		8
-#define AR_MCI_RX_REMOTE_SLEEP			0x00001000
-#define AR_MCI_RX_REMOTE_SLEEP_S		12
-#define AR_MCI_RX_MCI_CLK_REQ			0x00002000
-#define AR_MCI_RX_MCI_CLK_REQ_S			13
-
-#define AR_MCI_CONT_STATUS			0x1848
-#define AR_MCI_CONT_RSSI_POWER			0x000000FF
-#define AR_MCI_CONT_RSSI_POWER_S		0
-#define AR_MCI_CONT_PRIORITY			0x0000FF00
-#define AR_MCI_CONT_PRIORITY_S			8
-#define AR_MCI_CONT_TXRX			0x00010000
-#define AR_MCI_CONT_TXRX_S			16
-
-#define AR_MCI_BT_PRI0				0x184c
-#define AR_MCI_BT_PRI1				0x1850
-#define AR_MCI_BT_PRI2				0x1854
-#define AR_MCI_BT_PRI3				0x1858
-#define AR_MCI_BT_PRI				0x185c
-#define AR_MCI_WL_FREQ0				0x1860
-#define AR_MCI_WL_FREQ1				0x1864
-#define AR_MCI_WL_FREQ2				0x1868
-#define AR_MCI_GAIN				0x186c
-#define AR_MCI_WBTIMER1				0x1870
-#define AR_MCI_WBTIMER2				0x1874
-#define AR_MCI_WBTIMER3				0x1878
-#define AR_MCI_WBTIMER4				0x187c
-#define AR_MCI_MAXGAIN				0x1880
-#define AR_MCI_HW_SCHD_TBL_CTL			0x1884
-#define AR_MCI_HW_SCHD_TBL_D0			0x1888
-#define AR_MCI_HW_SCHD_TBL_D1			0x188c
-#define AR_MCI_HW_SCHD_TBL_D2			0x1890
-#define AR_MCI_HW_SCHD_TBL_D3			0x1894
-#define AR_MCI_TX_PAYLOAD0			0x1898
-#define AR_MCI_TX_PAYLOAD1			0x189c
-#define AR_MCI_TX_PAYLOAD2			0x18a0
-#define AR_MCI_TX_PAYLOAD3			0x18a4
-#define AR_BTCOEX_WBTIMER			0x18a8
-
-#define AR_BTCOEX_CTRL					0x18ac
-#define AR_BTCOEX_CTRL_AR9462_MODE			0x00000001
-#define AR_BTCOEX_CTRL_AR9462_MODE_S			0
-#define AR_BTCOEX_CTRL_WBTIMER_EN			0x00000002
-#define AR_BTCOEX_CTRL_WBTIMER_EN_S			1
-#define AR_BTCOEX_CTRL_MCI_MODE_EN			0x00000004
-#define AR_BTCOEX_CTRL_MCI_MODE_EN_S			2
-#define AR_BTCOEX_CTRL_LNA_SHARED			0x00000008
-#define AR_BTCOEX_CTRL_LNA_SHARED_S			3
-#define AR_BTCOEX_CTRL_PA_SHARED			0x00000010
-#define AR_BTCOEX_CTRL_PA_SHARED_S			4
-#define AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN		0x00000020
-#define AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN_S		5
-#define AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN	0x00000040
-#define AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN_S	6
-#define AR_BTCOEX_CTRL_NUM_ANTENNAS			0x00000180
-#define AR_BTCOEX_CTRL_NUM_ANTENNAS_S			7
-#define AR_BTCOEX_CTRL_RX_CHAIN_MASK			0x00000E00
-#define AR_BTCOEX_CTRL_RX_CHAIN_MASK_S			9
-#define AR_BTCOEX_CTRL_AGGR_THRESH			0x00007000
-#define AR_BTCOEX_CTRL_AGGR_THRESH_S			12
-#define AR_BTCOEX_CTRL_1_CHAIN_BCN			0x00080000
-#define AR_BTCOEX_CTRL_1_CHAIN_BCN_S			19
-#define AR_BTCOEX_CTRL_1_CHAIN_ACK			0x00100000
-#define AR_BTCOEX_CTRL_1_CHAIN_ACK_S			20
-#define AR_BTCOEX_CTRL_WAIT_BA_MARGIN			0x1FE00000
-#define AR_BTCOEX_CTRL_WAIT_BA_MARGIN_S			28
-#define AR_BTCOEX_CTRL_REDUCE_TXPWR			0x20000000
-#define AR_BTCOEX_CTRL_REDUCE_TXPWR_S			29
-#define AR_BTCOEX_CTRL_SPDT_ENABLE_10			0x40000000
-#define AR_BTCOEX_CTRL_SPDT_ENABLE_10_S			30
-#define AR_BTCOEX_CTRL_SPDT_POLARITY			0x80000000
-#define AR_BTCOEX_CTRL_SPDT_POLARITY_S			31
-
-#define AR_BTCOEX_MAX_TXPWR(_x)				(0x18c0 + ((_x) << 2))
-#define AR_BTCOEX_WL_LNA				0x1940
-#define AR_BTCOEX_RFGAIN_CTRL				0x1944
-#define AR_BTCOEX_WL_LNA_TIMEOUT			0x003FFFFF
-#define AR_BTCOEX_WL_LNA_TIMEOUT_S			0
-
-#define AR_BTCOEX_CTRL2					0x1948
-#define AR_BTCOEX_CTRL2_TXPWR_THRESH			0x0007F800
-#define AR_BTCOEX_CTRL2_TXPWR_THRESH_S			11
-#define AR_BTCOEX_CTRL2_TX_CHAIN_MASK			0x00380000
-#define AR_BTCOEX_CTRL2_TX_CHAIN_MASK_S			19
-#define AR_BTCOEX_CTRL2_RX_DEWEIGHT			0x00400000
-#define AR_BTCOEX_CTRL2_RX_DEWEIGHT_S			22
-#define AR_BTCOEX_CTRL2_GPIO_OBS_SEL			0x00800000
-#define AR_BTCOEX_CTRL2_GPIO_OBS_SEL_S			23
-#define AR_BTCOEX_CTRL2_MAC_BB_OBS_SEL			0x01000000
-#define AR_BTCOEX_CTRL2_MAC_BB_OBS_SEL_S		24
-#define AR_BTCOEX_CTRL2_DESC_BASED_TXPWR_ENABLE		0x02000000
-#define AR_BTCOEX_CTRL2_DESC_BASED_TXPWR_ENABLE_S	25
-
-#define AR_BTCOEX_CTRL_SPDT_ENABLE          0x00000001
-#define AR_BTCOEX_CTRL_SPDT_ENABLE_S        0
-#define AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL     0x00000002
-#define AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL_S   1
-#define AR_BTCOEX_CTRL_USE_LATCHED_BT_ANT   0x00000004
-#define AR_BTCOEX_CTRL_USE_LATCHED_BT_ANT_S 2
-#define AR_GLB_WLAN_UART_INTF_EN            0x00020000
-#define AR_GLB_WLAN_UART_INTF_EN_S          17
-#define AR_GLB_DS_JTAG_DISABLE              0x00040000
-#define AR_GLB_DS_JTAG_DISABLE_S            18
-
-#define AR_BTCOEX_RC                    0x194c
-#define AR_BTCOEX_MAX_RFGAIN(_x)        (0x1950 + ((_x) << 2))
-#define AR_BTCOEX_DBG                   0x1a50
-#define AR_MCI_LAST_HW_MSG_HDR          0x1a54
-#define AR_MCI_LAST_HW_MSG_BDY          0x1a58
-
-#define AR_MCI_SCHD_TABLE_2             0x1a5c
-#define AR_MCI_SCHD_TABLE_2_MEM_BASED   0x00000001
-#define AR_MCI_SCHD_TABLE_2_MEM_BASED_S 0
-#define AR_MCI_SCHD_TABLE_2_HW_BASED    0x00000002
-#define AR_MCI_SCHD_TABLE_2_HW_BASED_S  1
-
-#define AR_BTCOEX_CTRL3               0x1a60
-#define AR_BTCOEX_CTRL3_CONT_INFO_TIMEOUT	0x00000fff
-#define AR_BTCOEX_CTRL3_CONT_INFO_TIMEOUT_S	0
-
-#define AR_GLB_SWREG_DISCONT_MODE         0x2002c
-#define AR_GLB_SWREG_DISCONT_EN_BT_WLAN   0x3
-
-#define AR_MCI_MISC                    0x1a74
-#define AR_MCI_MISC_HW_FIX_EN          0x00000001
-#define AR_MCI_MISC_HW_FIX_EN_S        0
-#define AR_MCI_DBG_CNT_CTRL            0x1a78
-#define AR_MCI_DBG_CNT_CTRL_ENABLE     0x00000001
-#define AR_MCI_DBG_CNT_CTRL_ENABLE_S   0
-
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/reg_aic.h b/drivers/net/wireless/ath/ath9k/reg_aic.h
new file mode 100644
index 000000000000..955147ab48a2
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/reg_aic.h
@@ -0,0 +1,168 @@
+/*
+ * Copyright (c) 2015 Qualcomm Atheros Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef REG_AIC_H
+#define REG_AIC_H
+
+#define AR_SM_BASE                              0xa200
+#define AR_SM1_BASE                             0xb200
+#define AR_AGC_BASE                             0x9e00
+
+#define AR_PHY_AIC_CTRL_0_B0                    (AR_SM_BASE + 0x4b0)
+#define AR_PHY_AIC_CTRL_1_B0                    (AR_SM_BASE + 0x4b4)
+#define AR_PHY_AIC_CTRL_2_B0                    (AR_SM_BASE + 0x4b8)
+#define AR_PHY_AIC_CTRL_3_B0                    (AR_SM_BASE + 0x4bc)
+#define AR_PHY_AIC_CTRL_4_B0                    (AR_SM_BASE + 0x4c0)
+
+#define AR_PHY_AIC_STAT_0_B0                    (AR_SM_BASE + 0x4c4)
+#define AR_PHY_AIC_STAT_1_B0                    (AR_SM_BASE + 0x4c8)
+#define AR_PHY_AIC_STAT_2_B0                    (AR_SM_BASE + 0x4cc)
+
+#define AR_PHY_AIC_CTRL_0_B1                    (AR_SM1_BASE + 0x4b0)
+#define AR_PHY_AIC_CTRL_1_B1                    (AR_SM1_BASE + 0x4b4)
+#define AR_PHY_AIC_CTRL_4_B1                    (AR_SM1_BASE + 0x4c0)
+
+#define AR_PHY_AIC_STAT_0_B1                    (AR_SM1_BASE + 0x4c4)
+#define AR_PHY_AIC_STAT_1_B1                    (AR_SM1_BASE + 0x4c8)
+#define AR_PHY_AIC_STAT_2_B1                    (AR_SM1_BASE + 0x4cc)
+
+#define AR_PHY_AIC_SRAM_ADDR_B0                 (AR_SM_BASE + 0x5f0)
+#define AR_PHY_AIC_SRAM_DATA_B0                 (AR_SM_BASE + 0x5f4)
+
+#define AR_PHY_AIC_SRAM_ADDR_B1                 (AR_SM1_BASE + 0x5f0)
+#define AR_PHY_AIC_SRAM_DATA_B1                 (AR_SM1_BASE + 0x5f4)
+
+#define AR_PHY_BT_COEX_4                        (AR_AGC_BASE + 0x60)
+#define AR_PHY_BT_COEX_5                        (AR_AGC_BASE + 0x64)
+
+/* AIC fields */
+#define AR_PHY_AIC_MON_ENABLE                   0x80000000
+#define AR_PHY_AIC_MON_ENABLE_S                 31
+#define AR_PHY_AIC_CAL_MAX_HOP_COUNT            0x7F000000
+#define AR_PHY_AIC_CAL_MAX_HOP_COUNT_S          24
+#define AR_PHY_AIC_CAL_MIN_VALID_COUNT          0x00FE0000
+#define AR_PHY_AIC_CAL_MIN_VALID_COUNT_S        17
+#define AR_PHY_AIC_F_WLAN                       0x0001FC00
+#define AR_PHY_AIC_F_WLAN_S                     10
+#define AR_PHY_AIC_CAL_CH_VALID_RESET           0x00000200
+#define AR_PHY_AIC_CAL_CH_VALID_RESET_S         9
+#define AR_PHY_AIC_CAL_ENABLE                   0x00000100
+#define AR_PHY_AIC_CAL_ENABLE_S                 8
+#define AR_PHY_AIC_BTTX_PWR_THR                 0x000000FE
+#define AR_PHY_AIC_BTTX_PWR_THR_S               1
+#define AR_PHY_AIC_ENABLE                       0x00000001
+#define AR_PHY_AIC_ENABLE_S                     0
+#define AR_PHY_AIC_CAL_BT_REF_DELAY             0x00F00000
+#define AR_PHY_AIC_CAL_BT_REF_DELAY_S           20
+#define AR_PHY_AIC_BT_IDLE_CFG                  0x00080000
+#define AR_PHY_AIC_BT_IDLE_CFG_S                19
+#define AR_PHY_AIC_STDBY_COND                   0x00060000
+#define AR_PHY_AIC_STDBY_COND_S                 17
+#define AR_PHY_AIC_STDBY_ROT_ATT_DB             0x0001F800
+#define AR_PHY_AIC_STDBY_ROT_ATT_DB_S           11
+#define AR_PHY_AIC_STDBY_COM_ATT_DB             0x00000700
+#define AR_PHY_AIC_STDBY_COM_ATT_DB_S           8
+#define AR_PHY_AIC_RSSI_MAX                     0x000000F0
+#define AR_PHY_AIC_RSSI_MAX_S                   4
+#define AR_PHY_AIC_RSSI_MIN                     0x0000000F
+#define AR_PHY_AIC_RSSI_MIN_S                   0
+#define AR_PHY_AIC_RADIO_DELAY                  0x7F000000
+#define AR_PHY_AIC_RADIO_DELAY_S                24
+#define AR_PHY_AIC_CAL_STEP_SIZE_CORR           0x00F00000
+#define AR_PHY_AIC_CAL_STEP_SIZE_CORR_S         20
+#define AR_PHY_AIC_CAL_ROT_IDX_CORR             0x000F8000
+#define AR_PHY_AIC_CAL_ROT_IDX_CORR_S           15
+#define AR_PHY_AIC_CAL_CONV_CHECK_FACTOR        0x00006000
+#define AR_PHY_AIC_CAL_CONV_CHECK_FACTOR_S      13
+#define AR_PHY_AIC_ROT_IDX_COUNT_MAX            0x00001C00
+#define AR_PHY_AIC_ROT_IDX_COUNT_MAX_S          10
+#define AR_PHY_AIC_CAL_SYNTH_TOGGLE             0x00000200
+#define AR_PHY_AIC_CAL_SYNTH_TOGGLE_S           9
+#define AR_PHY_AIC_CAL_SYNTH_AFTER_BTRX         0x00000100
+#define AR_PHY_AIC_CAL_SYNTH_AFTER_BTRX_S       8
+#define AR_PHY_AIC_CAL_SYNTH_SETTLING           0x000000FF
+#define AR_PHY_AIC_CAL_SYNTH_SETTLING_S         0
+#define AR_PHY_AIC_MON_MAX_HOP_COUNT            0x07F00000
+#define AR_PHY_AIC_MON_MAX_HOP_COUNT_S          20
+#define AR_PHY_AIC_MON_MIN_STALE_COUNT          0x000FE000
+#define AR_PHY_AIC_MON_MIN_STALE_COUNT_S        13
+#define AR_PHY_AIC_MON_PWR_EST_LONG             0x00001000
+#define AR_PHY_AIC_MON_PWR_EST_LONG_S           12
+#define AR_PHY_AIC_MON_PD_TALLY_SCALING         0x00000C00
+#define AR_PHY_AIC_MON_PD_TALLY_SCALING_S       10
+#define AR_PHY_AIC_MON_PERF_THR                 0x000003E0
+#define AR_PHY_AIC_MON_PERF_THR_S               5
+#define AR_PHY_AIC_CAL_TARGET_MAG_SETTING       0x00000018
+#define AR_PHY_AIC_CAL_TARGET_MAG_SETTING_S     3
+#define AR_PHY_AIC_CAL_PERF_CHECK_FACTOR        0x00000006
+#define AR_PHY_AIC_CAL_PERF_CHECK_FACTOR_S      1
+#define AR_PHY_AIC_CAL_PWR_EST_LONG             0x00000001
+#define AR_PHY_AIC_CAL_PWR_EST_LONG_S           0
+#define AR_PHY_AIC_MON_DONE                     0x80000000
+#define AR_PHY_AIC_MON_DONE_S                   31
+#define AR_PHY_AIC_MON_ACTIVE                   0x40000000
+#define AR_PHY_AIC_MON_ACTIVE_S                 30
+#define AR_PHY_AIC_MEAS_COUNT                   0x3F000000
+#define AR_PHY_AIC_MEAS_COUNT_S                 24
+#define AR_PHY_AIC_CAL_ANT_ISO_EST              0x00FC0000
+#define AR_PHY_AIC_CAL_ANT_ISO_EST_S            18
+#define AR_PHY_AIC_CAL_HOP_COUNT                0x0003F800
+#define AR_PHY_AIC_CAL_HOP_COUNT_S              11
+#define AR_PHY_AIC_CAL_VALID_COUNT              0x000007F0
+#define AR_PHY_AIC_CAL_VALID_COUNT_S            4
+#define AR_PHY_AIC_CAL_BT_TOO_WEAK_ERR          0x00000008
+#define AR_PHY_AIC_CAL_BT_TOO_WEAK_ERR_S        3
+#define AR_PHY_AIC_CAL_BT_TOO_STRONG_ERR        0x00000004
+#define AR_PHY_AIC_CAL_BT_TOO_STRONG_ERR_S      2
+#define AR_PHY_AIC_CAL_DONE                     0x00000002
+#define AR_PHY_AIC_CAL_DONE_S                   1
+#define AR_PHY_AIC_CAL_ACTIVE                   0x00000001
+#define AR_PHY_AIC_CAL_ACTIVE_S                 0
+
+#define AR_PHY_AIC_MEAS_MAG_MIN                 0xFFC00000
+#define AR_PHY_AIC_MEAS_MAG_MIN_S               22
+#define AR_PHY_AIC_MON_STALE_COUNT              0x003F8000
+#define AR_PHY_AIC_MON_STALE_COUNT_S            15
+#define AR_PHY_AIC_MON_HOP_COUNT                0x00007F00
+#define AR_PHY_AIC_MON_HOP_COUNT_S              8
+#define AR_PHY_AIC_CAL_AIC_SM                   0x000000F8
+#define AR_PHY_AIC_CAL_AIC_SM_S                 3
+#define AR_PHY_AIC_SM                           0x00000007
+#define AR_PHY_AIC_SM_S                         0
+#define AR_PHY_AIC_SRAM_VALID                   0x00000001
+#define AR_PHY_AIC_SRAM_VALID_S                 0
+#define AR_PHY_AIC_SRAM_ROT_QUAD_ATT_DB         0x0000007E
+#define AR_PHY_AIC_SRAM_ROT_QUAD_ATT_DB_S       1
+#define AR_PHY_AIC_SRAM_VGA_QUAD_SIGN           0x00000080
+#define AR_PHY_AIC_SRAM_VGA_QUAD_SIGN_S         7
+#define AR_PHY_AIC_SRAM_ROT_DIR_ATT_DB          0x00003F00
+#define AR_PHY_AIC_SRAM_ROT_DIR_ATT_DB_S        8
+#define AR_PHY_AIC_SRAM_VGA_DIR_SIGN            0x00004000
+#define AR_PHY_AIC_SRAM_VGA_DIR_SIGN_S          14
+#define AR_PHY_AIC_SRAM_COM_ATT_6DB             0x00038000
+#define AR_PHY_AIC_SRAM_COM_ATT_6DB_S           15
+#define AR_PHY_AIC_CAL_ROT_ATT_DB_EST_ISO       0x0000E000
+#define AR_PHY_AIC_CAL_ROT_ATT_DB_EST_ISO_S     13
+#define AR_PHY_AIC_CAL_COM_ATT_DB_EST_ISO       0x00001E00
+#define AR_PHY_AIC_CAL_COM_ATT_DB_EST_ISO_S     9
+#define AR_PHY_AIC_CAL_ISO_EST_INIT_SETTING     0x000001F8
+#define AR_PHY_AIC_CAL_ISO_EST_INIT_SETTING_S   3
+#define AR_PHY_AIC_CAL_COM_ATT_DB_BACKOFF       0x00000006
+#define AR_PHY_AIC_CAL_COM_ATT_DB_BACKOFF_S     1
+#define AR_PHY_AIC_CAL_COM_ATT_DB_FIXED         0x00000001
+#define AR_PHY_AIC_CAL_COM_ATT_DB_FIXED_S       0
+
+#endif /* REG_AIC_H */
diff --git a/drivers/net/wireless/ath/ath9k/reg_mci.h b/drivers/net/wireless/ath/ath9k/reg_mci.h
new file mode 100644
index 000000000000..6251310704e3
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/reg_mci.h
@@ -0,0 +1,310 @@
+/*
+ * Copyright (c) 2015 Qualcomm Atheros Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef REG_MCI_H
+#define REG_MCI_H
+
+#define AR_MCI_COMMAND0                                 0x1800
+#define AR_MCI_COMMAND0_HEADER                          0xFF
+#define AR_MCI_COMMAND0_HEADER_S                        0
+#define AR_MCI_COMMAND0_LEN                             0x1f00
+#define AR_MCI_COMMAND0_LEN_S                           8
+#define AR_MCI_COMMAND0_DISABLE_TIMESTAMP               0x2000
+#define AR_MCI_COMMAND0_DISABLE_TIMESTAMP_S             13
+
+#define AR_MCI_COMMAND1                                 0x1804
+
+#define AR_MCI_COMMAND2                                 0x1808
+#define AR_MCI_COMMAND2_RESET_TX                        0x01
+#define AR_MCI_COMMAND2_RESET_TX_S                      0
+#define AR_MCI_COMMAND2_RESET_RX                        0x02
+#define AR_MCI_COMMAND2_RESET_RX_S                      1
+#define AR_MCI_COMMAND2_RESET_RX_NUM_CYCLES             0x3FC
+#define AR_MCI_COMMAND2_RESET_RX_NUM_CYCLES_S           2
+#define AR_MCI_COMMAND2_RESET_REQ_WAKEUP                0x400
+#define AR_MCI_COMMAND2_RESET_REQ_WAKEUP_S              10
+
+#define AR_MCI_RX_CTRL                                  0x180c
+
+#define AR_MCI_TX_CTRL                                  0x1810
+/*
+ * 0 = no division,
+ * 1 = divide by 2,
+ * 2 = divide by 4,
+ * 3 = divide by 8
+ */
+#define AR_MCI_TX_CTRL_CLK_DIV                          0x03
+#define AR_MCI_TX_CTRL_CLK_DIV_S                        0
+#define AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE               0x04
+#define AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE_S             2
+#define AR_MCI_TX_CTRL_GAIN_UPDATE_FREQ                 0xFFFFF8
+#define AR_MCI_TX_CTRL_GAIN_UPDATE_FREQ_S               3
+#define AR_MCI_TX_CTRL_GAIN_UPDATE_NUM                  0xF000000
+#define AR_MCI_TX_CTRL_GAIN_UPDATE_NUM_S                24
+
+#define AR_MCI_MSG_ATTRIBUTES_TABLE                     0x1814
+#define AR_MCI_MSG_ATTRIBUTES_TABLE_CHECKSUM            0xFFFF
+#define AR_MCI_MSG_ATTRIBUTES_TABLE_CHECKSUM_S          0
+#define AR_MCI_MSG_ATTRIBUTES_TABLE_INVALID_HDR         0xFFFF0000
+#define AR_MCI_MSG_ATTRIBUTES_TABLE_INVALID_HDR_S       16
+
+#define AR_MCI_SCHD_TABLE_0                             0x1818
+#define AR_MCI_SCHD_TABLE_1                             0x181c
+#define AR_MCI_GPM_0                                    0x1820
+#define AR_MCI_GPM_1                                    0x1824
+#define AR_MCI_GPM_WRITE_PTR                            0xFFFF0000
+#define AR_MCI_GPM_WRITE_PTR_S                          16
+#define AR_MCI_GPM_BUF_LEN                              0x0000FFFF
+#define AR_MCI_GPM_BUF_LEN_S                            0
+
+#define AR_MCI_INTERRUPT_RAW                            0x1828
+
+#define AR_MCI_INTERRUPT_EN                             0x182c
+#define AR_MCI_INTERRUPT_SW_MSG_DONE                    0x00000001
+#define AR_MCI_INTERRUPT_SW_MSG_DONE_S                  0
+#define AR_MCI_INTERRUPT_CPU_INT_MSG                    0x00000002
+#define AR_MCI_INTERRUPT_CPU_INT_MSG_S                  1
+#define AR_MCI_INTERRUPT_RX_CKSUM_FAIL                  0x00000004
+#define AR_MCI_INTERRUPT_RX_CKSUM_FAIL_S                2
+#define AR_MCI_INTERRUPT_RX_INVALID_HDR                 0x00000008
+#define AR_MCI_INTERRUPT_RX_INVALID_HDR_S               3
+#define AR_MCI_INTERRUPT_RX_HW_MSG_FAIL                 0x00000010
+#define AR_MCI_INTERRUPT_RX_HW_MSG_FAIL_S               4
+#define AR_MCI_INTERRUPT_RX_SW_MSG_FAIL                 0x00000020
+#define AR_MCI_INTERRUPT_RX_SW_MSG_FAIL_S               5
+#define AR_MCI_INTERRUPT_TX_HW_MSG_FAIL                 0x00000080
+#define AR_MCI_INTERRUPT_TX_HW_MSG_FAIL_S               7
+#define AR_MCI_INTERRUPT_TX_SW_MSG_FAIL                 0x00000100
+#define AR_MCI_INTERRUPT_TX_SW_MSG_FAIL_S               8
+#define AR_MCI_INTERRUPT_RX_MSG                         0x00000200
+#define AR_MCI_INTERRUPT_RX_MSG_S                       9
+#define AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE            0x00000400
+#define AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE_S          10
+#define AR_MCI_INTERRUPT_BT_PRI                         0x07fff800
+#define AR_MCI_INTERRUPT_BT_PRI_S                       11
+#define AR_MCI_INTERRUPT_BT_PRI_THRESH                  0x08000000
+#define AR_MCI_INTERRUPT_BT_PRI_THRESH_S                27
+#define AR_MCI_INTERRUPT_BT_FREQ                        0x10000000
+#define AR_MCI_INTERRUPT_BT_FREQ_S                      28
+#define AR_MCI_INTERRUPT_BT_STOMP                       0x20000000
+#define AR_MCI_INTERRUPT_BT_STOMP_S                     29
+#define AR_MCI_INTERRUPT_BB_AIC_IRQ                     0x40000000
+#define AR_MCI_INTERRUPT_BB_AIC_IRQ_S                   30
+#define AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT              0x80000000
+#define AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT_S            31
+
+#define AR_MCI_REMOTE_CPU_INT                           0x1830
+#define AR_MCI_REMOTE_CPU_INT_EN                        0x1834
+#define AR_MCI_INTERRUPT_RX_MSG_RAW                     0x1838
+#define AR_MCI_INTERRUPT_RX_MSG_EN                      0x183c
+#define AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET            0x00000001
+#define AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET_S          0
+#define AR_MCI_INTERRUPT_RX_MSG_LNA_CONTROL             0x00000002
+#define AR_MCI_INTERRUPT_RX_MSG_LNA_CONTROL_S           1
+#define AR_MCI_INTERRUPT_RX_MSG_CONT_NACK               0x00000004
+#define AR_MCI_INTERRUPT_RX_MSG_CONT_NACK_S             2
+#define AR_MCI_INTERRUPT_RX_MSG_CONT_INFO               0x00000008
+#define AR_MCI_INTERRUPT_RX_MSG_CONT_INFO_S             3
+#define AR_MCI_INTERRUPT_RX_MSG_CONT_RST                0x00000010
+#define AR_MCI_INTERRUPT_RX_MSG_CONT_RST_S              4
+#define AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO               0x00000020
+#define AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO_S             5
+#define AR_MCI_INTERRUPT_RX_MSG_CPU_INT                 0x00000040
+#define AR_MCI_INTERRUPT_RX_MSG_CPU_INT_S               6
+#define AR_MCI_INTERRUPT_RX_MSG_GPM                     0x00000100
+#define AR_MCI_INTERRUPT_RX_MSG_GPM_S                   8
+#define AR_MCI_INTERRUPT_RX_MSG_LNA_INFO                0x00000200
+#define AR_MCI_INTERRUPT_RX_MSG_LNA_INFO_S              9
+#define AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING            0x00000400
+#define AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING_S          10
+#define AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING              0x00000800
+#define AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING_S            11
+#define AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE                0x00001000
+#define AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE_S              12
+
+#define AR_MCI_CPU_INT                                  0x1840
+
+#define AR_MCI_RX_STATUS                                0x1844
+#define AR_MCI_RX_LAST_SCHD_MSG_INDEX                   0x00000F00
+#define AR_MCI_RX_LAST_SCHD_MSG_INDEX_S                 8
+#define AR_MCI_RX_REMOTE_SLEEP                          0x00001000
+#define AR_MCI_RX_REMOTE_SLEEP_S                        12
+#define AR_MCI_RX_MCI_CLK_REQ                           0x00002000
+#define AR_MCI_RX_MCI_CLK_REQ_S                         13
+
+#define AR_MCI_CONT_STATUS                              0x1848
+#define AR_MCI_CONT_RSSI_POWER                          0x000000FF
+#define AR_MCI_CONT_RSSI_POWER_S                        0
+#define AR_MCI_CONT_PRIORITY                            0x0000FF00
+#define AR_MCI_CONT_PRIORITY_S                          8
+#define AR_MCI_CONT_TXRX                                0x00010000
+#define AR_MCI_CONT_TXRX_S                              16
+
+#define AR_MCI_BT_PRI0                                  0x184c
+#define AR_MCI_BT_PRI1                                  0x1850
+#define AR_MCI_BT_PRI2                                  0x1854
+#define AR_MCI_BT_PRI3                                  0x1858
+#define AR_MCI_BT_PRI                                   0x185c
+#define AR_MCI_WL_FREQ0                                 0x1860
+#define AR_MCI_WL_FREQ1                                 0x1864
+#define AR_MCI_WL_FREQ2                                 0x1868
+#define AR_MCI_GAIN                                     0x186c
+#define AR_MCI_WBTIMER1                                 0x1870
+#define AR_MCI_WBTIMER2                                 0x1874
+#define AR_MCI_WBTIMER3                                 0x1878
+#define AR_MCI_WBTIMER4                                 0x187c
+#define AR_MCI_MAXGAIN                                  0x1880
+#define AR_MCI_HW_SCHD_TBL_CTL                          0x1884
+#define AR_MCI_HW_SCHD_TBL_D0                           0x1888
+#define AR_MCI_HW_SCHD_TBL_D1                           0x188c
+#define AR_MCI_HW_SCHD_TBL_D2                           0x1890
+#define AR_MCI_HW_SCHD_TBL_D3                           0x1894
+#define AR_MCI_TX_PAYLOAD0                              0x1898
+#define AR_MCI_TX_PAYLOAD1                              0x189c
+#define AR_MCI_TX_PAYLOAD2                              0x18a0
+#define AR_MCI_TX_PAYLOAD3                              0x18a4
+#define AR_BTCOEX_WBTIMER                               0x18a8
+
+#define AR_BTCOEX_CTRL                                  0x18ac
+#define AR_BTCOEX_CTRL_AR9462_MODE                      0x00000001
+#define AR_BTCOEX_CTRL_AR9462_MODE_S                    0
+#define AR_BTCOEX_CTRL_WBTIMER_EN                       0x00000002
+#define AR_BTCOEX_CTRL_WBTIMER_EN_S                     1
+#define AR_BTCOEX_CTRL_MCI_MODE_EN                      0x00000004
+#define AR_BTCOEX_CTRL_MCI_MODE_EN_S                    2
+#define AR_BTCOEX_CTRL_LNA_SHARED                       0x00000008
+#define AR_BTCOEX_CTRL_LNA_SHARED_S                     3
+#define AR_BTCOEX_CTRL_PA_SHARED                        0x00000010
+#define AR_BTCOEX_CTRL_PA_SHARED_S                      4
+#define AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN           0x00000020
+#define AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN_S         5
+#define AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN        0x00000040
+#define AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN_S      6
+#define AR_BTCOEX_CTRL_NUM_ANTENNAS                     0x00000180
+#define AR_BTCOEX_CTRL_NUM_ANTENNAS_S                   7
+#define AR_BTCOEX_CTRL_RX_CHAIN_MASK                    0x00000E00
+#define AR_BTCOEX_CTRL_RX_CHAIN_MASK_S                  9
+#define AR_BTCOEX_CTRL_AGGR_THRESH                      0x00007000
+#define AR_BTCOEX_CTRL_AGGR_THRESH_S                    12
+#define AR_BTCOEX_CTRL_1_CHAIN_BCN                      0x00080000
+#define AR_BTCOEX_CTRL_1_CHAIN_BCN_S                    19
+#define AR_BTCOEX_CTRL_1_CHAIN_ACK                      0x00100000
+#define AR_BTCOEX_CTRL_1_CHAIN_ACK_S                    20
+#define AR_BTCOEX_CTRL_WAIT_BA_MARGIN                   0x1FE00000
+#define AR_BTCOEX_CTRL_WAIT_BA_MARGIN_S                 28
+#define AR_BTCOEX_CTRL_REDUCE_TXPWR                     0x20000000
+#define AR_BTCOEX_CTRL_REDUCE_TXPWR_S                   29
+#define AR_BTCOEX_CTRL_SPDT_ENABLE_10                   0x40000000
+#define AR_BTCOEX_CTRL_SPDT_ENABLE_10_S                 30
+#define AR_BTCOEX_CTRL_SPDT_POLARITY                    0x80000000
+#define AR_BTCOEX_CTRL_SPDT_POLARITY_S                  31
+
+#define AR_BTCOEX_WL_WEIGHTS0                           0x18b0
+#define AR_BTCOEX_WL_WEIGHTS1                           0x18b4
+#define AR_BTCOEX_WL_WEIGHTS2                           0x18b8
+#define AR_BTCOEX_WL_WEIGHTS3                           0x18bc
+
+#define AR_BTCOEX_MAX_TXPWR(_x)                         (0x18c0 + ((_x) << 2))
+#define AR_BTCOEX_WL_LNA                                0x1940
+#define AR_BTCOEX_RFGAIN_CTRL                           0x1944
+#define AR_BTCOEX_WL_LNA_TIMEOUT                        0x003FFFFF
+#define AR_BTCOEX_WL_LNA_TIMEOUT_S                      0
+
+#define AR_BTCOEX_CTRL2                                 0x1948
+#define AR_BTCOEX_CTRL2_TXPWR_THRESH                    0x0007F800
+#define AR_BTCOEX_CTRL2_TXPWR_THRESH_S                  11
+#define AR_BTCOEX_CTRL2_TX_CHAIN_MASK                   0x00380000
+#define AR_BTCOEX_CTRL2_TX_CHAIN_MASK_S                 19
+#define AR_BTCOEX_CTRL2_RX_DEWEIGHT                     0x00400000
+#define AR_BTCOEX_CTRL2_RX_DEWEIGHT_S                   22
+#define AR_BTCOEX_CTRL2_GPIO_OBS_SEL                    0x00800000
+#define AR_BTCOEX_CTRL2_GPIO_OBS_SEL_S                  23
+#define AR_BTCOEX_CTRL2_MAC_BB_OBS_SEL                  0x01000000
+#define AR_BTCOEX_CTRL2_MAC_BB_OBS_SEL_S                24
+#define AR_BTCOEX_CTRL2_DESC_BASED_TXPWR_ENABLE         0x02000000
+#define AR_BTCOEX_CTRL2_DESC_BASED_TXPWR_ENABLE_S       25
+
+#define AR_BTCOEX_CTRL_SPDT_ENABLE                      0x00000001
+#define AR_BTCOEX_CTRL_SPDT_ENABLE_S                    0
+#define AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL                 0x00000002
+#define AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL_S               1
+#define AR_BTCOEX_CTRL_USE_LATCHED_BT_ANT               0x00000004
+#define AR_BTCOEX_CTRL_USE_LATCHED_BT_ANT_S             2
+#define AR_GLB_WLAN_UART_INTF_EN                        0x00020000
+#define AR_GLB_WLAN_UART_INTF_EN_S                      17
+#define AR_GLB_DS_JTAG_DISABLE                          0x00040000
+#define AR_GLB_DS_JTAG_DISABLE_S                        18
+
+#define AR_BTCOEX_RC                                    0x194c
+#define AR_BTCOEX_MAX_RFGAIN(_x)                        (0x1950 + ((_x) << 2))
+#define AR_BTCOEX_DBG                                   0x1a50
+#define AR_MCI_LAST_HW_MSG_HDR                          0x1a54
+#define AR_MCI_LAST_HW_MSG_BDY                          0x1a58
+
+#define AR_MCI_SCHD_TABLE_2                             0x1a5c
+#define AR_MCI_SCHD_TABLE_2_MEM_BASED                   0x00000001
+#define AR_MCI_SCHD_TABLE_2_MEM_BASED_S                 0
+#define AR_MCI_SCHD_TABLE_2_HW_BASED                    0x00000002
+#define AR_MCI_SCHD_TABLE_2_HW_BASED_S                  1
+
+#define AR_BTCOEX_CTRL3                                 0x1a60
+#define AR_BTCOEX_CTRL3_CONT_INFO_TIMEOUT               0x00000fff
+#define AR_BTCOEX_CTRL3_CONT_INFO_TIMEOUT_S             0
+
+#define AR_GLB_SWREG_DISCONT_MODE                       0x2002c
+#define AR_GLB_SWREG_DISCONT_EN_BT_WLAN                 0x3
+
+#define AR_MCI_MISC                                     0x1a74
+#define AR_MCI_MISC_HW_FIX_EN                           0x00000001
+#define AR_MCI_MISC_HW_FIX_EN_S                         0
+
+#define AR_MCI_DBG_CNT_CTRL                             0x1a78
+#define AR_MCI_DBG_CNT_CTRL_ENABLE                      0x00000001
+#define AR_MCI_DBG_CNT_CTRL_ENABLE_S                    0
+#define AR_MCI_DBG_CNT_CTRL_BT_LINKID                   0x000007f8
+#define AR_MCI_DBG_CNT_CTRL_BT_LINKID_S                 3
+
+#define MCI_STAT_ALL_BT_LINKID                          0xffff
+
+#define AR_MCI_INTERRUPT_DEFAULT (AR_MCI_INTERRUPT_SW_MSG_DONE         | \
+				  AR_MCI_INTERRUPT_RX_INVALID_HDR      | \
+				  AR_MCI_INTERRUPT_RX_HW_MSG_FAIL      | \
+				  AR_MCI_INTERRUPT_RX_SW_MSG_FAIL      | \
+				  AR_MCI_INTERRUPT_TX_HW_MSG_FAIL      | \
+				  AR_MCI_INTERRUPT_TX_SW_MSG_FAIL      | \
+				  AR_MCI_INTERRUPT_RX_MSG              | \
+				  AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE | \
+				  AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT)
+
+#define AR_MCI_INTERRUPT_MSG_FAIL_MASK (AR_MCI_INTERRUPT_RX_HW_MSG_FAIL | \
+                                        AR_MCI_INTERRUPT_RX_SW_MSG_FAIL | \
+                                        AR_MCI_INTERRUPT_TX_HW_MSG_FAIL | \
+                                        AR_MCI_INTERRUPT_TX_SW_MSG_FAIL)
+
+#define AR_MCI_INTERRUPT_RX_HW_MSG_MASK (AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO   | \
+					 AR_MCI_INTERRUPT_RX_MSG_LNA_CONTROL | \
+					 AR_MCI_INTERRUPT_RX_MSG_LNA_INFO    | \
+					 AR_MCI_INTERRUPT_RX_MSG_CONT_NACK   | \
+					 AR_MCI_INTERRUPT_RX_MSG_CONT_INFO   | \
+					 AR_MCI_INTERRUPT_RX_MSG_CONT_RST)
+
+#define AR_MCI_INTERRUPT_RX_MSG_DEFAULT (AR_MCI_INTERRUPT_RX_MSG_GPM           | \
+                                         AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET  | \
+                                         AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING    | \
+                                         AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING  | \
+                                         AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE)
+
+#endif /* REG_MCI_H */
diff --git a/drivers/net/wireless/ath/ath9k/reg_wow.h b/drivers/net/wireless/ath/ath9k/reg_wow.h
index 3abfca56ca58..453054078cc4 100644
--- a/drivers/net/wireless/ath/ath9k/reg_wow.h
+++ b/drivers/net/wireless/ath/ath9k/reg_wow.h
@@ -72,7 +72,7 @@
 #define AR_WOW_MAC_INTR_EN              0x00040000
 #define AR_WOW_MAGIC_EN                 0x00010000
 #define AR_WOW_PATTERN_EN(x)            (x & 0xff)
-#define AR_WOW_PAT_FOUND_SHIFT  8
+#define AR_WOW_PAT_FOUND_SHIFT          8
 #define AR_WOW_PATTERN_FOUND(x)         (x & (0xff << AR_WOW_PAT_FOUND_SHIFT))
 #define AR_WOW_PATTERN_FOUND_MASK       ((0xff) << AR_WOW_PAT_FOUND_SHIFT)
 #define AR_WOW_MAGIC_PAT_FOUND          0x00020000
@@ -90,6 +90,14 @@
                                                AR_WOW_BEACON_FAIL |	\
                                                AR_WOW_KEEP_ALIVE_FAIL))
 
+#define AR_WOW2_PATTERN_EN(x)           ((x & 0xff) << 0)
+#define AR_WOW2_PATTERN_FOUND_SHIFT     8
+#define AR_WOW2_PATTERN_FOUND(x)        (x & (0xff << AR_WOW2_PATTERN_FOUND_SHIFT))
+#define AR_WOW2_PATTERN_FOUND_MASK      ((0xff) << AR_WOW2_PATTERN_FOUND_SHIFT)
+
+#define AR_WOW_STATUS2(x)               (x & AR_WOW2_PATTERN_FOUND_MASK)
+#define AR_WOW_CLEAR_EVENTS2(x)         (x & ~(AR_WOW2_PATTERN_EN(0xff)))
+
 #define AR_WOW_AIFS_CNT(x)              (x & 0xff)
 #define AR_WOW_SLOT_CNT(x)              ((x & 0xff) << 8)
 #define AR_WOW_KEEP_ALIVE_CNT(x)        ((x & 0xff) << 16)
diff --git a/drivers/net/wireless/ath/ath9k/wmi.c b/drivers/net/wireless/ath/ath9k/wmi.c
index 65c8894c5f81..ca533b4321bd 100644
--- a/drivers/net/wireless/ath/ath9k/wmi.c
+++ b/drivers/net/wireless/ath/ath9k/wmi.c
@@ -61,6 +61,8 @@ static const char *wmi_cmd_to_name(enum wmi_cmd_id wmi_cmd)
 		return "WMI_REG_READ_CMDID";
 	case WMI_REG_WRITE_CMDID:
 		return "WMI_REG_WRITE_CMDID";
+	case WMI_REG_RMW_CMDID:
+		return "WMI_REG_RMW_CMDID";
 	case WMI_RC_STATE_CHANGE_CMDID:
 		return "WMI_RC_STATE_CHANGE_CMDID";
 	case WMI_RC_RATE_UPDATE_CMDID:
@@ -101,6 +103,7 @@ struct wmi *ath9k_init_wmi(struct ath9k_htc_priv *priv)
 	spin_lock_init(&wmi->event_lock);
 	mutex_init(&wmi->op_mutex);
 	mutex_init(&wmi->multi_write_mutex);
+	mutex_init(&wmi->multi_rmw_mutex);
 	init_completion(&wmi->cmd_wait);
 	INIT_LIST_HEAD(&wmi->pending_tx_events);
 	tasklet_init(&wmi->wmi_event_tasklet, ath9k_wmi_event_tasklet,
@@ -224,7 +227,7 @@ static void ath9k_wmi_ctrl_rx(void *priv, struct sk_buff *skb,
 
 	/* Check if there has been a timeout. */
 	spin_lock(&wmi->wmi_lock);
-	if (cmd_id != wmi->last_cmd_id) {
+	if (be16_to_cpu(hdr->seq_no) != wmi->last_seq_id) {
 		spin_unlock(&wmi->wmi_lock);
 		goto free_skb;
 	}
@@ -272,11 +275,16 @@ static int ath9k_wmi_cmd_issue(struct wmi *wmi,
 			       enum wmi_cmd_id cmd, u16 len)
 {
 	struct wmi_cmd_hdr *hdr;
+	unsigned long flags;
 
 	hdr = (struct wmi_cmd_hdr *) skb_push(skb, sizeof(struct wmi_cmd_hdr));
 	hdr->command_id = cpu_to_be16(cmd);
 	hdr->seq_no = cpu_to_be16(++wmi->tx_seq_id);
 
+	spin_lock_irqsave(&wmi->wmi_lock, flags);
+	wmi->last_seq_id = wmi->tx_seq_id;
+	spin_unlock_irqrestore(&wmi->wmi_lock, flags);
+
 	return htc_send_epid(wmi->htc, skb, wmi->ctrl_epid);
 }
 
@@ -292,7 +300,6 @@ int ath9k_wmi_cmd(struct wmi *wmi, enum wmi_cmd_id cmd_id,
 	struct sk_buff *skb;
 	u8 *data;
 	int time_left, ret = 0;
-	unsigned long flags;
 
 	if (ah->ah_flags & AH_UNPLUGGED)
 		return 0;
@@ -320,10 +327,6 @@ int ath9k_wmi_cmd(struct wmi *wmi, enum wmi_cmd_id cmd_id,
 	wmi->cmd_rsp_buf = rsp_buf;
 	wmi->cmd_rsp_len = rsp_len;
 
-	spin_lock_irqsave(&wmi->wmi_lock, flags);
-	wmi->last_cmd_id = cmd_id;
-	spin_unlock_irqrestore(&wmi->wmi_lock, flags);
-
 	ret = ath9k_wmi_cmd_issue(wmi, skb, cmd_id, cmd_len);
 	if (ret)
 		goto out;
diff --git a/drivers/net/wireless/ath/ath9k/wmi.h b/drivers/net/wireless/ath/ath9k/wmi.h
index 0db37f230018..380175d5ecd7 100644
--- a/drivers/net/wireless/ath/ath9k/wmi.h
+++ b/drivers/net/wireless/ath/ath9k/wmi.h
@@ -112,6 +112,7 @@ enum wmi_cmd_id {
 	WMI_TX_STATS_CMDID,
 	WMI_RX_STATS_CMDID,
 	WMI_BITRATE_MASK_CMDID,
+	WMI_REG_RMW_CMDID,
 };
 
 enum wmi_event_id {
@@ -125,12 +126,19 @@ enum wmi_event_id {
 };
 
 #define MAX_CMD_NUMBER 62
+#define MAX_RMW_CMD_NUMBER 15
 
 struct register_write {
 	__be32 reg;
 	__be32 val;
 };
 
+struct register_rmw {
+	__be32 reg;
+	__be32 set;
+	__be32 clr;
+} __packed;
+
 struct ath9k_htc_tx_event {
 	int count;
 	struct __wmi_event_txstatus txs;
@@ -143,7 +151,7 @@ struct wmi {
 	enum htc_endpoint_id ctrl_epid;
 	struct mutex op_mutex;
 	struct completion cmd_wait;
-	enum wmi_cmd_id last_cmd_id;
+	u16 last_seq_id;
 	struct sk_buff_head wmi_event_queue;
 	struct tasklet_struct wmi_event_tasklet;
 	u16 tx_seq_id;
@@ -156,10 +164,18 @@ struct wmi {
 
 	spinlock_t wmi_lock;
 
+	/* multi write section */
 	atomic_t mwrite_cnt;
 	struct register_write multi_write[MAX_CMD_NUMBER];
 	u32 multi_write_idx;
 	struct mutex multi_write_mutex;
+
+	/* multi rmw section */
+	atomic_t m_rmw_cnt;
+	struct register_rmw multi_rmw[MAX_RMW_CMD_NUMBER];
+	u32 multi_rmw_idx;
+	struct mutex multi_rmw_mutex;
+
 };
 
 struct wmi *ath9k_init_wmi(struct ath9k_htc_priv *priv);
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 1b8e75c4d2c2..0acd079ba96b 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -1103,14 +1103,28 @@ static u8 ath_get_rate_txpower(struct ath_softc *sc, struct ath_buf *bf,
 	struct sk_buff *skb;
 	struct ath_frame_info *fi;
 	struct ieee80211_tx_info *info;
+	struct ieee80211_vif *vif;
 	struct ath_hw *ah = sc->sc_ah;
 
 	if (sc->tx99_state || !ah->tpc_enabled)
 		return MAX_RATE_POWER;
 
 	skb = bf->bf_mpdu;
-	fi = get_frame_info(skb);
 	info = IEEE80211_SKB_CB(skb);
+	vif = info->control.vif;
+
+	if (!vif) {
+		max_power = sc->cur_chan->cur_txpower;
+		goto out;
+	}
+
+	if (vif->bss_conf.txpower_type != NL80211_TX_POWER_LIMITED) {
+		max_power = min_t(u8, sc->cur_chan->cur_txpower,
+				  2 * vif->bss_conf.txpower);
+		goto out;
+	}
+
+	fi = get_frame_info(skb);
 
 	if (!AR_SREV_9300_20_OR_LATER(ah)) {
 		int txpower = fi->tx_power;
@@ -1147,25 +1161,25 @@ static u8 ath_get_rate_txpower(struct ath_softc *sc, struct ath_buf *bf,
 			txpower -= 2;
 
 		txpower = max(txpower, 0);
-		max_power = min_t(u8, ah->tx_power[rateidx], txpower);
-
-		/* XXX: clamp minimum TX power at 1 for AR9160 since if
-		 * max_power is set to 0, frames are transmitted at max
-		 * TX power
-		 */
-		if (!max_power && !AR_SREV_9280_20_OR_LATER(ah))
-			max_power = 1;
+		max_power = min_t(u8, ah->tx_power[rateidx],
+				  2 * vif->bss_conf.txpower);
+		max_power = min_t(u8, max_power, txpower);
 	} else if (!bf->bf_state.bfs_paprd) {
 		if (rateidx < 8 && (info->flags & IEEE80211_TX_CTL_STBC))
-			max_power = min(ah->tx_power_stbc[rateidx],
-					fi->tx_power);
+			max_power = min_t(u8, ah->tx_power_stbc[rateidx],
+					  2 * vif->bss_conf.txpower);
 		else
-			max_power = min(ah->tx_power[rateidx], fi->tx_power);
+			max_power = min_t(u8, ah->tx_power[rateidx],
+					  2 * vif->bss_conf.txpower);
+		max_power = min(max_power, fi->tx_power);
 	} else {
 		max_power = ah->paprd_training_power;
 	}
-
-	return max_power;
+out:
+	/* XXX: clamp minimum TX power at 1 for AR9160 since if max_power
+	 * is set to 0, frames are transmitted at max TX power
+	 */
+	return (!max_power && !AR_SREV_9280_20_OR_LATER(ah)) ? 1 : max_power;
 }
 
 static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
diff --git a/drivers/net/wireless/ath/dfs_pattern_detector.c b/drivers/net/wireless/ath/dfs_pattern_detector.c
index 3d57f8772389..c657ca26a71a 100644
--- a/drivers/net/wireless/ath/dfs_pattern_detector.c
+++ b/drivers/net/wireless/ath/dfs_pattern_detector.c
@@ -289,7 +289,7 @@ dpd_add_pulse(struct dfs_pattern_detector *dpd, struct pulse_event *event)
 				"count=%d, count_false=%d\n",
 				event->freq, pd->rs->type_id,
 				ps->pri, ps->count, ps->count_falses);
-			channel_detector_reset(dpd, cd);
+			pd->reset(pd, dpd->last_pulse_ts);
 			return true;
 		}
 	}
diff --git a/drivers/net/wireless/ath/wcn36xx/smd.c b/drivers/net/wireless/ath/wcn36xx/smd.c
index 69ed39731902..dbd894428be6 100644
--- a/drivers/net/wireless/ath/wcn36xx/smd.c
+++ b/drivers/net/wireless/ath/wcn36xx/smd.c
@@ -1701,7 +1701,7 @@ int wcn36xx_smd_keep_alive_req(struct wcn36xx *wcn,
 	} else if (packet_type == WCN36XX_HAL_KEEP_ALIVE_UNSOLICIT_ARP_RSP) {
 		/* TODO: it also support ARP response type */
 	} else {
-		wcn36xx_warn("unknow keep alive packet type %d\n", packet_type);
+		wcn36xx_warn("unknown keep alive packet type %d\n", packet_type);
 		ret = -EINVAL;
 		goto out;
 	}
diff --git a/drivers/net/wireless/ath/wil6210/cfg80211.c b/drivers/net/wireless/ath/wil6210/cfg80211.c
index 2d5ea21be47e..b97172667bc7 100644
--- a/drivers/net/wireless/ath/wil6210/cfg80211.c
+++ b/drivers/net/wireless/ath/wil6210/cfg80211.c
@@ -14,6 +14,7 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#include <linux/etherdevice.h>
 #include "wil6210.h"
 #include "wmi.h"
 
@@ -217,7 +218,7 @@ static int wil_cfg80211_dump_station(struct wiphy *wiphy,
 	if (cid < 0)
 		return -ENOENT;
 
-	memcpy(mac, wil->sta[cid].addr, ETH_ALEN);
+	ether_addr_copy(mac, wil->sta[cid].addr);
 	wil_dbg_misc(wil, "%s(%pM) CID %d\n", __func__, mac, cid);
 
 	rc = wil_cid_fill_sinfo(wil, cid, sinfo);
@@ -387,15 +388,29 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 	int ch;
 	int rc = 0;
 
+	wil_print_connect_params(wil, sme);
+
 	if (test_bit(wil_status_fwconnecting, wil->status) ||
 	    test_bit(wil_status_fwconnected, wil->status))
 		return -EALREADY;
 
-	wil_print_connect_params(wil, sme);
+	if (sme->ie_len > WMI_MAX_IE_LEN) {
+		wil_err(wil, "IE too large (%td bytes)\n", sme->ie_len);
+		return -ERANGE;
+	}
+
+	rsn_eid = sme->ie ?
+			cfg80211_find_ie(WLAN_EID_RSN, sme->ie, sme->ie_len) :
+			NULL;
+
+	if (sme->privacy && !rsn_eid) {
+		wil_err(wil, "Missing RSN IE for secure connection\n");
+		return -EINVAL;
+	}
 
 	bss = cfg80211_get_bss(wiphy, sme->channel, sme->bssid,
 			       sme->ssid, sme->ssid_len,
-			       WLAN_CAPABILITY_ESS, WLAN_CAPABILITY_ESS);
+			       IEEE80211_BSS_TYPE_ESS, IEEE80211_PRIVACY_ANY);
 	if (!bss) {
 		wil_err(wil, "Unable to find BSS\n");
 		return -ENOENT;
@@ -407,17 +422,9 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 		rc = -ENOENT;
 		goto out;
 	}
+	wil->privacy = sme->privacy;
 
-	rsn_eid = sme->ie ?
-			cfg80211_find_ie(WLAN_EID_RSN, sme->ie, sme->ie_len) :
-			NULL;
-	if (rsn_eid) {
-		if (sme->ie_len > WMI_MAX_IE_LEN) {
-			rc = -ERANGE;
-			wil_err(wil, "IE too large (%td bytes)\n",
-				sme->ie_len);
-			goto out;
-		}
+	if (wil->privacy) {
 		/* For secure assoc, send WMI_DELETE_CIPHER_KEY_CMD */
 		rc = wmi_del_cipher_key(wil, 0, bss->bssid);
 		if (rc) {
@@ -450,7 +457,7 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 			bss->capability);
 		goto out;
 	}
-	if (rsn_eid) {
+	if (wil->privacy) {
 		conn.dot11_auth_mode = WMI_AUTH11_SHARED;
 		conn.auth_mode = WMI_AUTH_WPA2_PSK;
 		conn.pairwise_crypto_type = WMI_CRYPT_AES_GCMP;
@@ -472,8 +479,8 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 	}
 	conn.channel = ch - 1;
 
-	memcpy(conn.bssid, bss->bssid, ETH_ALEN);
-	memcpy(conn.dst_mac, bss->bssid, ETH_ALEN);
+	ether_addr_copy(conn.bssid, bss->bssid);
+	ether_addr_copy(conn.dst_mac, bss->bssid);
 
 	set_bit(wil_status_fwconnecting, wil->status);
 
@@ -769,15 +776,24 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 	wmi_set_ie(wil, WMI_FRAME_ASSOC_RESP, bcon->assocresp_ies_len,
 		   bcon->assocresp_ies);
 
-	wil->secure_pcp = info->privacy;
+	wil->privacy = info->privacy;
 
 	netif_carrier_on(ndev);
 
 	rc = wmi_pcp_start(wil, info->beacon_interval, wmi_nettype,
 			   channel->hw_value);
 	if (rc)
-		netif_carrier_off(ndev);
+		goto err_pcp_start;
+
+	rc = wil_bcast_init(wil);
+	if (rc)
+		goto err_bcast;
 
+	goto out; /* success */
+err_bcast:
+	wmi_pcp_stop(wil);
+err_pcp_start:
+	netif_carrier_off(ndev);
 out:
 	mutex_unlock(&wil->mutex);
 	return rc;
@@ -911,6 +927,21 @@ static int wil_cfg80211_probe_client(struct wiphy *wiphy,
 	return 0;
 }
 
+static int wil_cfg80211_change_bss(struct wiphy *wiphy,
+				   struct net_device *dev,
+				   struct bss_parameters *params)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+
+	if (params->ap_isolate >= 0) {
+		wil_dbg_misc(wil, "%s(ap_isolate %d => %d)\n", __func__,
+			     wil->ap_isolate, params->ap_isolate);
+		wil->ap_isolate = params->ap_isolate;
+	}
+
+	return 0;
+}
+
 static struct cfg80211_ops wil_cfg80211_ops = {
 	.scan = wil_cfg80211_scan,
 	.connect = wil_cfg80211_connect,
@@ -931,6 +962,7 @@ static struct cfg80211_ops wil_cfg80211_ops = {
 	.stop_ap = wil_cfg80211_stop_ap,
 	.del_station = wil_cfg80211_del_station,
 	.probe_client = wil_cfg80211_probe_client,
+	.change_bss = wil_cfg80211_change_bss,
 };
 
 static void wil_wiphy_init(struct wiphy *wiphy)
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 45c3558ec804..bbc22d88f78f 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -29,6 +29,7 @@
 static u32 mem_addr;
 static u32 dbg_txdesc_index;
 static u32 dbg_vring_index; /* 24+ for Rx, 0..23 for Tx */
+u32 vring_idle_trsh = 16; /* HW fetches up to 16 descriptors at once */
 
 enum dbg_off_type {
 	doff_u32 = 0,
@@ -102,23 +103,36 @@ static int wil_vring_debugfs_show(struct seq_file *s, void *data)
 				   % vring->size;
 			int avail = vring->size - used - 1;
 			char name[10];
+			char sidle[10];
 			/* performance monitoring */
 			cycles_t now = get_cycles();
 			uint64_t idle = txdata->idle * 100;
 			uint64_t total = now - txdata->begin;
 
-			do_div(idle, total);
+			if (total != 0) {
+				do_div(idle, total);
+				snprintf(sidle, sizeof(sidle), "%3d%%",
+					 (int)idle);
+			} else {
+				snprintf(sidle, sizeof(sidle), "N/A");
+			}
 			txdata->begin = now;
 			txdata->idle = 0ULL;
 
 			snprintf(name, sizeof(name), "tx_%2d", i);
 
-			seq_printf(s,
-				   "\n%pM CID %d TID %d BACK([%d] %d TU A%s) [%3d|%3d] idle %3d%%\n",
-				   wil->sta[cid].addr, cid, tid,
-				   txdata->agg_wsize, txdata->agg_timeout,
-				   txdata->agg_amsdu ? "+" : "-",
-				   used, avail, (int)idle);
+			if (cid < WIL6210_MAX_CID)
+				seq_printf(s,
+					   "\n%pM CID %d TID %d BACK([%u] %u TU A%s) [%3d|%3d] idle %s\n",
+					   wil->sta[cid].addr, cid, tid,
+					   txdata->agg_wsize,
+					   txdata->agg_timeout,
+					   txdata->agg_amsdu ? "+" : "-",
+					   used, avail, sidle);
+			else
+				seq_printf(s,
+					   "\nBroadcast [%3d|%3d] idle %s\n",
+					   used, avail, sidle);
 
 			wil_print_vring(s, wil, name, vring, '_', 'H');
 		}
@@ -549,7 +563,7 @@ static ssize_t wil_write_file_reset(struct file *file, const char __user *buf,
 	dev_close(ndev);
 	ndev->flags &= ~IFF_UP;
 	rtnl_unlock();
-	wil_reset(wil);
+	wil_reset(wil, true);
 
 	return len;
 }
@@ -618,7 +632,7 @@ static ssize_t wil_write_back(struct file *file, const char __user *buf,
 	struct wil6210_priv *wil = file->private_data;
 	int rc;
 	char *kbuf = kmalloc(len + 1, GFP_KERNEL);
-	char cmd[8];
+	char cmd[9];
 	int p1, p2, p3;
 
 	if (!kbuf)
@@ -1392,11 +1406,12 @@ static void wil6210_debugfs_init_isr(struct wil6210_priv *wil,
 
 /* fields in struct wil6210_priv */
 static const struct dbg_off dbg_wil_off[] = {
-	WIL_FIELD(secure_pcp,	S_IRUGO | S_IWUSR,	doff_u32),
+	WIL_FIELD(privacy,	S_IRUGO,		doff_u32),
 	WIL_FIELD(status[0],	S_IRUGO | S_IWUSR,	doff_ulong),
 	WIL_FIELD(fw_version,	S_IRUGO,		doff_u32),
 	WIL_FIELD(hw_version,	S_IRUGO,		doff_x32),
 	WIL_FIELD(recovery_count, S_IRUGO,		doff_u32),
+	WIL_FIELD(ap_isolate,	S_IRUGO,		doff_u32),
 	{},
 };
 
@@ -1412,6 +1427,8 @@ static const struct dbg_off dbg_statics[] = {
 	{"desc_index",	S_IRUGO | S_IWUSR, (ulong)&dbg_txdesc_index, doff_u32},
 	{"vring_index",	S_IRUGO | S_IWUSR, (ulong)&dbg_vring_index, doff_u32},
 	{"mem_addr",	S_IRUGO | S_IWUSR, (ulong)&mem_addr, doff_u32},
+	{"vring_idle_trsh", S_IRUGO | S_IWUSR, (ulong)&vring_idle_trsh,
+	 doff_u32},
 	{},
 };
 
diff --git a/drivers/net/wireless/ath/wil6210/ethtool.c b/drivers/net/wireless/ath/wil6210/ethtool.c
index 4c44a82c34d7..0ea695ff98ad 100644
--- a/drivers/net/wireless/ath/wil6210/ethtool.c
+++ b/drivers/net/wireless/ath/wil6210/ethtool.c
@@ -50,27 +50,19 @@ static int wil_ethtoolops_get_coalesce(struct net_device *ndev,
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
-	if (test_bit(hw_capability_advanced_itr_moderation,
-		     wil->hw_capabilities)) {
-		tx_itr_en = ioread32(wil->csr +
-				     HOSTADDR(RGF_DMA_ITR_TX_CNT_CTL));
-		if (tx_itr_en & BIT_DMA_ITR_TX_CNT_CTL_EN)
-			tx_itr_val =
-				ioread32(wil->csr +
-					 HOSTADDR(RGF_DMA_ITR_TX_CNT_TRSH));
-
-		rx_itr_en = ioread32(wil->csr +
-				     HOSTADDR(RGF_DMA_ITR_RX_CNT_CTL));
-		if (rx_itr_en & BIT_DMA_ITR_RX_CNT_CTL_EN)
-			rx_itr_val =
-				ioread32(wil->csr +
-					 HOSTADDR(RGF_DMA_ITR_RX_CNT_TRSH));
-	} else {
-		rx_itr_en = ioread32(wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_CRL));
-		if (rx_itr_en & BIT_DMA_ITR_CNT_CRL_EN)
-			rx_itr_val = ioread32(wil->csr +
-					      HOSTADDR(RGF_DMA_ITR_CNT_TRSH));
-	}
+	tx_itr_en = ioread32(wil->csr +
+			     HOSTADDR(RGF_DMA_ITR_TX_CNT_CTL));
+	if (tx_itr_en & BIT_DMA_ITR_TX_CNT_CTL_EN)
+		tx_itr_val =
+			ioread32(wil->csr +
+				 HOSTADDR(RGF_DMA_ITR_TX_CNT_TRSH));
+
+	rx_itr_en = ioread32(wil->csr +
+			     HOSTADDR(RGF_DMA_ITR_RX_CNT_CTL));
+	if (rx_itr_en & BIT_DMA_ITR_RX_CNT_CTL_EN)
+		rx_itr_val =
+			ioread32(wil->csr +
+				 HOSTADDR(RGF_DMA_ITR_RX_CNT_TRSH));
 
 	cp->tx_coalesce_usecs = tx_itr_val;
 	cp->rx_coalesce_usecs = rx_itr_val;
diff --git a/drivers/net/wireless/ath/wil6210/fw.c b/drivers/net/wireless/ath/wil6210/fw.c
index 93c5cc16c515..4428345e5a47 100644
--- a/drivers/net/wireless/ath/wil6210/fw.c
+++ b/drivers/net/wireless/ath/wil6210/fw.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2014-2015 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -20,6 +20,7 @@
 #include "fw.h"
 
 MODULE_FIRMWARE(WIL_FW_NAME);
+MODULE_FIRMWARE(WIL_FW2_NAME);
 
 /* target operations */
 /* register read */
diff --git a/drivers/net/wireless/ath/wil6210/fw_inc.c b/drivers/net/wireless/ath/wil6210/fw_inc.c
index d4acf93a9a02..157f5ef384e0 100644
--- a/drivers/net/wireless/ath/wil6210/fw_inc.c
+++ b/drivers/net/wireless/ath/wil6210/fw_inc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2014-2015 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -451,8 +451,6 @@ static int wil_fw_load(struct wil6210_priv *wil, const void *data, size_t size)
 		}
 		return -EINVAL;
 	}
-	/* Mark FW as loaded from host */
-	S(RGF_USER_USAGE_6, 1);
 
 	return rc;
 }
diff --git a/drivers/net/wireless/ath/wil6210/interrupt.c b/drivers/net/wireless/ath/wil6210/interrupt.c
index a6f923086f31..28ffc18466c4 100644
--- a/drivers/net/wireless/ath/wil6210/interrupt.c
+++ b/drivers/net/wireless/ath/wil6210/interrupt.c
@@ -166,9 +166,16 @@ void wil_unmask_irq(struct wil6210_priv *wil)
 /* target write operation */
 #define W(a, v) do { iowrite32(v, wil->csr + HOSTADDR(a)); wmb(); } while (0)
 
-static
-void wil_configure_interrupt_moderation_new(struct wil6210_priv *wil)
+void wil_configure_interrupt_moderation(struct wil6210_priv *wil)
 {
+	wil_dbg_irq(wil, "%s()\n", __func__);
+
+	/* disable interrupt moderation for monitor
+	 * to get better timestamp precision
+	 */
+	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR)
+		return;
+
 	/* Disable and clear tx counter before (re)configuration */
 	W(RGF_DMA_ITR_TX_CNT_CTL, BIT_DMA_ITR_TX_CNT_CTL_CLR);
 	W(RGF_DMA_ITR_TX_CNT_TRSH, wil->tx_max_burst_duration);
@@ -206,42 +213,8 @@ void wil_configure_interrupt_moderation_new(struct wil6210_priv *wil)
 				      BIT_DMA_ITR_RX_IDL_CNT_CTL_EXT_TIC_SEL);
 }
 
-static
-void wil_configure_interrupt_moderation_lgc(struct wil6210_priv *wil)
-{
-	/* disable, use usec resolution */
-	W(RGF_DMA_ITR_CNT_CRL, BIT_DMA_ITR_CNT_CRL_CLR);
-
-	wil_info(wil, "set ITR_TRSH = %d usec\n", wil->rx_max_burst_duration);
-	W(RGF_DMA_ITR_CNT_TRSH, wil->rx_max_burst_duration);
-	/* start it */
-	W(RGF_DMA_ITR_CNT_CRL,
-	  BIT_DMA_ITR_CNT_CRL_EN | BIT_DMA_ITR_CNT_CRL_EXT_TICK);
-}
-
 #undef W
 
-void wil_configure_interrupt_moderation(struct wil6210_priv *wil)
-{
-	wil_dbg_irq(wil, "%s()\n", __func__);
-
-	/* disable interrupt moderation for monitor
-	 * to get better timestamp precision
-	 */
-	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR)
-		return;
-
-	if (test_bit(hw_capability_advanced_itr_moderation,
-		     wil->hw_capabilities))
-		wil_configure_interrupt_moderation_new(wil);
-	else {
-		/* Advanced interrupt moderation is not available before
-		 * Sparrow v2. Will use legacy interrupt moderation
-		 */
-		wil_configure_interrupt_moderation_lgc(wil);
-	}
-}
-
 static irqreturn_t wil6210_irq_rx(int irq, void *cookie)
 {
 	struct wil6210_priv *wil = cookie;
@@ -253,7 +226,7 @@ static irqreturn_t wil6210_irq_rx(int irq, void *cookie)
 	trace_wil6210_irq_rx(isr);
 	wil_dbg_irq(wil, "ISR RX 0x%08x\n", isr);
 
-	if (!isr) {
+	if (unlikely(!isr)) {
 		wil_err(wil, "spurious IRQ: RX\n");
 		return IRQ_NONE;
 	}
@@ -266,17 +239,18 @@ static irqreturn_t wil6210_irq_rx(int irq, void *cookie)
 	 * action is always the same - should empty the accumulated
 	 * packets from the RX ring.
 	 */
-	if (isr & (BIT_DMA_EP_RX_ICR_RX_DONE | BIT_DMA_EP_RX_ICR_RX_HTRSH)) {
+	if (likely(isr & (BIT_DMA_EP_RX_ICR_RX_DONE |
+			  BIT_DMA_EP_RX_ICR_RX_HTRSH))) {
 		wil_dbg_irq(wil, "RX done\n");
 
-		if (isr & BIT_DMA_EP_RX_ICR_RX_HTRSH)
+		if (unlikely(isr & BIT_DMA_EP_RX_ICR_RX_HTRSH))
 			wil_err_ratelimited(wil,
 					    "Received \"Rx buffer is in risk of overflow\" interrupt\n");
 
 		isr &= ~(BIT_DMA_EP_RX_ICR_RX_DONE |
 			 BIT_DMA_EP_RX_ICR_RX_HTRSH);
-		if (test_bit(wil_status_reset_done, wil->status)) {
-			if (test_bit(wil_status_napi_en, wil->status)) {
+		if (likely(test_bit(wil_status_reset_done, wil->status))) {
+			if (likely(test_bit(wil_status_napi_en, wil->status))) {
 				wil_dbg_txrx(wil, "NAPI(Rx) schedule\n");
 				need_unmask = false;
 				napi_schedule(&wil->napi_rx);
@@ -289,7 +263,7 @@ static irqreturn_t wil6210_irq_rx(int irq, void *cookie)
 		}
 	}
 
-	if (isr)
+	if (unlikely(isr))
 		wil_err(wil, "un-handled RX ISR bits 0x%08x\n", isr);
 
 	/* Rx IRQ will be enabled when NAPI processing finished */
@@ -313,19 +287,19 @@ static irqreturn_t wil6210_irq_tx(int irq, void *cookie)
 	trace_wil6210_irq_tx(isr);
 	wil_dbg_irq(wil, "ISR TX 0x%08x\n", isr);
 
-	if (!isr) {
+	if (unlikely(!isr)) {
 		wil_err(wil, "spurious IRQ: TX\n");
 		return IRQ_NONE;
 	}
 
 	wil6210_mask_irq_tx(wil);
 
-	if (isr & BIT_DMA_EP_TX_ICR_TX_DONE) {
+	if (likely(isr & BIT_DMA_EP_TX_ICR_TX_DONE)) {
 		wil_dbg_irq(wil, "TX done\n");
 		isr &= ~BIT_DMA_EP_TX_ICR_TX_DONE;
 		/* clear also all VRING interrupts */
 		isr &= ~(BIT(25) - 1UL);
-		if (test_bit(wil_status_reset_done, wil->status)) {
+		if (likely(test_bit(wil_status_reset_done, wil->status))) {
 			wil_dbg_txrx(wil, "NAPI(Tx) schedule\n");
 			need_unmask = false;
 			napi_schedule(&wil->napi_tx);
@@ -334,7 +308,7 @@ static irqreturn_t wil6210_irq_tx(int irq, void *cookie)
 		}
 	}
 
-	if (isr)
+	if (unlikely(isr))
 		wil_err(wil, "un-handled TX ISR bits 0x%08x\n", isr);
 
 	/* Tx IRQ will be enabled when NAPI processing finished */
@@ -523,11 +497,11 @@ static irqreturn_t wil6210_hardirq(int irq, void *cookie)
 	/**
 	 * pseudo_cause is Clear-On-Read, no need to ACK
 	 */
-	if ((pseudo_cause == 0) || ((pseudo_cause & 0xff) == 0xff))
+	if (unlikely((pseudo_cause == 0) || ((pseudo_cause & 0xff) == 0xff)))
 		return IRQ_NONE;
 
 	/* FIXME: IRQ mask debug */
-	if (wil6210_debug_irq_mask(wil, pseudo_cause))
+	if (unlikely(wil6210_debug_irq_mask(wil, pseudo_cause)))
 		return IRQ_NONE;
 
 	trace_wil6210_irq_pseudo(pseudo_cause);
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index b04e0afdcb21..c2a238426425 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -29,10 +29,6 @@ bool no_fw_recovery;
 module_param(no_fw_recovery, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(no_fw_recovery, " disable automatic FW error recovery");
 
-static bool no_fw_load = true;
-module_param(no_fw_load, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(no_fw_load, " do not download FW, use one in on-card flash.");
-
 /* if not set via modparam, will be set to default value of 1/8 of
  * rx ring size during init flow
  */
@@ -72,6 +68,7 @@ MODULE_PARM_DESC(mtu_max, " Max MTU value.");
 
 static uint rx_ring_order = WIL_RX_RING_SIZE_ORDER_DEFAULT;
 static uint tx_ring_order = WIL_TX_RING_SIZE_ORDER_DEFAULT;
+static uint bcast_ring_order = WIL_BCAST_RING_SIZE_ORDER_DEFAULT;
 
 static int ring_order_set(const char *val, const struct kernel_param *kp)
 {
@@ -220,6 +217,7 @@ static void _wil6210_disconnect(struct wil6210_priv *wil, const u8 *bssid,
 	switch (wdev->iftype) {
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_P2P_CLIENT:
+		wil_bcast_fini(wil);
 		netif_tx_stop_all_queues(ndev);
 		netif_carrier_off(ndev);
 
@@ -364,6 +362,35 @@ static int wil_find_free_vring(struct wil6210_priv *wil)
 	return -EINVAL;
 }
 
+int wil_bcast_init(struct wil6210_priv *wil)
+{
+	int ri = wil->bcast_vring, rc;
+
+	if ((ri >= 0) && wil->vring_tx[ri].va)
+		return 0;
+
+	ri = wil_find_free_vring(wil);
+	if (ri < 0)
+		return ri;
+
+	rc = wil_vring_init_bcast(wil, ri, 1 << bcast_ring_order);
+	if (rc == 0)
+		wil->bcast_vring = ri;
+
+	return rc;
+}
+
+void wil_bcast_fini(struct wil6210_priv *wil)
+{
+	int ri = wil->bcast_vring;
+
+	if (ri < 0)
+		return;
+
+	wil->bcast_vring = -1;
+	wil_vring_fini_tx(wil, ri);
+}
+
 static void wil_connect_worker(struct work_struct *work)
 {
 	int rc;
@@ -411,6 +438,7 @@ int wil_priv_init(struct wil6210_priv *wil)
 	init_completion(&wil->wmi_call);
 
 	wil->pending_connect_cid = -1;
+	wil->bcast_vring = -1;
 	setup_timer(&wil->connect_timer, wil_connect_timer_fn, (ulong)wil);
 	setup_timer(&wil->scan_timer, wil_scan_timer_fn, (ulong)wil);
 
@@ -520,8 +548,6 @@ static int wil_target_reset(struct wil6210_priv *wil)
 {
 	int delay = 0;
 	u32 x;
-	bool is_reset_v2 = test_bit(hw_capability_reset_v2,
-				    wil->hw_capabilities);
 
 	wil_dbg_misc(wil, "Resetting \"%s\"...\n", wil->hw_name);
 
@@ -532,82 +558,67 @@ static int wil_target_reset(struct wil6210_priv *wil)
 
 	wil_halt_cpu(wil);
 
+	/* clear all boot loader "ready" bits */
+	W(RGF_USER_BL + offsetof(struct RGF_BL, ready), 0);
 	/* Clear Fw Download notification */
 	C(RGF_USER_USAGE_6, BIT(0));
 
-	if (is_reset_v2) {
-		S(RGF_CAF_OSC_CONTROL, BIT_CAF_OSC_XTAL_EN);
-		/* XTAL stabilization should take about 3ms */
-		usleep_range(5000, 7000);
-		x = R(RGF_CAF_PLL_LOCK_STATUS);
-		if (!(x & BIT_CAF_OSC_DIG_XTAL_STABLE)) {
-			wil_err(wil, "Xtal stabilization timeout\n"
-				"RGF_CAF_PLL_LOCK_STATUS = 0x%08x\n", x);
-			return -ETIME;
-		}
-		/* switch 10k to XTAL*/
-		C(RGF_USER_SPARROW_M_4, BIT_SPARROW_M_4_SEL_SLEEP_OR_REF);
-		/* 40 MHz */
-		C(RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_CAR_AHB_SW_SEL);
-
-		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0, 0x3ff81f);
-		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1, 0xf);
+	S(RGF_CAF_OSC_CONTROL, BIT_CAF_OSC_XTAL_EN);
+	/* XTAL stabilization should take about 3ms */
+	usleep_range(5000, 7000);
+	x = R(RGF_CAF_PLL_LOCK_STATUS);
+	if (!(x & BIT_CAF_OSC_DIG_XTAL_STABLE)) {
+		wil_err(wil, "Xtal stabilization timeout\n"
+			"RGF_CAF_PLL_LOCK_STATUS = 0x%08x\n", x);
+		return -ETIME;
 	}
+	/* switch 10k to XTAL*/
+	C(RGF_USER_SPARROW_M_4, BIT_SPARROW_M_4_SEL_SLEEP_OR_REF);
+	/* 40 MHz */
+	C(RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_CAR_AHB_SW_SEL);
+
+	W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0, 0x3ff81f);
+	W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1, 0xf);
 
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0xFE000000);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0x0000003F);
-	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3,
-	  is_reset_v2 ? 0x000000f0 : 0x00000170);
+	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x000000f0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0xFFE7FE00);
 
-	if (is_reset_v2) {
-		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0, 0x0);
-		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1, 0x0);
-	}
+	W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0, 0x0);
+	W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1, 0x0);
 
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0);
 
-	if (is_reset_v2) {
-		W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000003);
-		/* reset A2 PCIE AHB */
-		W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000);
-	} else {
-		W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000001);
-		W(RGF_PCIE_LOS_COUNTER_CTL, BIT(6) | BIT(8));
-		W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000);
-	}
+	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000003);
+	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000); /* reset A2 PCIE AHB */
 
-	/* TODO: check order here!!! Erez code is different */
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0);
 
-	/* wait until device ready. typical time is 200..250 msec */
+	/* wait until device ready. typical time is 20..80 msec */
 	do {
 		msleep(RST_DELAY);
-		x = R(RGF_USER_HW_MACHINE_STATE);
+		x = R(RGF_USER_BL + offsetof(struct RGF_BL, ready));
 		if (delay++ > RST_COUNT) {
-			wil_err(wil, "Reset not completed, hw_state 0x%08x\n",
+			wil_err(wil, "Reset not completed, bl.ready 0x%08x\n",
 				x);
 			return -ETIME;
 		}
-	} while (x != HW_MACHINE_BOOT_DONE);
-
-	if (!is_reset_v2)
-		W(RGF_PCIE_LOS_COUNTER_CTL, BIT(8));
+	} while (!(x & BIT_BL_READY));
 
 	C(RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_RST_PWGD);
 
+	/* enable fix for HW bug related to the SA/DA swap in AP Rx */
+	S(RGF_DMA_OFUL_NID_0, BIT_DMA_OFUL_NID_0_RX_EXT_TR_EN |
+	  BIT_DMA_OFUL_NID_0_RX_EXT_A3_SRC);
+
 	wil_dbg_misc(wil, "Reset completed in %d ms\n", delay * RST_DELAY);
 	return 0;
 }
 
-#undef R
-#undef W
-#undef S
-#undef C
-
 void wil_mbox_ring_le2cpus(struct wil6210_mbox_ring *r)
 {
 	le32_to_cpus(&r->base);
@@ -617,6 +628,32 @@ void wil_mbox_ring_le2cpus(struct wil6210_mbox_ring *r)
 	le32_to_cpus(&r->head);
 }
 
+static int wil_get_bl_info(struct wil6210_priv *wil)
+{
+	struct net_device *ndev = wil_to_ndev(wil);
+	struct RGF_BL bl;
+
+	wil_memcpy_fromio_32(&bl, wil->csr + HOSTADDR(RGF_USER_BL), sizeof(bl));
+	le32_to_cpus(&bl.ready);
+	le32_to_cpus(&bl.version);
+	le32_to_cpus(&bl.rf_type);
+	le32_to_cpus(&bl.baseband_type);
+
+	if (!is_valid_ether_addr(bl.mac_address)) {
+		wil_err(wil, "BL: Invalid MAC %pM\n", bl.mac_address);
+		return -EINVAL;
+	}
+
+	ether_addr_copy(ndev->perm_addr, bl.mac_address);
+	if (!is_valid_ether_addr(ndev->dev_addr))
+		ether_addr_copy(ndev->dev_addr, bl.mac_address);
+	wil_info(wil,
+		 "Boot Loader: ver = %d MAC = %pM RF = 0x%08x bband = 0x%08x\n",
+		 bl.version, bl.mac_address, bl.rf_type, bl.baseband_type);
+
+	return 0;
+}
+
 static int wil_wait_for_fw_ready(struct wil6210_priv *wil)
 {
 	ulong to = msecs_to_jiffies(1000);
@@ -637,7 +674,7 @@ static int wil_wait_for_fw_ready(struct wil6210_priv *wil)
  * After calling this routine, you're expected to reload
  * the firmware.
  */
-int wil_reset(struct wil6210_priv *wil)
+int wil_reset(struct wil6210_priv *wil, bool load_fw)
 {
 	int rc;
 
@@ -651,6 +688,7 @@ int wil_reset(struct wil6210_priv *wil)
 
 	cancel_work_sync(&wil->disconnect_worker);
 	wil6210_disconnect(wil, NULL, WLAN_REASON_DEAUTH_LEAVING, false);
+	wil_bcast_fini(wil);
 
 	/* prevent NAPI from being scheduled */
 	bitmap_zero(wil->status, wil_status_last);
@@ -675,46 +713,62 @@ int wil_reset(struct wil6210_priv *wil)
 	if (rc)
 		return rc;
 
-	if (!no_fw_load) {
-		wil_info(wil, "Use firmware <%s>\n", WIL_FW_NAME);
+	rc = wil_get_bl_info(wil);
+	if (rc)
+		return rc;
+
+	if (load_fw) {
+		wil_info(wil, "Use firmware <%s> + board <%s>\n", WIL_FW_NAME,
+			 WIL_FW2_NAME);
+
 		wil_halt_cpu(wil);
 		/* Loading f/w from the file */
 		rc = wil_request_firmware(wil, WIL_FW_NAME);
 		if (rc)
 			return rc;
+		rc = wil_request_firmware(wil, WIL_FW2_NAME);
+		if (rc)
+			return rc;
+
+		/* Mark FW as loaded from host */
+		S(RGF_USER_USAGE_6, 1);
 
-		/* clear any interrupts which on-card-firmware may have set */
+		/* clear any interrupts which on-card-firmware
+		 * may have set
+		 */
 		wil6210_clear_irq(wil);
-		{ /* CAF_ICR - clear and mask */
-			u32 a = HOSTADDR(RGF_CAF_ICR) +
-				offsetof(struct RGF_ICR, ICR);
-			u32 m = HOSTADDR(RGF_CAF_ICR) +
-				offsetof(struct RGF_ICR, IMV);
-			u32 icr = ioread32(wil->csr + a);
-
-			iowrite32(icr, wil->csr + a); /* W1C */
-			iowrite32(~0, wil->csr + m);
-			wmb(); /* wait for completion */
-		}
+		/* CAF_ICR - clear and mask */
+		/* it is W1C, clear by writing back same value */
+		S(RGF_CAF_ICR + offsetof(struct RGF_ICR, ICR), 0);
+		W(RGF_CAF_ICR + offsetof(struct RGF_ICR, IMV), ~0);
+
 		wil_release_cpu(wil);
-	} else {
-		wil_info(wil, "Use firmware from on-card flash\n");
 	}
 
 	/* init after reset */
 	wil->pending_connect_cid = -1;
+	wil->ap_isolate = 0;
 	reinit_completion(&wil->wmi_ready);
 	reinit_completion(&wil->wmi_call);
 
-	wil_configure_interrupt_moderation(wil);
-	wil_unmask_irq(wil);
+	if (load_fw) {
+		wil_configure_interrupt_moderation(wil);
+		wil_unmask_irq(wil);
 
-	/* we just started MAC, wait for FW ready */
-	rc = wil_wait_for_fw_ready(wil);
+		/* we just started MAC, wait for FW ready */
+		rc = wil_wait_for_fw_ready(wil);
+		if (rc == 0) /* check FW is responsive */
+			rc = wmi_echo(wil);
+	}
 
 	return rc;
 }
 
+#undef R
+#undef W
+#undef S
+#undef C
+
 void wil_fw_error_recovery(struct wil6210_priv *wil)
 {
 	wil_dbg_misc(wil, "starting fw error recovery\n");
@@ -730,7 +784,7 @@ int __wil_up(struct wil6210_priv *wil)
 
 	WARN_ON(!mutex_is_locked(&wil->mutex));
 
-	rc = wil_reset(wil);
+	rc = wil_reset(wil, true);
 	if (rc)
 		return rc;
 
@@ -837,7 +891,7 @@ int __wil_down(struct wil6210_priv *wil)
 	if (!iter)
 		wil_err(wil, "timeout waiting for idle FW/HW\n");
 
-	wil_rx_fini(wil);
+	wil_reset(wil, false);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/wil6210/netdev.c b/drivers/net/wireless/ath/wil6210/netdev.c
index ace30c1b5c64..f2f7ea29558e 100644
--- a/drivers/net/wireless/ath/wil6210/netdev.c
+++ b/drivers/net/wireless/ath/wil6210/netdev.c
@@ -82,7 +82,7 @@ static int wil6210_netdev_poll_rx(struct napi_struct *napi, int budget)
 	wil_rx_handle(wil, &quota);
 	done = budget - quota;
 
-	if (done <= 1) { /* burst ends - only one packet processed */
+	if (done < budget) {
 		napi_complete(napi);
 		wil6210_unmask_irq_rx(wil);
 		wil_dbg_txrx(wil, "NAPI RX complete\n");
@@ -110,7 +110,7 @@ static int wil6210_netdev_poll_tx(struct napi_struct *napi, int budget)
 		tx_done += wil_tx_complete(wil, i);
 	}
 
-	if (tx_done <= 1) { /* burst ends - only one packet processed */
+	if (tx_done < budget) {
 		napi_complete(napi);
 		wil6210_unmask_irq_tx(wil);
 		wil_dbg_txrx(wil, "NAPI TX complete\n");
diff --git a/drivers/net/wireless/ath/wil6210/pcie_bus.c b/drivers/net/wireless/ath/wil6210/pcie_bus.c
index 3dd26709ccb2..109986114abf 100644
--- a/drivers/net/wireless/ath/wil6210/pcie_bus.c
+++ b/drivers/net/wireless/ath/wil6210/pcie_bus.c
@@ -39,18 +39,6 @@ void wil_set_capabilities(struct wil6210_priv *wil)
 	bitmap_zero(wil->hw_capabilities, hw_capability_last);
 
 	switch (rev_id) {
-	case JTAG_DEV_ID_MARLON_B0:
-		wil->hw_name = "Marlon B0";
-		wil->hw_version = HW_VER_MARLON_B0;
-		break;
-	case JTAG_DEV_ID_SPARROW_A0:
-		wil->hw_name = "Sparrow A0";
-		wil->hw_version = HW_VER_SPARROW_A0;
-		break;
-	case JTAG_DEV_ID_SPARROW_A1:
-		wil->hw_name = "Sparrow A1";
-		wil->hw_version = HW_VER_SPARROW_A1;
-		break;
 	case JTAG_DEV_ID_SPARROW_B0:
 		wil->hw_name = "Sparrow B0";
 		wil->hw_version = HW_VER_SPARROW_B0;
@@ -62,13 +50,6 @@ void wil_set_capabilities(struct wil6210_priv *wil)
 	}
 
 	wil_info(wil, "Board hardware is %s\n", wil->hw_name);
-
-	if (wil->hw_version >= HW_VER_SPARROW_A0)
-		set_bit(hw_capability_reset_v2, wil->hw_capabilities);
-
-	if (wil->hw_version >= HW_VER_SPARROW_B0)
-		set_bit(hw_capability_advanced_itr_moderation,
-			wil->hw_capabilities);
 }
 
 void wil_disable_irq(struct wil6210_priv *wil)
@@ -150,7 +131,7 @@ static int wil_if_pcie_enable(struct wil6210_priv *wil)
 
 	/* need reset here to obtain MAC */
 	mutex_lock(&wil->mutex);
-	rc = wil_reset(wil);
+	rc = wil_reset(wil, false);
 	mutex_unlock(&wil->mutex);
 	if (debug_fw)
 		rc = 0;
@@ -265,8 +246,6 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	wil6210_debugfs_init(wil);
 
-	/* check FW is alive */
-	wmi_echo(wil);
 
 	return 0;
 
@@ -305,7 +284,6 @@ static void wil_pcie_remove(struct pci_dev *pdev)
 }
 
 static const struct pci_device_id wil6210_pcie_ids[] = {
-	{ PCI_DEVICE(0x1ae9, 0x0301) },
 	{ PCI_DEVICE(0x1ae9, 0x0310) },
 	{ PCI_DEVICE(0x1ae9, 0x0302) }, /* same as above, firmware broken */
 	{ /* end: all zeroes */	},
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index 8439f65db259..e8bd512d81a9 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -33,6 +33,15 @@ module_param(rtap_include_phy_info, bool, S_IRUGO);
 MODULE_PARM_DESC(rtap_include_phy_info,
 		 " Include PHY info in the radiotap header, default - no");
 
+bool rx_align_2;
+module_param(rx_align_2, bool, S_IRUGO);
+MODULE_PARM_DESC(rx_align_2, " align Rx buffers on 4*n+2, default - no");
+
+static inline uint wil_rx_snaplen(void)
+{
+	return rx_align_2 ? 6 : 0;
+}
+
 static inline int wil_vring_is_empty(struct vring *vring)
 {
 	return vring->swhead == vring->swtail;
@@ -53,34 +62,38 @@ static inline int wil_vring_is_full(struct vring *vring)
 	return wil_vring_next_tail(vring) == vring->swhead;
 }
 
-/*
- * Available space in Tx Vring
- */
-static inline int wil_vring_avail_tx(struct vring *vring)
+/* Used space in Tx Vring */
+static inline int wil_vring_used_tx(struct vring *vring)
 {
 	u32 swhead = vring->swhead;
 	u32 swtail = vring->swtail;
-	int used = (vring->size + swhead - swtail) % vring->size;
+	return (vring->size + swhead - swtail) % vring->size;
+}
 
-	return vring->size - used - 1;
+/* Available space in Tx Vring */
+static inline int wil_vring_avail_tx(struct vring *vring)
+{
+	return vring->size - wil_vring_used_tx(vring) - 1;
 }
 
-/**
- * wil_vring_wmark_low - low watermark for available descriptor space
- */
+/* wil_vring_wmark_low - low watermark for available descriptor space */
 static inline int wil_vring_wmark_low(struct vring *vring)
 {
 	return vring->size/8;
 }
 
-/**
- * wil_vring_wmark_high - high watermark for available descriptor space
- */
+/* wil_vring_wmark_high - high watermark for available descriptor space */
 static inline int wil_vring_wmark_high(struct vring *vring)
 {
 	return vring->size/4;
 }
 
+/* wil_val_in_range - check if value in [min,max) */
+static inline bool wil_val_in_range(int val, int min, int max)
+{
+	return val >= min && val < max;
+}
+
 static int wil_vring_alloc(struct wil6210_priv *wil, struct vring *vring)
 {
 	struct device *dev = wil_to_dev(wil);
@@ -98,8 +111,7 @@ static int wil_vring_alloc(struct wil6210_priv *wil, struct vring *vring)
 		vring->va = NULL;
 		return -ENOMEM;
 	}
-	/*
-	 * vring->va should be aligned on its size rounded up to power of 2
+	/* vring->va should be aligned on its size rounded up to power of 2
 	 * This is granted by the dma_alloc_coherent
 	 */
 	vring->va = dma_alloc_coherent(dev, sz, &vring->pa, GFP_KERNEL);
@@ -206,7 +218,7 @@ static int wil_vring_alloc_skb(struct wil6210_priv *wil, struct vring *vring,
 			       u32 i, int headroom)
 {
 	struct device *dev = wil_to_dev(wil);
-	unsigned int sz = mtu_max + ETH_HLEN;
+	unsigned int sz = mtu_max + ETH_HLEN + wil_rx_snaplen();
 	struct vring_rx_desc dd, *d = &dd;
 	volatile struct vring_rx_desc *_d = &vring->va[i].rx;
 	dma_addr_t pa;
@@ -346,27 +358,6 @@ static void wil_rx_add_radiotap_header(struct wil6210_priv *wil,
 	}
 }
 
-/*
- * Fast swap in place between 2 registers
- */
-static void wil_swap_u16(u16 *a, u16 *b)
-{
-	*a ^= *b;
-	*b ^= *a;
-	*a ^= *b;
-}
-
-static void wil_swap_ethaddr(void *data)
-{
-	struct ethhdr *eth = data;
-	u16 *s = (u16 *)eth->h_source;
-	u16 *d = (u16 *)eth->h_dest;
-
-	wil_swap_u16(s++, d++);
-	wil_swap_u16(s++, d++);
-	wil_swap_u16(s, d);
-}
-
 /**
  * reap 1 frame from @swhead
  *
@@ -383,40 +374,45 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	struct vring_rx_desc *d;
 	struct sk_buff *skb;
 	dma_addr_t pa;
-	unsigned int sz = mtu_max + ETH_HLEN;
+	unsigned int snaplen = wil_rx_snaplen();
+	unsigned int sz = mtu_max + ETH_HLEN + snaplen;
 	u16 dmalen;
 	u8 ftype;
-	u8 ds_bits;
 	int cid;
+	int i = (int)vring->swhead;
 	struct wil_net_stats *stats;
 
 	BUILD_BUG_ON(sizeof(struct vring_rx_desc) > sizeof(skb->cb));
 
-	if (wil_vring_is_empty(vring))
+	if (unlikely(wil_vring_is_empty(vring)))
 		return NULL;
 
-	_d = &vring->va[vring->swhead].rx;
-	if (!(_d->dma.status & RX_DMA_STATUS_DU)) {
+	_d = &vring->va[i].rx;
+	if (unlikely(!(_d->dma.status & RX_DMA_STATUS_DU))) {
 		/* it is not error, we just reached end of Rx done area */
 		return NULL;
 	}
 
-	skb = vring->ctx[vring->swhead].skb;
+	skb = vring->ctx[i].skb;
+	vring->ctx[i].skb = NULL;
+	wil_vring_advance_head(vring, 1);
+	if (!skb) {
+		wil_err(wil, "No Rx skb at [%d]\n", i);
+		return NULL;
+	}
 	d = wil_skb_rxdesc(skb);
 	*d = *_d;
 	pa = wil_desc_addr(&d->dma.addr);
-	vring->ctx[vring->swhead].skb = NULL;
-	wil_vring_advance_head(vring, 1);
 
 	dma_unmap_single(dev, pa, sz, DMA_FROM_DEVICE);
 	dmalen = le16_to_cpu(d->dma.length);
 
-	trace_wil6210_rx(vring->swhead, d);
-	wil_dbg_txrx(wil, "Rx[%3d] : %d bytes\n", vring->swhead, dmalen);
+	trace_wil6210_rx(i, d);
+	wil_dbg_txrx(wil, "Rx[%3d] : %d bytes\n", i, dmalen);
 	wil_hex_dump_txrx("Rx ", DUMP_PREFIX_NONE, 32, 4,
 			  (const void *)d, sizeof(*d), false);
 
-	if (dmalen > sz) {
+	if (unlikely(dmalen > sz)) {
 		wil_err(wil, "Rx size too large: %d bytes!\n", dmalen);
 		kfree_skb(skb);
 		return NULL;
@@ -445,14 +441,14 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	 * in Rx descriptor. If type is not data, it is 802.11 frame as is
 	 */
 	ftype = wil_rxdesc_ftype(d) << 2;
-	if (ftype != IEEE80211_FTYPE_DATA) {
+	if (unlikely(ftype != IEEE80211_FTYPE_DATA)) {
 		wil_dbg_txrx(wil, "Non-data frame ftype 0x%08x\n", ftype);
 		/* TODO: process it */
 		kfree_skb(skb);
 		return NULL;
 	}
 
-	if (skb->len < ETH_HLEN) {
+	if (unlikely(skb->len < ETH_HLEN + snaplen)) {
 		wil_err(wil, "Short frame, len = %d\n", skb->len);
 		/* TODO: process it (i.e. BAR) */
 		kfree_skb(skb);
@@ -463,9 +459,9 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	 * and in case of error drop the packet
 	 * higher stack layers will handle retransmission (if required)
 	 */
-	if (d->dma.status & RX_DMA_STATUS_L4I) {
+	if (likely(d->dma.status & RX_DMA_STATUS_L4I)) {
 		/* L4 protocol identified, csum calculated */
-		if ((d->dma.error & RX_DMA_ERROR_L4_ERR) == 0)
+		if (likely((d->dma.error & RX_DMA_ERROR_L4_ERR) == 0))
 			skb->ip_summed = CHECKSUM_UNNECESSARY;
 		/* If HW reports bad checksum, let IP stack re-check it
 		 * For example, HW don't understand Microsoft IP stack that
@@ -474,13 +470,15 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 		 */
 	}
 
-	ds_bits = wil_rxdesc_ds_bits(d);
-	if (ds_bits == 1) {
-		/*
-		 * HW bug - in ToDS mode, i.e. Rx on AP side,
-		 * addresses get swapped
+	if (snaplen) {
+		/* Packet layout
+		 * +-------+-------+---------+------------+------+
+		 * | SA(6) | DA(6) | SNAP(6) | ETHTYPE(2) | DATA |
+		 * +-------+-------+---------+------------+------+
+		 * Need to remove SNAP, shifting SA and DA forward
 		 */
-		wil_swap_ethaddr(skb->data);
+		memmove(skb->data + snaplen, skb->data, 2 * ETH_ALEN);
+		skb_pull(skb, snaplen);
 	}
 
 	return skb;
@@ -503,7 +501,7 @@ static int wil_rx_refill(struct wil6210_priv *wil, int count)
 			(next_tail != v->swhead) && (count-- > 0);
 			v->swtail = next_tail) {
 		rc = wil_vring_alloc_skb(wil, v, v->swtail, headroom);
-		if (rc) {
+		if (unlikely(rc)) {
 			wil_err(wil, "Error %d in wil_rx_refill[%d]\n",
 				rc, v->swtail);
 			break;
@@ -520,17 +518,71 @@ static int wil_rx_refill(struct wil6210_priv *wil, int count)
  */
 void wil_netif_rx_any(struct sk_buff *skb, struct net_device *ndev)
 {
-	gro_result_t rc;
+	gro_result_t rc = GRO_NORMAL;
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
+	struct wireless_dev *wdev = wil_to_wdev(wil);
 	unsigned int len = skb->len;
 	struct vring_rx_desc *d = wil_skb_rxdesc(skb);
-	int cid = wil_rxdesc_cid(d);
+	int cid = wil_rxdesc_cid(d); /* always 0..7, no need to check */
+	struct ethhdr *eth = (void *)skb->data;
+	/* here looking for DA, not A1, thus Rxdesc's 'mcast' indication
+	 * is not suitable, need to look at data
+	 */
+	int mcast = is_multicast_ether_addr(eth->h_dest);
 	struct wil_net_stats *stats = &wil->sta[cid].stats;
+	struct sk_buff *xmit_skb = NULL;
+	static const char * const gro_res_str[] = {
+		[GRO_MERGED]		= "GRO_MERGED",
+		[GRO_MERGED_FREE]	= "GRO_MERGED_FREE",
+		[GRO_HELD]		= "GRO_HELD",
+		[GRO_NORMAL]		= "GRO_NORMAL",
+		[GRO_DROP]		= "GRO_DROP",
+	};
 
 	skb_orphan(skb);
 
-	rc = napi_gro_receive(&wil->napi_rx, skb);
+	if (wdev->iftype == NL80211_IFTYPE_AP && !wil->ap_isolate) {
+		if (mcast) {
+			/* send multicast frames both to higher layers in
+			 * local net stack and back to the wireless medium
+			 */
+			xmit_skb = skb_copy(skb, GFP_ATOMIC);
+		} else {
+			int xmit_cid = wil_find_cid(wil, eth->h_dest);
+
+			if (xmit_cid >= 0) {
+				/* The destination station is associated to
+				 * this AP (in this VLAN), so send the frame
+				 * directly to it and do not pass it to local
+				 * net stack.
+				 */
+				xmit_skb = skb;
+				skb = NULL;
+			}
+		}
+	}
+	if (xmit_skb) {
+		/* Send to wireless media and increase priority by 256 to
+		 * keep the received priority instead of reclassifying
+		 * the frame (see cfg80211_classify8021d).
+		 */
+		xmit_skb->dev = ndev;
+		xmit_skb->priority += 256;
+		xmit_skb->protocol = htons(ETH_P_802_3);
+		skb_reset_network_header(xmit_skb);
+		skb_reset_mac_header(xmit_skb);
+		wil_dbg_txrx(wil, "Rx -> Tx %d bytes\n", len);
+		dev_queue_xmit(xmit_skb);
+	}
 
+	if (skb) { /* deliver to local stack */
+
+		skb->protocol = eth_type_trans(skb, ndev);
+		rc = napi_gro_receive(&wil->napi_rx, skb);
+		wil_dbg_txrx(wil, "Rx complete %d bytes => %s\n",
+			     len, gro_res_str[rc]);
+	}
+	/* statistics. rc set to GRO_NORMAL for AP bridging */
 	if (unlikely(rc == GRO_DROP)) {
 		ndev->stats.rx_dropped++;
 		stats->rx_dropped++;
@@ -540,17 +592,8 @@ void wil_netif_rx_any(struct sk_buff *skb, struct net_device *ndev)
 		stats->rx_packets++;
 		ndev->stats.rx_bytes += len;
 		stats->rx_bytes += len;
-	}
-	{
-		static const char * const gro_res_str[] = {
-			[GRO_MERGED]		= "GRO_MERGED",
-			[GRO_MERGED_FREE]	= "GRO_MERGED_FREE",
-			[GRO_HELD]		= "GRO_HELD",
-			[GRO_NORMAL]		= "GRO_NORMAL",
-			[GRO_DROP]		= "GRO_DROP",
-		};
-		wil_dbg_txrx(wil, "Rx complete %d bytes => %s\n",
-			     len, gro_res_str[rc]);
+		if (mcast)
+			ndev->stats.multicast++;
 	}
 }
 
@@ -565,7 +608,7 @@ void wil_rx_handle(struct wil6210_priv *wil, int *quota)
 	struct vring *v = &wil->vring_rx;
 	struct sk_buff *skb;
 
-	if (!v->va) {
+	if (unlikely(!v->va)) {
 		wil_err(wil, "Rx IRQ while Rx not yet initialized\n");
 		return;
 	}
@@ -581,7 +624,6 @@ void wil_rx_handle(struct wil6210_priv *wil, int *quota)
 			skb->protocol = htons(ETH_P_802_2);
 			wil_netif_rx_any(skb, ndev);
 		} else {
-			skb->protocol = eth_type_trans(skb, ndev);
 			wil_rx_reorder(wil, skb);
 		}
 	}
@@ -707,6 +749,72 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 	return rc;
 }
 
+int wil_vring_init_bcast(struct wil6210_priv *wil, int id, int size)
+{
+	int rc;
+	struct wmi_bcast_vring_cfg_cmd cmd = {
+		.action = cpu_to_le32(WMI_VRING_CMD_ADD),
+		.vring_cfg = {
+			.tx_sw_ring = {
+				.max_mpdu_size =
+					cpu_to_le16(wil_mtu2macbuf(mtu_max)),
+				.ring_size = cpu_to_le16(size),
+			},
+			.ringid = id,
+			.encap_trans_type = WMI_VRING_ENC_TYPE_802_3,
+		},
+	};
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_vring_cfg_done_event cmd;
+	} __packed reply;
+	struct vring *vring = &wil->vring_tx[id];
+	struct vring_tx_data *txdata = &wil->vring_tx_data[id];
+
+	wil_dbg_misc(wil, "%s() max_mpdu_size %d\n", __func__,
+		     cmd.vring_cfg.tx_sw_ring.max_mpdu_size);
+
+	if (vring->va) {
+		wil_err(wil, "Tx ring [%d] already allocated\n", id);
+		rc = -EINVAL;
+		goto out;
+	}
+
+	memset(txdata, 0, sizeof(*txdata));
+	spin_lock_init(&txdata->lock);
+	vring->size = size;
+	rc = wil_vring_alloc(wil, vring);
+	if (rc)
+		goto out;
+
+	wil->vring2cid_tid[id][0] = WIL6210_MAX_CID; /* CID */
+	wil->vring2cid_tid[id][1] = 0; /* TID */
+
+	cmd.vring_cfg.tx_sw_ring.ring_mem_base = cpu_to_le64(vring->pa);
+
+	rc = wmi_call(wil, WMI_BCAST_VRING_CFG_CMDID, &cmd, sizeof(cmd),
+		      WMI_VRING_CFG_DONE_EVENTID, &reply, sizeof(reply), 100);
+	if (rc)
+		goto out_free;
+
+	if (reply.cmd.status != WMI_FW_STATUS_SUCCESS) {
+		wil_err(wil, "Tx config failed, status 0x%02x\n",
+			reply.cmd.status);
+		rc = -EINVAL;
+		goto out_free;
+	}
+	vring->hwtail = le32_to_cpu(reply.cmd.tx_vring_tail_ptr);
+
+	txdata->enabled = 1;
+
+	return 0;
+ out_free:
+	wil_vring_free(wil, vring, 1);
+ out:
+
+	return rc;
+}
+
 void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 {
 	struct vring *vring = &wil->vring_tx[id];
@@ -730,7 +838,7 @@ void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 	memset(txdata, 0, sizeof(*txdata));
 }
 
-static struct vring *wil_find_tx_vring(struct wil6210_priv *wil,
+static struct vring *wil_find_tx_ucast(struct wil6210_priv *wil,
 				       struct sk_buff *skb)
 {
 	int i;
@@ -763,15 +871,6 @@ static struct vring *wil_find_tx_vring(struct wil6210_priv *wil,
 	return NULL;
 }
 
-static void wil_set_da_for_vring(struct wil6210_priv *wil,
-				 struct sk_buff *skb, int vring_index)
-{
-	struct ethhdr *eth = (void *)skb->data;
-	int cid = wil->vring2cid_tid[vring_index][0];
-
-	memcpy(eth->h_dest, wil->sta[cid].addr, ETH_ALEN);
-}
-
 static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 			struct sk_buff *skb);
 
@@ -792,6 +891,9 @@ static struct vring *wil_find_tx_vring_sta(struct wil6210_priv *wil,
 			continue;
 
 		cid = wil->vring2cid_tid[i][0];
+		if (cid >= WIL6210_MAX_CID) /* skip BCAST */
+			continue;
+
 		if (!wil->sta[cid].data_port_open &&
 		    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
 			break;
@@ -806,17 +908,51 @@ static struct vring *wil_find_tx_vring_sta(struct wil6210_priv *wil,
 	return NULL;
 }
 
-/*
- * Find 1-st vring and return it; set dest address for this vring in skb
- * duplicate skb and send it to other active vrings
+/* Use one of 2 strategies:
+ *
+ * 1. New (real broadcast):
+ *    use dedicated broadcast vring
+ * 2. Old (pseudo-DMS):
+ *    Find 1-st vring and return it;
+ *    duplicate skb and send it to other active vrings;
+ *    in all cases override dest address to unicast peer's address
+ * Use old strategy when new is not supported yet:
+ *  - for PBSS
+ *  - for secure link
  */
-static struct vring *wil_tx_bcast(struct wil6210_priv *wil,
-				  struct sk_buff *skb)
+static struct vring *wil_find_tx_bcast_1(struct wil6210_priv *wil,
+					 struct sk_buff *skb)
+{
+	struct vring *v;
+	int i = wil->bcast_vring;
+
+	if (i < 0)
+		return NULL;
+	v = &wil->vring_tx[i];
+	if (!v->va)
+		return NULL;
+
+	return v;
+}
+
+static void wil_set_da_for_vring(struct wil6210_priv *wil,
+				 struct sk_buff *skb, int vring_index)
+{
+	struct ethhdr *eth = (void *)skb->data;
+	int cid = wil->vring2cid_tid[vring_index][0];
+
+	ether_addr_copy(eth->h_dest, wil->sta[cid].addr);
+}
+
+static struct vring *wil_find_tx_bcast_2(struct wil6210_priv *wil,
+					 struct sk_buff *skb)
 {
 	struct vring *v, *v2;
 	struct sk_buff *skb2;
 	int i;
 	u8 cid;
+	struct ethhdr *eth = (void *)skb->data;
+	char *src = eth->h_source;
 
 	/* find 1-st vring eligible for data */
 	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
@@ -825,9 +961,15 @@ static struct vring *wil_tx_bcast(struct wil6210_priv *wil,
 			continue;
 
 		cid = wil->vring2cid_tid[i][0];
+		if (cid >= WIL6210_MAX_CID) /* skip BCAST */
+			continue;
 		if (!wil->sta[cid].data_port_open)
 			continue;
 
+		/* don't Tx back to source when re-routing Rx->Tx at the AP */
+		if (0 == memcmp(wil->sta[cid].addr, src, ETH_ALEN))
+			continue;
+
 		goto found;
 	}
 
@@ -845,9 +987,14 @@ found:
 		if (!v2->va)
 			continue;
 		cid = wil->vring2cid_tid[i][0];
+		if (cid >= WIL6210_MAX_CID) /* skip BCAST */
+			continue;
 		if (!wil->sta[cid].data_port_open)
 			continue;
 
+		if (0 == memcmp(wil->sta[cid].addr, src, ETH_ALEN))
+			continue;
+
 		skb2 = skb_copy(skb, GFP_ATOMIC);
 		if (skb2) {
 			wil_dbg_txrx(wil, "BCAST DUP -> ring %d\n", i);
@@ -861,6 +1008,20 @@ found:
 	return v;
 }
 
+static struct vring *wil_find_tx_bcast(struct wil6210_priv *wil,
+				       struct sk_buff *skb)
+{
+	struct wireless_dev *wdev = wil->wdev;
+
+	if (wdev->iftype != NL80211_IFTYPE_AP)
+		return wil_find_tx_bcast_2(wil, skb);
+
+	if (wil->privacy)
+		return wil_find_tx_bcast_2(wil, skb);
+
+	return wil_find_tx_bcast_1(wil, skb);
+}
+
 static int wil_tx_desc_map(struct vring_tx_desc *d, dma_addr_t pa, u32 len,
 			   int vring_index)
 {
@@ -952,13 +1113,16 @@ static int __wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 	struct vring_tx_data *txdata = &wil->vring_tx_data[vring_index];
 	uint i = swhead;
 	dma_addr_t pa;
+	int used;
+	bool mcast = (vring_index == wil->bcast_vring);
+	uint len = skb_headlen(skb);
 
 	wil_dbg_txrx(wil, "%s()\n", __func__);
 
 	if (unlikely(!txdata->enabled))
 		return -EINVAL;
 
-	if (avail < 1 + nr_frags) {
+	if (unlikely(avail < 1 + nr_frags)) {
 		wil_err_ratelimited(wil,
 				    "Tx ring[%2d] full. No space for %d fragments\n",
 				    vring_index, 1 + nr_frags);
@@ -977,9 +1141,19 @@ static int __wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 		return -EINVAL;
 	vring->ctx[i].mapped_as = wil_mapped_as_single;
 	/* 1-st segment */
-	wil_tx_desc_map(d, pa, skb_headlen(skb), vring_index);
+	wil_tx_desc_map(d, pa, len, vring_index);
+	if (unlikely(mcast)) {
+		d->mac.d[0] |= BIT(MAC_CFG_DESC_TX_0_MCS_EN_POS); /* MCS 0 */
+		if (unlikely(len > WIL_BCAST_MCS0_LIMIT)) {
+			/* set MCS 1 */
+			d->mac.d[0] |= (1 << MAC_CFG_DESC_TX_0_MCS_INDEX_POS);
+			/* packet mode 2 */
+			d->mac.d[1] |= BIT(MAC_CFG_DESC_TX_1_PKT_MODE_EN_POS) |
+				       (2 << MAC_CFG_DESC_TX_1_PKT_MODE_POS);
+		}
+	}
 	/* Process TCP/UDP checksum offloading */
-	if (wil_tx_desc_offload_cksum_set(wil, d, skb)) {
+	if (unlikely(wil_tx_desc_offload_cksum_set(wil, d, skb))) {
 		wil_err(wil, "Tx[%2d] Failed to set cksum, drop packet\n",
 			vring_index);
 		goto dma_error;
@@ -1027,8 +1201,14 @@ static int __wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 	 */
 	vring->ctx[i].skb = skb_get(skb);
 
-	if (wil_vring_is_empty(vring)) /* performance monitoring */
+	/* performance monitoring */
+	used = wil_vring_used_tx(vring);
+	if (wil_val_in_range(vring_idle_trsh,
+			     used, used + nr_frags + 1)) {
 		txdata->idle += get_cycles() - txdata->last_idle;
+		wil_dbg_txrx(wil,  "Ring[%2d] not idle %d -> %d\n",
+			     vring_index, used, used + nr_frags + 1);
+	}
 
 	/* advance swhead */
 	wil_vring_advance_head(vring, nr_frags + 1);
@@ -1077,23 +1257,24 @@ netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
 	struct ethhdr *eth = (void *)skb->data;
+	bool bcast = is_multicast_ether_addr(eth->h_dest);
 	struct vring *vring;
 	static bool pr_once_fw;
 	int rc;
 
 	wil_dbg_txrx(wil, "%s()\n", __func__);
-	if (!test_bit(wil_status_fwready, wil->status)) {
+	if (unlikely(!test_bit(wil_status_fwready, wil->status))) {
 		if (!pr_once_fw) {
 			wil_err(wil, "FW not ready\n");
 			pr_once_fw = true;
 		}
 		goto drop;
 	}
-	if (!test_bit(wil_status_fwconnected, wil->status)) {
+	if (unlikely(!test_bit(wil_status_fwconnected, wil->status))) {
 		wil_err(wil, "FW not connected\n");
 		goto drop;
 	}
-	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR) {
+	if (unlikely(wil->wdev->iftype == NL80211_IFTYPE_MONITOR)) {
 		wil_err(wil, "Xmit in monitor mode not supported\n");
 		goto drop;
 	}
@@ -1104,12 +1285,10 @@ netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		/* in STA mode (ESS), all to same VRING */
 		vring = wil_find_tx_vring_sta(wil, skb);
 	} else { /* direct communication, find matching VRING */
-		if (is_unicast_ether_addr(eth->h_dest))
-			vring = wil_find_tx_vring(wil, skb);
-		else
-			vring = wil_tx_bcast(wil, skb);
+		vring = bcast ? wil_find_tx_bcast(wil, skb) :
+				wil_find_tx_ucast(wil, skb);
 	}
-	if (!vring) {
+	if (unlikely(!vring)) {
 		wil_dbg_txrx(wil, "No Tx VRING found for %pM\n", eth->h_dest);
 		goto drop;
 	}
@@ -1117,7 +1296,7 @@ netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	rc = wil_tx_vring(wil, vring, skb);
 
 	/* do we still have enough room in the vring? */
-	if (wil_vring_avail_tx(vring) < wil_vring_wmark_low(vring)) {
+	if (unlikely(wil_vring_avail_tx(vring) < wil_vring_wmark_low(vring))) {
 		netif_tx_stop_all_queues(wil_to_ndev(wil));
 		wil_dbg_txrx(wil, "netif_tx_stop : ring full\n");
 	}
@@ -1170,21 +1349,28 @@ int wil_tx_complete(struct wil6210_priv *wil, int ringid)
 	struct vring_tx_data *txdata = &wil->vring_tx_data[ringid];
 	int done = 0;
 	int cid = wil->vring2cid_tid[ringid][0];
-	struct wil_net_stats *stats = &wil->sta[cid].stats;
+	struct wil_net_stats *stats = NULL;
 	volatile struct vring_tx_desc *_d;
+	int used_before_complete;
+	int used_new;
 
-	if (!vring->va) {
+	if (unlikely(!vring->va)) {
 		wil_err(wil, "Tx irq[%d]: vring not initialized\n", ringid);
 		return 0;
 	}
 
-	if (!txdata->enabled) {
+	if (unlikely(!txdata->enabled)) {
 		wil_info(wil, "Tx irq[%d]: vring disabled\n", ringid);
 		return 0;
 	}
 
 	wil_dbg_txrx(wil, "%s(%d)\n", __func__, ringid);
 
+	used_before_complete = wil_vring_used_tx(vring);
+
+	if (cid < WIL6210_MAX_CID)
+		stats = &wil->sta[cid].stats;
+
 	while (!wil_vring_is_empty(vring)) {
 		int new_swtail;
 		struct wil_ctx *ctx = &vring->ctx[vring->swtail];
@@ -1196,7 +1382,7 @@ int wil_tx_complete(struct wil6210_priv *wil, int ringid)
 		/* TODO: check we are not past head */
 
 		_d = &vring->va[lf].tx;
-		if (!(_d->dma.status & TX_DMA_STATUS_DU))
+		if (unlikely(!(_d->dma.status & TX_DMA_STATUS_DU)))
 			break;
 
 		new_swtail = (lf + 1) % vring->size;
@@ -1224,14 +1410,17 @@ int wil_tx_complete(struct wil6210_priv *wil, int ringid)
 			wil_txdesc_unmap(dev, d, ctx);
 
 			if (skb) {
-				if (d->dma.error == 0) {
+				if (likely(d->dma.error == 0)) {
 					ndev->stats.tx_packets++;
-					stats->tx_packets++;
 					ndev->stats.tx_bytes += skb->len;
-					stats->tx_bytes += skb->len;
+					if (stats) {
+						stats->tx_packets++;
+						stats->tx_bytes += skb->len;
+					}
 				} else {
 					ndev->stats.tx_errors++;
-					stats->tx_errors++;
+					if (stats)
+						stats->tx_errors++;
 				}
 				wil_consume_skb(skb, d->dma.error == 0);
 			}
@@ -1246,8 +1435,12 @@ int wil_tx_complete(struct wil6210_priv *wil, int ringid)
 		}
 	}
 
-	if (wil_vring_is_empty(vring)) { /* performance monitoring */
-		wil_dbg_txrx(wil, "Ring[%2d] empty\n", ringid);
+	/* performance monitoring */
+	used_new = wil_vring_used_tx(vring);
+	if (wil_val_in_range(vring_idle_trsh,
+			     used_new, used_before_complete)) {
+		wil_dbg_txrx(wil, "Ring[%2d] idle %d -> %d\n",
+			     ringid, used_before_complete, used_new);
 		txdata->last_idle = get_cycles();
 	}
 
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index 94611568fc9a..4310972c9e16 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -27,9 +27,12 @@ extern bool no_fw_recovery;
 extern unsigned int mtu_max;
 extern unsigned short rx_ring_overflow_thrsh;
 extern int agg_wsize;
+extern u32 vring_idle_trsh;
+extern bool rx_align_2;
 
 #define WIL_NAME "wil6210"
-#define WIL_FW_NAME "wil6210.fw"
+#define WIL_FW_NAME "wil6210.fw" /* code */
+#define WIL_FW2_NAME "wil6210.board" /* board & radio parameters */
 
 #define WIL_MAX_BUS_REQUEST_KBPS 800000 /* ~6.1Gbps */
 
@@ -47,6 +50,8 @@ static inline u32 WIL_GET_BITS(u32 x, int b0, int b1)
 #define WIL_TX_Q_LEN_DEFAULT		(4000)
 #define WIL_RX_RING_SIZE_ORDER_DEFAULT	(10)
 #define WIL_TX_RING_SIZE_ORDER_DEFAULT	(10)
+#define WIL_BCAST_RING_SIZE_ORDER_DEFAULT	(7)
+#define WIL_BCAST_MCS0_LIMIT		(1024) /* limit for MCS0 frame size */
 /* limit ring size in range [32..32k] */
 #define WIL_RING_SIZE_ORDER_MIN	(5)
 #define WIL_RING_SIZE_ORDER_MAX	(15)
@@ -120,6 +125,16 @@ struct RGF_ICR {
 	u32 IMC; /* Mask Clear, write 1 to clear */
 } __packed;
 
+struct RGF_BL {
+	u32 ready;		/* 0x880A3C bit [0] */
+#define BIT_BL_READY	BIT(0)
+	u32 version;		/* 0x880A40 version of the BL struct */
+	u32 rf_type;		/* 0x880A44 ID of the connected RF */
+	u32 baseband_type;	/* 0x880A48 ID of the baseband */
+	u8  mac_address[ETH_ALEN]; /* 0x880A4C permanent MAC */
+	u8 pad[2];
+} __packed;
+
 /* registers - FW addresses */
 #define RGF_USER_USAGE_1		(0x880004)
 #define RGF_USER_USAGE_6		(0x880018)
@@ -130,6 +145,7 @@ struct RGF_ICR {
 #define RGF_USER_MAC_CPU_0		(0x8801fc)
 	#define BIT_USER_MAC_CPU_MAN_RST	BIT(1) /* mac_cpu_man_rst */
 #define RGF_USER_USER_SCRATCH_PAD	(0x8802bc)
+#define RGF_USER_BL			(0x880A3C) /* Boot Loader */
 #define RGF_USER_FW_REV_ID		(0x880a8c) /* chip revision */
 #define RGF_USER_CLKS_CTL_0		(0x880abc)
 	#define BIT_USER_CLKS_CAR_AHB_SW_SEL	BIT(1) /* ref clk/PLL */
@@ -169,6 +185,13 @@ struct RGF_ICR {
 	#define BIT_DMA_ITR_CNT_CRL_CLR		BIT(3)
 	#define BIT_DMA_ITR_CNT_CRL_REACH_TRSH	BIT(4)
 
+/* Offload control (Sparrow B0+) */
+#define RGF_DMA_OFUL_NID_0		(0x881cd4)
+	#define BIT_DMA_OFUL_NID_0_RX_EXT_TR_EN		BIT(0)
+	#define BIT_DMA_OFUL_NID_0_TX_EXT_TR_EN		BIT(1)
+	#define BIT_DMA_OFUL_NID_0_RX_EXT_A3_SRC	BIT(2)
+	#define BIT_DMA_OFUL_NID_0_TX_EXT_A3_SRC	BIT(3)
+
 /* New (sparrow v2+) interrupt moderation control */
 #define RGF_DMA_ITR_TX_DESQ_NO_MOD		(0x881d40)
 #define RGF_DMA_ITR_TX_CNT_TRSH			(0x881d34)
@@ -229,16 +252,10 @@ struct RGF_ICR {
 	#define BIT_CAF_OSC_DIG_XTAL_STABLE	BIT(0)
 
 #define RGF_USER_JTAG_DEV_ID	(0x880b34) /* device ID */
-	#define JTAG_DEV_ID_MARLON_B0	(0x0612072f)
-	#define JTAG_DEV_ID_SPARROW_A0	(0x0632072f)
-	#define JTAG_DEV_ID_SPARROW_A1	(0x1632072f)
 	#define JTAG_DEV_ID_SPARROW_B0	(0x2632072f)
 
 enum {
 	HW_VER_UNKNOWN,
-	HW_VER_MARLON_B0,  /* JTAG_DEV_ID_MARLON_B0  */
-	HW_VER_SPARROW_A0, /* JTAG_DEV_ID_SPARROW_A0 */
-	HW_VER_SPARROW_A1, /* JTAG_DEV_ID_SPARROW_A1 */
 	HW_VER_SPARROW_B0, /* JTAG_DEV_ID_SPARROW_B0 */
 };
 
@@ -482,8 +499,6 @@ enum {
 };
 
 enum {
-	hw_capability_reset_v2 = 0,
-	hw_capability_advanced_itr_moderation = 1,
 	hw_capability_last
 };
 
@@ -528,8 +543,9 @@ struct wil6210_priv {
 	wait_queue_head_t wq; /* for all wait_event() use */
 	/* profile */
 	u32 monitor_flags;
-	u32 secure_pcp; /* create secure PCP? */
+	u32 privacy; /* secure connection? */
 	int sinfo_gen;
+	u32 ap_isolate; /* no intra-BSS communication */
 	/* interrupt moderation */
 	u32 tx_max_burst_duration;
 	u32 tx_interframe_timeout;
@@ -581,6 +597,7 @@ struct wil6210_priv {
 	struct vring_tx_data vring_tx_data[WIL6210_MAX_TX_RINGS];
 	u8 vring2cid_tid[WIL6210_MAX_TX_RINGS][2]; /* [0] - CID, [1] - TID */
 	struct wil_sta_info sta[WIL6210_MAX_CID];
+	int bcast_vring;
 	/* scan */
 	struct cfg80211_scan_request *scan_request;
 
@@ -658,7 +675,7 @@ int wil_if_add(struct wil6210_priv *wil);
 void wil_if_remove(struct wil6210_priv *wil);
 int wil_priv_init(struct wil6210_priv *wil);
 void wil_priv_deinit(struct wil6210_priv *wil);
-int wil_reset(struct wil6210_priv *wil);
+int wil_reset(struct wil6210_priv *wil, bool no_fw);
 void wil_fw_error_recovery(struct wil6210_priv *wil);
 void wil_set_recovery_state(struct wil6210_priv *wil, int state);
 int wil_up(struct wil6210_priv *wil);
@@ -743,6 +760,9 @@ void wil_rx_fini(struct wil6210_priv *wil);
 int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 		      int cid, int tid);
 void wil_vring_fini_tx(struct wil6210_priv *wil, int id);
+int wil_vring_init_bcast(struct wil6210_priv *wil, int id, int size);
+int wil_bcast_init(struct wil6210_priv *wil);
+void wil_bcast_fini(struct wil6210_priv *wil);
 
 netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev);
 int wil_tx_complete(struct wil6210_priv *wil, int ringid);
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 0f3e4334c8e3..9fe2085be2c5 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -281,7 +281,6 @@ int wmi_send(struct wil6210_priv *wil, u16 cmdid, void *buf, u16 len)
 /*=== Event handlers ===*/
 static void wmi_evt_ready(struct wil6210_priv *wil, int id, void *d, int len)
 {
-	struct net_device *ndev = wil_to_ndev(wil);
 	struct wireless_dev *wdev = wil->wdev;
 	struct wmi_ready_event *evt = d;
 
@@ -290,11 +289,7 @@ static void wmi_evt_ready(struct wil6210_priv *wil, int id, void *d, int len)
 
 	wil_info(wil, "FW ver. %d; MAC %pM; %d MID's\n", wil->fw_version,
 		 evt->mac, wil->n_mids);
-
-	if (!is_valid_ether_addr(ndev->dev_addr)) {
-		memcpy(ndev->dev_addr, evt->mac, ETH_ALEN);
-		memcpy(ndev->perm_addr, evt->mac, ETH_ALEN);
-	}
+	/* ignore MAC address, we already have it from the boot loader */
 	snprintf(wdev->wiphy->fw_version, sizeof(wdev->wiphy->fw_version),
 		 "%d", wil->fw_version);
 }
@@ -471,7 +466,7 @@ static void wmi_evt_connect(struct wil6210_priv *wil, int id, void *d, int len)
 
 	/* FIXME FW can transmit only ucast frames to peer */
 	/* FIXME real ring_id instead of hard coded 0 */
-	memcpy(wil->sta[evt->cid].addr, evt->bssid, ETH_ALEN);
+	ether_addr_copy(wil->sta[evt->cid].addr, evt->bssid);
 	wil->sta[evt->cid].status = wil_sta_conn_pending;
 
 	wil->pending_connect_cid = evt->cid;
@@ -529,8 +524,8 @@ static void wmi_evt_eapol_rx(struct wil6210_priv *wil, int id,
 	}
 
 	eth = (struct ethhdr *)skb_put(skb, ETH_HLEN);
-	memcpy(eth->h_dest, ndev->dev_addr, ETH_ALEN);
-	memcpy(eth->h_source, evt->src_mac, ETH_ALEN);
+	ether_addr_copy(eth->h_dest, ndev->dev_addr);
+	ether_addr_copy(eth->h_source, evt->src_mac);
 	eth->h_proto = cpu_to_be16(ETH_P_PAE);
 	memcpy(skb_put(skb, eapol_len), evt->eapol, eapol_len);
 	skb->protocol = eth_type_trans(skb, ndev);
@@ -856,7 +851,7 @@ int wmi_set_mac_address(struct wil6210_priv *wil, void *addr)
 {
 	struct wmi_set_mac_address_cmd cmd;
 
-	memcpy(cmd.mac, addr, ETH_ALEN);
+	ether_addr_copy(cmd.mac, addr);
 
 	wil_dbg_wmi(wil, "Set MAC %pM\n", addr);
 
@@ -879,7 +874,7 @@ int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan)
 		struct wmi_pcp_started_event evt;
 	} __packed reply;
 
-	if (!wil->secure_pcp)
+	if (!wil->privacy)
 		cmd.disable_sec = 1;
 
 	if ((cmd.pcp_max_assoc_sta > WIL6210_MAX_CID) ||
@@ -1114,6 +1109,11 @@ int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring)
 		 */
 		cmd.l3_l4_ctrl |= (1 << L3_L4_CTRL_TCPIP_CHECKSUM_EN_POS);
 	}
+
+	if (rx_align_2)
+		cmd.l2_802_3_offload_ctrl |=
+				L2_802_3_OFFLOAD_CTRL_SNAP_KEEP_MSK;
+
 	/* typical time for secure PCP is 840ms */
 	rc = wmi_call(wil, WMI_CFG_RX_CHAIN_CMDID, &cmd, sizeof(cmd),
 		      WMI_CFG_RX_CHAIN_DONE_EVENTID, &evt, sizeof(evt), 2000);
@@ -1162,7 +1162,8 @@ int wmi_disconnect_sta(struct wil6210_priv *wil, const u8 *mac, u16 reason)
 	struct wmi_disconnect_sta_cmd cmd = {
 		.disconnect_reason = cpu_to_le16(reason),
 	};
-	memcpy(cmd.dst_mac, mac, ETH_ALEN);
+
+	ether_addr_copy(cmd.dst_mac, mac);
 
 	wil_dbg_wmi(wil, "%s(%pM, reason %d)\n", __func__, mac, reason);
 
diff --git a/drivers/net/wireless/ath/wil6210/wmi.h b/drivers/net/wireless/ath/wil6210/wmi.h
index 8a4af613e191..b29055315350 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.h
+++ b/drivers/net/wireless/ath/wil6210/wmi.h
@@ -70,7 +70,6 @@ enum wmi_command_id {
 	WMI_SET_UCODE_IDLE_CMDID	= 0x0813,
 	WMI_SET_WORK_MODE_CMDID		= 0x0815,
 	WMI_LO_LEAKAGE_CALIB_CMDID	= 0x0816,
-	WMI_MARLON_R_ACTIVATE_CMDID	= 0x0817,
 	WMI_MARLON_R_READ_CMDID		= 0x0818,
 	WMI_MARLON_R_WRITE_CMDID	= 0x0819,
 	WMI_MARLON_R_TXRX_SEL_CMDID	= 0x081a,
@@ -80,6 +79,7 @@ enum wmi_command_id {
 	WMI_RF_RX_TEST_CMDID		= 0x081e,
 	WMI_CFG_RX_CHAIN_CMDID		= 0x0820,
 	WMI_VRING_CFG_CMDID		= 0x0821,
+	WMI_BCAST_VRING_CFG_CMDID	= 0x0822,
 	WMI_VRING_BA_EN_CMDID		= 0x0823,
 	WMI_VRING_BA_DIS_CMDID		= 0x0824,
 	WMI_RCP_ADDBA_RESP_CMDID	= 0x0825,
@@ -99,6 +99,7 @@ enum wmi_command_id {
 	WMI_BF_TXSS_MGMT_CMDID		= 0x0837,
 	WMI_BF_SM_MGMT_CMDID		= 0x0838,
 	WMI_BF_RXSS_MGMT_CMDID		= 0x0839,
+	WMI_BF_TRIG_CMDID		= 0x083A,
 	WMI_SET_SECTORS_CMDID		= 0x0849,
 	WMI_MAINTAIN_PAUSE_CMDID	= 0x0850,
 	WMI_MAINTAIN_RESUME_CMDID	= 0x0851,
@@ -596,6 +597,22 @@ struct wmi_vring_cfg_cmd {
 } __packed;
 
 /*
+ * WMI_BCAST_VRING_CFG_CMDID
+ */
+struct wmi_bcast_vring_cfg {
+	struct wmi_sw_ring_cfg tx_sw_ring;
+	u8 ringid;				/* 0-23 vrings */
+	u8 encap_trans_type;
+	u8 ds_cfg;				/* 802.3 DS cfg */
+	u8 nwifi_ds_trans_type;
+} __packed;
+
+struct wmi_bcast_vring_cfg_cmd {
+	__le32 action;
+	struct wmi_bcast_vring_cfg vring_cfg;
+} __packed;
+
+/*
  * WMI_VRING_BA_EN_CMDID
  */
 struct wmi_vring_ba_en_cmd {
@@ -687,6 +704,9 @@ struct wmi_cfg_rx_chain_cmd {
 	#define L2_802_3_OFFLOAD_CTRL_VLAN_TAG_INSERTION_POS (0)
 	#define L2_802_3_OFFLOAD_CTRL_VLAN_TAG_INSERTION_LEN (1)
 	#define L2_802_3_OFFLOAD_CTRL_VLAN_TAG_INSERTION_MSK (0x1)
+	#define L2_802_3_OFFLOAD_CTRL_SNAP_KEEP_POS (1)
+	#define L2_802_3_OFFLOAD_CTRL_SNAP_KEEP_LEN (1)
+	#define L2_802_3_OFFLOAD_CTRL_SNAP_KEEP_MSK (0x2)
 	u8 l2_802_3_offload_ctrl;
 
 	#define L2_NWIFI_OFFLOAD_CTRL_REMOVE_QOS_POS (0)
@@ -841,7 +861,6 @@ enum wmi_event_id {
 	WMI_IQ_RX_CALIB_DONE_EVENTID		= 0x1812,
 	WMI_SET_WORK_MODE_DONE_EVENTID		= 0x1815,
 	WMI_LO_LEAKAGE_CALIB_DONE_EVENTID	= 0x1816,
-	WMI_MARLON_R_ACTIVATE_DONE_EVENTID	= 0x1817,
 	WMI_MARLON_R_READ_DONE_EVENTID		= 0x1818,
 	WMI_MARLON_R_WRITE_DONE_EVENTID		= 0x1819,
 	WMI_MARLON_R_TXRX_SEL_DONE_EVENTID	= 0x181a,
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index 55db9f03eb2a..6a1f03c271c1 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -1004,7 +1004,7 @@ static void frag_rx_path(struct atmel_private *priv,
 			atmel_copy_to_host(priv->dev, (void *)&netcrc, rx_packet_loc + msdu_size, 4);
 			if ((crc ^ 0xffffffff) != netcrc) {
 				priv->dev->stats.rx_crc_errors++;
-				memset(priv->frag_source, 0xff, ETH_ALEN);
+				eth_broadcast_addr(priv->frag_source);
 			}
 		}
 
@@ -1022,7 +1022,7 @@ static void frag_rx_path(struct atmel_private *priv,
 			atmel_copy_to_host(priv->dev, (void *)&netcrc, rx_packet_loc + msdu_size, 4);
 			if ((crc ^ 0xffffffff) != netcrc) {
 				priv->dev->stats.rx_crc_errors++;
-				memset(priv->frag_source, 0xff, ETH_ALEN);
+				eth_broadcast_addr(priv->frag_source);
 				more_frags = 1; /* don't send broken assembly */
 			}
 		}
@@ -1031,7 +1031,7 @@ static void frag_rx_path(struct atmel_private *priv,
 		priv->frag_no++;
 
 		if (!more_frags) { /* last one */
-			memset(priv->frag_source, 0xff, ETH_ALEN);
+			eth_broadcast_addr(priv->frag_source);
 			if (!(skb = dev_alloc_skb(priv->frag_len + 14))) {
 				priv->dev->stats.rx_dropped++;
 			} else {
@@ -1127,7 +1127,7 @@ static void rx_done_irq(struct atmel_private *priv)
 			atmel_copy_to_host(priv->dev, (unsigned char *)&priv->rx_buf, rx_packet_loc + 24, msdu_size);
 
 			/* we use the same buffer for frag reassembly and control packets */
-			memset(priv->frag_source, 0xff, ETH_ALEN);
+			eth_broadcast_addr(priv->frag_source);
 
 			if (priv->do_rx_crc) {
 				/* last 4 octets is crc */
@@ -1379,7 +1379,7 @@ static int atmel_close(struct net_device *dev)
 		wrqu.data.length = 0;
 		wrqu.data.flags = 0;
 		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(priv->dev, SIOCGIWAP, &wrqu, NULL);
 	}
 
@@ -1555,7 +1555,7 @@ struct net_device *init_atmel_card(unsigned short irq, unsigned long port,
 	priv->last_qual = jiffies;
 	priv->last_beacon_timestamp = 0;
 	memset(priv->frag_source, 0xff, sizeof(priv->frag_source));
-	memset(priv->BSSID, 0, ETH_ALEN);
+	eth_zero_addr(priv->BSSID);
 	priv->CurrentBSSID[0] = 0xFF; /* Initialize to something invalid.... */
 	priv->station_was_associated = 0;
 
@@ -2760,7 +2760,7 @@ static void atmel_scan(struct atmel_private *priv, int specific_ssid)
 		u8 SSID_size;
 	} cmd;
 
-	memset(cmd.BSSID, 0xff, ETH_ALEN);
+	eth_broadcast_addr(cmd.BSSID);
 
 	if (priv->fast_scan) {
 		cmd.SSID_size = priv->SSID_size;
@@ -4049,7 +4049,7 @@ static int reset_atmel_card(struct net_device *dev)
 		wrqu.data.length = 0;
 		wrqu.data.flags = 0;
 		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(priv->dev, SIOCGIWAP, &wrqu, NULL);
 	}
 
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index 1d7982afc0ad..6837064908be 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -553,7 +553,7 @@ static bool b43_dma_mapping_error(struct b43_dmaring *ring,
 				  size_t buffersize, bool dma_to_device)
 {
 	if (unlikely(dma_mapping_error(ring->dev->dev->dma_dev, addr)))
-		return 1;
+		return true;
 
 	switch (ring->type) {
 	case B43_DMA_30BIT:
@@ -571,13 +571,13 @@ static bool b43_dma_mapping_error(struct b43_dmaring *ring,
 	}
 
 	/* The address is OK. */
-	return 0;
+	return false;
 
 address_error:
 	/* We can't support this address. Unmap it again. */
 	unmap_descbuffer(ring, addr, buffersize, dma_to_device);
 
-	return 1;
+	return true;
 }
 
 static bool b43_rx_buffer_is_poisoned(struct b43_dmaring *ring, struct sk_buff *skb)
@@ -1099,16 +1099,16 @@ static bool b43_dma_translation_in_low_word(struct b43_wldev *dev,
 					    enum b43_dmatype type)
 {
 	if (type != B43_DMA_64BIT)
-		return 1;
+		return true;
 
 #ifdef CONFIG_B43_SSB
 	if (dev->dev->bus_type == B43_BUS_SSB &&
 	    dev->dev->sdev->bus->bustype == SSB_BUSTYPE_PCI &&
 	    !(pci_is_pcie(dev->dev->sdev->bus->host_pci) &&
 	      ssb_read32(dev->dev->sdev, SSB_TMSHIGH) & SSB_TMSHIGH_DMA64))
-			return 1;
+			return true;
 #endif
-	return 0;
+	return false;
 }
 
 int b43_dma_init(struct b43_wldev *dev)
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index ccbdb05b28cd..b2f9521fe551 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -4132,7 +4132,7 @@ static void b43_op_bss_info_changed(struct ieee80211_hw *hw,
 		if (conf->bssid)
 			memcpy(wl->bssid, conf->bssid, ETH_ALEN);
 		else
-			memset(wl->bssid, 0, ETH_ALEN);
+			eth_zero_addr(wl->bssid);
 	}
 
 	if (b43_status(dev) >= B43_STAT_INITIALIZED) {
@@ -4819,7 +4819,7 @@ static void b43_wireless_core_exit(struct b43_wldev *dev)
 	switch (dev->dev->bus_type) {
 #ifdef CONFIG_B43_BCMA
 	case B43_BUS_BCMA:
-		bcma_core_pci_down(dev->dev->bdev->bus);
+		bcma_host_pci_down(dev->dev->bdev->bus);
 		break;
 #endif
 #ifdef CONFIG_B43_SSB
@@ -4866,9 +4866,9 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
 	switch (dev->dev->bus_type) {
 #ifdef CONFIG_B43_BCMA
 	case B43_BUS_BCMA:
-		bcma_core_pci_irq_ctl(&dev->dev->bdev->bus->drv_pci[0],
+		bcma_host_pci_irq_ctl(dev->dev->bdev->bus,
 				      dev->dev->bdev, true);
-		bcma_core_pci_up(dev->dev->bdev->bus);
+		bcma_host_pci_up(dev->dev->bdev->bus);
 		break;
 #endif
 #ifdef CONFIG_B43_SSB
@@ -5051,7 +5051,7 @@ static void b43_op_remove_interface(struct ieee80211_hw *hw,
 	wl->operating = false;
 
 	b43_adjust_opmode(dev);
-	memset(wl->mac_addr, 0, ETH_ALEN);
+	eth_zero_addr(wl->mac_addr);
 	b43_upload_card_macaddress(dev);
 
 	mutex_unlock(&wl->mutex);
@@ -5067,8 +5067,8 @@ static int b43_op_start(struct ieee80211_hw *hw)
 	/* Kill all old instance specific information to make sure
 	 * the card won't use it in the short timeframe between start
 	 * and mac80211 reconfiguring it. */
-	memset(wl->bssid, 0, ETH_ALEN);
-	memset(wl->mac_addr, 0, ETH_ALEN);
+	eth_zero_addr(wl->bssid);
+	eth_zero_addr(wl->mac_addr);
 	wl->filter_flags = 0;
 	wl->radiotap_enabled = false;
 	b43_qos_clear(wl);
@@ -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/b43legacy/dma.c b/drivers/net/wireless/b43legacy/dma.c
index b2ed1795130b..f9dd892b9f27 100644
--- a/drivers/net/wireless/b43legacy/dma.c
+++ b/drivers/net/wireless/b43legacy/dma.c
@@ -427,7 +427,7 @@ static bool b43legacy_dma_mapping_error(struct b43legacy_dmaring *ring,
 					 bool dma_to_device)
 {
 	if (unlikely(dma_mapping_error(ring->dev->dev->dma_dev, addr)))
-		return 1;
+		return true;
 
 	switch (ring->type) {
 	case B43legacy_DMA_30BIT:
@@ -441,13 +441,13 @@ static bool b43legacy_dma_mapping_error(struct b43legacy_dmaring *ring,
 	}
 
 	/* The address is OK. */
-	return 0;
+	return false;
 
 address_error:
 	/* We can't support this address. Unmap it again. */
 	unmap_descbuffer(ring, addr, buffersize, dma_to_device);
 
-	return 1;
+	return true;
 }
 
 static int setup_rx_descbuffer(struct b43legacy_dmaring *ring,
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 4e58c0069830..c77b7f59505c 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -2866,7 +2866,7 @@ static void b43legacy_op_bss_info_changed(struct ieee80211_hw *hw,
 		if (conf->bssid)
 			memcpy(wl->bssid, conf->bssid, ETH_ALEN);
 		else
-			memset(wl->bssid, 0, ETH_ALEN);
+			eth_zero_addr(wl->bssid);
 	}
 
 	if (b43legacy_status(dev) >= B43legacy_STAT_INITIALIZED) {
@@ -3470,7 +3470,7 @@ static void b43legacy_op_remove_interface(struct ieee80211_hw *hw,
 
 	spin_lock_irqsave(&wl->irq_lock, flags);
 	b43legacy_adjust_opmode(dev);
-	memset(wl->mac_addr, 0, ETH_ALEN);
+	eth_zero_addr(wl->mac_addr);
 	b43legacy_upload_card_macaddress(dev);
 	spin_unlock_irqrestore(&wl->irq_lock, flags);
 
@@ -3487,8 +3487,8 @@ static int b43legacy_op_start(struct ieee80211_hw *hw)
 	/* Kill all old instance specific information to make sure
 	 * the card won't use it in the short timeframe between start
 	 * and mac80211 reconfiguring it. */
-	memset(wl->bssid, 0, ETH_ALEN);
-	memset(wl->mac_addr, 0, ETH_ALEN);
+	eth_zero_addr(wl->bssid);
+	eth_zero_addr(wl->mac_addr);
 	wl->filter_flags = 0;
 	wl->beacon0_uploaded = false;
 	wl->beacon1_uploaded = false;
diff --git a/drivers/net/wireless/b43legacy/rfkill.c b/drivers/net/wireless/b43legacy/rfkill.c
index c4559bcbc707..7c1bdbc02569 100644
--- a/drivers/net/wireless/b43legacy/rfkill.c
+++ b/drivers/net/wireless/b43legacy/rfkill.c
@@ -32,7 +32,7 @@ bool b43legacy_is_hw_radio_enabled(struct b43legacy_wldev *dev)
 	if (dev->dev->id.revision >= 3) {
 		if (!(b43legacy_read32(dev, B43legacy_MMIO_RADIO_HWENABLED_HI)
 		      & B43legacy_MMIO_RADIO_HWENABLED_HI_MASK))
-			return 1;
+			return true;
 	} else {
 		/* To prevent CPU fault on PPC, do not read a register
 		 * unless the interface is started; however, on resume
@@ -40,12 +40,12 @@ bool b43legacy_is_hw_radio_enabled(struct b43legacy_wldev *dev)
 		 * that happens, unconditionally return TRUE.
 		 */
 		if (b43legacy_status(dev) < B43legacy_STAT_STARTED)
-			return 1;
+			return true;
 		if (b43legacy_read16(dev, B43legacy_MMIO_RADIO_HWENABLED_LO)
 		    & B43legacy_MMIO_RADIO_HWENABLED_LO_MASK)
-			return 1;
+			return true;
 	}
-	return 0;
+	return false;
 }
 
 /* The poll callback for the hardware button. */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index 7944224e3fc9..9b508bd3b839 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -29,6 +29,7 @@
 #include <linux/mmc/host.h>
 #include <linux/platform_device.h>
 #include <linux/platform_data/brcmfmac-sdio.h>
+#include <linux/pm_runtime.h>
 #include <linux/suspend.h>
 #include <linux/errno.h>
 #include <linux/module.h>
@@ -58,6 +59,14 @@
 #define BRCMF_DEFAULT_TXGLOM_SIZE	32  /* max tx frames in glom chain */
 #define BRCMF_DEFAULT_RXGLOM_SIZE	32  /* max rx frames in glom chain */
 
+struct brcmf_sdiod_freezer {
+	atomic_t freezing;
+	atomic_t thread_count;
+	u32 frozen_count;
+	wait_queue_head_t thread_freeze;
+	struct completion resumed;
+};
+
 static int brcmf_sdiod_txglomsz = BRCMF_DEFAULT_TXGLOM_SIZE;
 module_param_named(txglomsz, brcmf_sdiod_txglomsz, int, 0);
 MODULE_PARM_DESC(txglomsz, "maximum tx packet chain size [SDIO]");
@@ -197,6 +206,30 @@ int brcmf_sdiod_intr_unregister(struct brcmf_sdio_dev *sdiodev)
 	return 0;
 }
 
+void brcmf_sdiod_change_state(struct brcmf_sdio_dev *sdiodev,
+			      enum brcmf_sdiod_state state)
+{
+	if (sdiodev->state == BRCMF_SDIOD_NOMEDIUM ||
+	    state == sdiodev->state)
+		return;
+
+	brcmf_dbg(TRACE, "%d -> %d\n", sdiodev->state, state);
+	switch (sdiodev->state) {
+	case BRCMF_SDIOD_DATA:
+		/* any other state means bus interface is down */
+		brcmf_bus_change_state(sdiodev->bus_if, BRCMF_BUS_DOWN);
+		break;
+	case BRCMF_SDIOD_DOWN:
+		/* transition from DOWN to DATA means bus interface is up */
+		if (state == BRCMF_SDIOD_DATA)
+			brcmf_bus_change_state(sdiodev->bus_if, BRCMF_BUS_UP);
+		break;
+	default:
+		break;
+	}
+	sdiodev->state = state;
+}
+
 static inline int brcmf_sdiod_f0_writeb(struct sdio_func *func,
 					uint regaddr, u8 byte)
 {
@@ -269,12 +302,6 @@ static int brcmf_sdiod_request_data(struct brcmf_sdio_dev *sdiodev, u8 fn,
 	return ret;
 }
 
-static void brcmf_sdiod_nomedium_state(struct brcmf_sdio_dev *sdiodev)
-{
-	sdiodev->state = BRCMF_STATE_NOMEDIUM;
-	brcmf_bus_change_state(sdiodev->bus_if, BRCMF_BUS_DOWN);
-}
-
 static int brcmf_sdiod_regrw_helper(struct brcmf_sdio_dev *sdiodev, u32 addr,
 				   u8 regsz, void *data, bool write)
 {
@@ -282,7 +309,7 @@ static int brcmf_sdiod_regrw_helper(struct brcmf_sdio_dev *sdiodev, u32 addr,
 	s32 retry = 0;
 	int ret;
 
-	if (sdiodev->state == BRCMF_STATE_NOMEDIUM)
+	if (sdiodev->state == BRCMF_SDIOD_NOMEDIUM)
 		return -ENOMEDIUM;
 
 	/*
@@ -308,7 +335,7 @@ static int brcmf_sdiod_regrw_helper(struct brcmf_sdio_dev *sdiodev, u32 addr,
 		 retry++ < SDIOH_API_ACCESS_RETRY_LIMIT);
 
 	if (ret == -ENOMEDIUM)
-		brcmf_sdiod_nomedium_state(sdiodev);
+		brcmf_sdiod_change_state(sdiodev, BRCMF_SDIOD_NOMEDIUM);
 	else if (ret != 0) {
 		/*
 		 * SleepCSR register access can fail when
@@ -331,7 +358,7 @@ brcmf_sdiod_set_sbaddr_window(struct brcmf_sdio_dev *sdiodev, u32 address)
 	int err = 0, i;
 	u8 addr[3];
 
-	if (sdiodev->state == BRCMF_STATE_NOMEDIUM)
+	if (sdiodev->state == BRCMF_SDIOD_NOMEDIUM)
 		return -ENOMEDIUM;
 
 	addr[0] = (address >> 8) & SBSDIO_SBADDRLOW_MASK;
@@ -460,7 +487,7 @@ static int brcmf_sdiod_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
 		err = sdio_readsb(sdiodev->func[fn], ((u8 *)(pkt->data)), addr,
 				  req_sz);
 	if (err == -ENOMEDIUM)
-		brcmf_sdiod_nomedium_state(sdiodev);
+		brcmf_sdiod_change_state(sdiodev, BRCMF_SDIOD_NOMEDIUM);
 	return err;
 }
 
@@ -595,7 +622,7 @@ static int brcmf_sdiod_sglist_rw(struct brcmf_sdio_dev *sdiodev, uint fn,
 
 		ret = mmc_cmd.error ? mmc_cmd.error : mmc_dat.error;
 		if (ret == -ENOMEDIUM) {
-			brcmf_sdiod_nomedium_state(sdiodev);
+			brcmf_sdiod_change_state(sdiodev, BRCMF_SDIOD_NOMEDIUM);
 			break;
 		} else if (ret != 0) {
 			brcmf_err("CMD53 sg block %s failed %d\n",
@@ -877,6 +904,87 @@ static void brcmf_sdiod_sgtable_alloc(struct brcmf_sdio_dev *sdiodev)
 	sdiodev->txglomsz = brcmf_sdiod_txglomsz;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int brcmf_sdiod_freezer_attach(struct brcmf_sdio_dev *sdiodev)
+{
+	sdiodev->freezer = kzalloc(sizeof(*sdiodev->freezer), GFP_KERNEL);
+	if (!sdiodev->freezer)
+		return -ENOMEM;
+	atomic_set(&sdiodev->freezer->thread_count, 0);
+	atomic_set(&sdiodev->freezer->freezing, 0);
+	init_waitqueue_head(&sdiodev->freezer->thread_freeze);
+	init_completion(&sdiodev->freezer->resumed);
+	return 0;
+}
+
+static void brcmf_sdiod_freezer_detach(struct brcmf_sdio_dev *sdiodev)
+{
+	if (sdiodev->freezer) {
+		WARN_ON(atomic_read(&sdiodev->freezer->freezing));
+		kfree(sdiodev->freezer);
+	}
+}
+
+static int brcmf_sdiod_freezer_on(struct brcmf_sdio_dev *sdiodev)
+{
+	atomic_t *expect = &sdiodev->freezer->thread_count;
+	int res = 0;
+
+	sdiodev->freezer->frozen_count = 0;
+	reinit_completion(&sdiodev->freezer->resumed);
+	atomic_set(&sdiodev->freezer->freezing, 1);
+	brcmf_sdio_trigger_dpc(sdiodev->bus);
+	wait_event(sdiodev->freezer->thread_freeze,
+		   atomic_read(expect) == sdiodev->freezer->frozen_count);
+	sdio_claim_host(sdiodev->func[1]);
+	res = brcmf_sdio_sleep(sdiodev->bus, true);
+	sdio_release_host(sdiodev->func[1]);
+	return res;
+}
+
+static void brcmf_sdiod_freezer_off(struct brcmf_sdio_dev *sdiodev)
+{
+	sdio_claim_host(sdiodev->func[1]);
+	brcmf_sdio_sleep(sdiodev->bus, false);
+	sdio_release_host(sdiodev->func[1]);
+	atomic_set(&sdiodev->freezer->freezing, 0);
+	complete_all(&sdiodev->freezer->resumed);
+}
+
+bool brcmf_sdiod_freezing(struct brcmf_sdio_dev *sdiodev)
+{
+	return atomic_read(&sdiodev->freezer->freezing);
+}
+
+void brcmf_sdiod_try_freeze(struct brcmf_sdio_dev *sdiodev)
+{
+	if (!brcmf_sdiod_freezing(sdiodev))
+		return;
+	sdiodev->freezer->frozen_count++;
+	wake_up(&sdiodev->freezer->thread_freeze);
+	wait_for_completion(&sdiodev->freezer->resumed);
+}
+
+void brcmf_sdiod_freezer_count(struct brcmf_sdio_dev *sdiodev)
+{
+	atomic_inc(&sdiodev->freezer->thread_count);
+}
+
+void brcmf_sdiod_freezer_uncount(struct brcmf_sdio_dev *sdiodev)
+{
+	atomic_dec(&sdiodev->freezer->thread_count);
+}
+#else
+static int brcmf_sdiod_freezer_attach(struct brcmf_sdio_dev *sdiodev)
+{
+	return 0;
+}
+
+static void brcmf_sdiod_freezer_detach(struct brcmf_sdio_dev *sdiodev)
+{
+}
+#endif /* CONFIG_PM_SLEEP */
+
 static int brcmf_sdiod_remove(struct brcmf_sdio_dev *sdiodev)
 {
 	if (sdiodev->bus) {
@@ -884,6 +992,8 @@ static int brcmf_sdiod_remove(struct brcmf_sdio_dev *sdiodev)
 		sdiodev->bus = NULL;
 	}
 
+	brcmf_sdiod_freezer_detach(sdiodev);
+
 	/* Disable Function 2 */
 	sdio_claim_host(sdiodev->func[2]);
 	sdio_disable_func(sdiodev->func[2]);
@@ -897,6 +1007,7 @@ static int brcmf_sdiod_remove(struct brcmf_sdio_dev *sdiodev)
 	sg_free_table(&sdiodev->sgtable);
 	sdiodev->sbwad = 0;
 
+	pm_runtime_allow(sdiodev->func[1]->card->host->parent);
 	return 0;
 }
 
@@ -955,13 +1066,17 @@ static int brcmf_sdiod_probe(struct brcmf_sdio_dev *sdiodev)
 	 */
 	brcmf_sdiod_sgtable_alloc(sdiodev);
 
+	ret = brcmf_sdiod_freezer_attach(sdiodev);
+	if (ret)
+		goto out;
+
 	/* try to attach to the target device */
 	sdiodev->bus = brcmf_sdio_probe(sdiodev);
 	if (!sdiodev->bus) {
 		ret = -ENODEV;
 		goto out;
 	}
-
+	pm_runtime_forbid(host->parent);
 out:
 	if (ret)
 		brcmf_sdiod_remove(sdiodev);
@@ -983,6 +1098,8 @@ static const struct sdio_device_id brcmf_sdmmc_ids[] = {
 	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43341),
 	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43362),
 	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4335_4339),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_43430),
+	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4345),
 	BRCMF_SDIO_DEVICE(SDIO_DEVICE_ID_BROADCOM_4354),
 	{ /* end: all zeroes */ }
 };
@@ -1050,9 +1167,7 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
 		bus_if->wowl_supported = true;
 #endif
 
-	sdiodev->sleeping = false;
-	atomic_set(&sdiodev->suspend, false);
-	init_waitqueue_head(&sdiodev->idle_wait);
+	brcmf_sdiod_change_state(sdiodev, BRCMF_SDIOD_DOWN);
 
 	brcmf_dbg(SDIO, "F2 found, calling brcmf_sdiod_probe...\n");
 	err = brcmf_sdiod_probe(sdiodev);
@@ -1083,7 +1198,7 @@ static void brcmf_ops_sdio_remove(struct sdio_func *func)
 	brcmf_dbg(SDIO, "sdio device ID: 0x%04x\n", func->device);
 	brcmf_dbg(SDIO, "Function: %d\n", func->num);
 
-	if (func->num != 1 && func->num != 2)
+	if (func->num != 1)
 		return;
 
 	bus_if = dev_get_drvdata(&func->dev);
@@ -1114,24 +1229,22 @@ void brcmf_sdio_wowl_config(struct device *dev, bool enabled)
 #ifdef CONFIG_PM_SLEEP
 static int brcmf_ops_sdio_suspend(struct device *dev)
 {
+	struct sdio_func *func;
 	struct brcmf_bus *bus_if;
 	struct brcmf_sdio_dev *sdiodev;
 	mmc_pm_flag_t sdio_flags;
 
-	brcmf_dbg(SDIO, "Enter\n");
+	func = container_of(dev, struct sdio_func, dev);
+	brcmf_dbg(SDIO, "Enter: F%d\n", func->num);
+	if (func->num != SDIO_FUNC_1)
+		return 0;
+
 
 	bus_if = dev_get_drvdata(dev);
 	sdiodev = bus_if->bus_priv.sdio;
 
-	/* wait for watchdog to go idle */
-	if (wait_event_timeout(sdiodev->idle_wait, sdiodev->sleeping,
-			       msecs_to_jiffies(3 * BRCMF_WD_POLL_MS)) == 0) {
-		brcmf_err("bus still active\n");
-		return -EBUSY;
-	}
-	/* disable watchdog */
+	brcmf_sdiod_freezer_on(sdiodev);
 	brcmf_sdio_wd_timer(sdiodev->bus, 0);
-	atomic_set(&sdiodev->suspend, true);
 
 	if (sdiodev->wowl_enabled) {
 		sdio_flags = MMC_PM_KEEP_POWER;
@@ -1149,12 +1262,13 @@ static int brcmf_ops_sdio_resume(struct device *dev)
 {
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
+	struct sdio_func *func = container_of(dev, struct sdio_func, dev);
 
-	brcmf_dbg(SDIO, "Enter\n");
-	if (sdiodev->pdata && sdiodev->pdata->oob_irq_supported)
-		disable_irq_wake(sdiodev->pdata->oob_irq_nr);
-	brcmf_sdio_wd_timer(sdiodev->bus, BRCMF_WD_POLL_MS);
-	atomic_set(&sdiodev->suspend, false);
+	brcmf_dbg(SDIO, "Enter: F%d\n", func->num);
+	if (func->num != SDIO_FUNC_2)
+		return 0;
+
+	brcmf_sdiod_freezer_off(sdiodev);
 	return 0;
 }
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
index b59b8c6c42ab..8a15ebbce4a3 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
@@ -625,6 +625,7 @@ static bool brcmf_is_ibssmode(struct brcmf_cfg80211_vif *vif)
 
 static struct wireless_dev *brcmf_cfg80211_add_iface(struct wiphy *wiphy,
 						     const char *name,
+						     unsigned char name_assign_type,
 						     enum nl80211_iftype type,
 						     u32 *flags,
 						     struct vif_params *params)
@@ -648,7 +649,7 @@ static struct wireless_dev *brcmf_cfg80211_add_iface(struct wiphy *wiphy,
 	case NL80211_IFTYPE_P2P_CLIENT:
 	case NL80211_IFTYPE_P2P_GO:
 	case NL80211_IFTYPE_P2P_DEVICE:
-		wdev = brcmf_p2p_add_vif(wiphy, name, type, flags, params);
+		wdev = brcmf_p2p_add_vif(wiphy, name, name_assign_type, type, flags, params);
 		if (!IS_ERR(wdev))
 			brcmf_cfg80211_update_proto_addr_mode(wdev);
 		return wdev;
@@ -700,7 +701,7 @@ s32 brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
 		/* Do a scan abort to stop the driver's scan engine */
 		brcmf_dbg(SCAN, "ABORT scan in firmware\n");
 		memset(&params_le, 0, sizeof(params_le));
-		memset(params_le.bssid, 0xFF, ETH_ALEN);
+		eth_broadcast_addr(params_le.bssid);
 		params_le.bss_type = DOT11_BSSTYPE_ANY;
 		params_le.scan_type = 0;
 		params_le.channel_num = cpu_to_le32(1);
@@ -866,7 +867,7 @@ static void brcmf_escan_prep(struct brcmf_cfg80211_info *cfg,
 	char *ptr;
 	struct brcmf_ssid_le ssid_le;
 
-	memset(params_le->bssid, 0xFF, ETH_ALEN);
+	eth_broadcast_addr(params_le->bssid);
 	params_le->bss_type = DOT11_BSSTYPE_ANY;
 	params_le->scan_type = 0;
 	params_le->channel_num = 0;
@@ -1050,10 +1051,6 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct brcmf_cfg80211_vif *vif,
 	if (vif == cfg->p2p.bss_idx[P2PAPI_BSSCFG_DEVICE].vif)
 		vif = cfg->p2p.bss_idx[P2PAPI_BSSCFG_PRIMARY].vif;
 
-	/* Arm scan timeout timer */
-	mod_timer(&cfg->escan_timeout, jiffies +
-			WL_ESCAN_TIMER_INTERVAL_MS * HZ / 1000);
-
 	escan_req = false;
 	if (request) {
 		/* scan bss */
@@ -1112,12 +1109,14 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct brcmf_cfg80211_vif *vif,
 		}
 	}
 
+	/* Arm scan timeout timer */
+	mod_timer(&cfg->escan_timeout, jiffies +
+			WL_ESCAN_TIMER_INTERVAL_MS * HZ / 1000);
+
 	return 0;
 
 scan_out:
 	clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
-	if (timer_pending(&cfg->escan_timeout))
-		del_timer_sync(&cfg->escan_timeout);
 	cfg->scan_request = NULL;
 	return err;
 }
@@ -1375,8 +1374,8 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 				   BRCMF_ASSOC_PARAMS_FIXED_SIZE;
 		memcpy(profile->bssid, params->bssid, ETH_ALEN);
 	} else {
-		memset(join_params.params_le.bssid, 0xFF, ETH_ALEN);
-		memset(profile->bssid, 0, ETH_ALEN);
+		eth_broadcast_addr(join_params.params_le.bssid);
+		eth_zero_addr(profile->bssid);
 	}
 
 	/* Channel */
@@ -1850,7 +1849,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 	if (sme->bssid)
 		memcpy(&ext_join_params->assoc_le.bssid, sme->bssid, ETH_ALEN);
 	else
-		memset(&ext_join_params->assoc_le.bssid, 0xFF, ETH_ALEN);
+		eth_broadcast_addr(ext_join_params->assoc_le.bssid);
 
 	if (cfg->channel) {
 		ext_join_params->assoc_le.chanspec_num = cpu_to_le32(1);
@@ -1895,7 +1894,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 	if (sme->bssid)
 		memcpy(join_params.params_le.bssid, sme->bssid, ETH_ALEN);
 	else
-		memset(join_params.params_le.bssid, 0xFF, ETH_ALEN);
+		eth_broadcast_addr(join_params.params_le.bssid);
 
 	if (cfg->channel) {
 		join_params.params_le.chanspec_list[0] = cpu_to_le16(chanspec);
@@ -2252,7 +2251,6 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *ndev,
 
 	if (key_idx >= BRCMF_MAX_DEFAULT_KEYS) {
 		/* we ignore this key index in this case */
-		brcmf_err("invalid key index (%d)\n", key_idx);
 		return -EINVAL;
 	}
 
@@ -4272,7 +4270,7 @@ brcmf_cfg80211_del_station(struct wiphy *wiphy, struct net_device *ndev,
 		return -EIO;
 
 	memcpy(&scbval.ea, params->mac, ETH_ALEN);
-	scbval.val = cpu_to_le32(WLAN_REASON_DEAUTH_LEAVING);
+	scbval.val = cpu_to_le32(params->reason_code);
 	err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCB_DEAUTHENTICATE_FOR_REASON,
 				     &scbval, sizeof(scbval));
 	if (err)
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.c b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
index 04d2ca0d87d6..ab2fac8b2760 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
@@ -100,9 +100,6 @@
 #define BCM4329_CORE_SOCRAM_BASE	0x18003000
 /* ARM Cortex M3 core, ID 0x82a */
 #define BCM4329_CORE_ARM_BASE		0x18002000
-#define BCM4329_RAMSIZE			0x48000
-/* bcm43143 */
-#define BCM43143_RAMSIZE		0x70000
 
 #define CORE_SB(base, field) \
 		(base + SBCONFIGOFF + offsetof(struct sbconfig, field))
@@ -150,6 +147,78 @@ struct sbconfig {
 	u32 sbidhigh;	/* identification */
 };
 
+/* bankidx and bankinfo reg defines corerev >= 8 */
+#define SOCRAM_BANKINFO_RETNTRAM_MASK	0x00010000
+#define SOCRAM_BANKINFO_SZMASK		0x0000007f
+#define SOCRAM_BANKIDX_ROM_MASK		0x00000100
+
+#define SOCRAM_BANKIDX_MEMTYPE_SHIFT	8
+/* socram bankinfo memtype */
+#define SOCRAM_MEMTYPE_RAM		0
+#define SOCRAM_MEMTYPE_R0M		1
+#define SOCRAM_MEMTYPE_DEVRAM		2
+
+#define SOCRAM_BANKINFO_SZBASE		8192
+#define SRCI_LSS_MASK		0x00f00000
+#define SRCI_LSS_SHIFT		20
+#define	SRCI_SRNB_MASK		0xf0
+#define	SRCI_SRNB_SHIFT		4
+#define	SRCI_SRBSZ_MASK		0xf
+#define	SRCI_SRBSZ_SHIFT	0
+#define SR_BSZ_BASE		14
+
+struct sbsocramregs {
+	u32 coreinfo;
+	u32 bwalloc;
+	u32 extracoreinfo;
+	u32 biststat;
+	u32 bankidx;
+	u32 standbyctrl;
+
+	u32 errlogstatus;	/* rev 6 */
+	u32 errlogaddr;	/* rev 6 */
+	/* used for patching rev 3 & 5 */
+	u32 cambankidx;
+	u32 cambankstandbyctrl;
+	u32 cambankpatchctrl;
+	u32 cambankpatchtblbaseaddr;
+	u32 cambankcmdreg;
+	u32 cambankdatareg;
+	u32 cambankmaskreg;
+	u32 PAD[1];
+	u32 bankinfo;	/* corev 8 */
+	u32 bankpda;
+	u32 PAD[14];
+	u32 extmemconfig;
+	u32 extmemparitycsr;
+	u32 extmemparityerrdata;
+	u32 extmemparityerrcnt;
+	u32 extmemwrctrlandsize;
+	u32 PAD[84];
+	u32 workaround;
+	u32 pwrctl;		/* corerev >= 2 */
+	u32 PAD[133];
+	u32 sr_control;     /* corerev >= 15 */
+	u32 sr_status;      /* corerev >= 15 */
+	u32 sr_address;     /* corerev >= 15 */
+	u32 sr_data;        /* corerev >= 15 */
+};
+
+#define SOCRAMREGOFFS(_f)	offsetof(struct sbsocramregs, _f)
+
+#define ARMCR4_CAP		(0x04)
+#define ARMCR4_BANKIDX		(0x40)
+#define ARMCR4_BANKINFO		(0x44)
+#define ARMCR4_BANKPDA		(0x4C)
+
+#define	ARMCR4_TCBBNB_MASK	0xf0
+#define	ARMCR4_TCBBNB_SHIFT	4
+#define	ARMCR4_TCBANB_MASK	0xf
+#define	ARMCR4_TCBANB_SHIFT	0
+
+#define	ARMCR4_BSZ_MASK		0x3f
+#define	ARMCR4_BSZ_MULT		8192
+
 struct brcmf_core_priv {
 	struct brcmf_core pub;
 	u32 wrapbase;
@@ -419,13 +488,13 @@ static struct brcmf_core *brcmf_chip_add_core(struct brcmf_chip_priv *ci,
 	return &core->pub;
 }
 
-#ifdef DEBUG
 /* safety check for chipinfo */
 static int brcmf_chip_cores_check(struct brcmf_chip_priv *ci)
 {
 	struct brcmf_core_priv *core;
 	bool need_socram = false;
 	bool has_socram = false;
+	bool cpu_found = false;
 	int idx = 1;
 
 	list_for_each_entry(core, &ci->cores, list) {
@@ -435,22 +504,24 @@ static int brcmf_chip_cores_check(struct brcmf_chip_priv *ci)
 
 		switch (core->pub.id) {
 		case BCMA_CORE_ARM_CM3:
+			cpu_found = true;
 			need_socram = true;
 			break;
 		case BCMA_CORE_INTERNAL_MEM:
 			has_socram = true;
 			break;
 		case BCMA_CORE_ARM_CR4:
-			if (ci->pub.rambase == 0) {
-				brcmf_err("RAM base not provided with ARM CR4 core\n");
-				return -ENOMEM;
-			}
+			cpu_found = true;
 			break;
 		default:
 			break;
 		}
 	}
 
+	if (!cpu_found) {
+		brcmf_err("CPU core not detected\n");
+		return -ENXIO;
+	}
 	/* check RAM core presence for ARM CM3 core */
 	if (need_socram && !has_socram) {
 		brcmf_err("RAM core not provided with ARM CM3 core\n");
@@ -458,56 +529,164 @@ static int brcmf_chip_cores_check(struct brcmf_chip_priv *ci)
 	}
 	return 0;
 }
-#else	/* DEBUG */
-static inline int brcmf_chip_cores_check(struct brcmf_chip_priv *ci)
+
+static u32 brcmf_chip_core_read32(struct brcmf_core_priv *core, u16 reg)
 {
-	return 0;
+	return core->chip->ops->read32(core->chip->ctx, core->pub.base + reg);
 }
-#endif
 
-static void brcmf_chip_get_raminfo(struct brcmf_chip_priv *ci)
+static void brcmf_chip_core_write32(struct brcmf_core_priv *core,
+				    u16 reg, u32 val)
 {
-	switch (ci->pub.chip) {
-	case BRCM_CC_4329_CHIP_ID:
-		ci->pub.ramsize = BCM4329_RAMSIZE;
-		break;
-	case BRCM_CC_43143_CHIP_ID:
-		ci->pub.ramsize = BCM43143_RAMSIZE;
-		break;
-	case BRCM_CC_43241_CHIP_ID:
-		ci->pub.ramsize = 0x90000;
-		break;
-	case BRCM_CC_4330_CHIP_ID:
-		ci->pub.ramsize = 0x48000;
-		break;
+	core->chip->ops->write32(core->chip->ctx, core->pub.base + reg, val);
+}
+
+static bool brcmf_chip_socram_banksize(struct brcmf_core_priv *core, u8 idx,
+				       u32 *banksize)
+{
+	u32 bankinfo;
+	u32 bankidx = (SOCRAM_MEMTYPE_RAM << SOCRAM_BANKIDX_MEMTYPE_SHIFT);
+
+	bankidx |= idx;
+	brcmf_chip_core_write32(core, SOCRAMREGOFFS(bankidx), bankidx);
+	bankinfo = brcmf_chip_core_read32(core, SOCRAMREGOFFS(bankinfo));
+	*banksize = (bankinfo & SOCRAM_BANKINFO_SZMASK) + 1;
+	*banksize *= SOCRAM_BANKINFO_SZBASE;
+	return !!(bankinfo & SOCRAM_BANKINFO_RETNTRAM_MASK);
+}
+
+static void brcmf_chip_socram_ramsize(struct brcmf_core_priv *sr, u32 *ramsize,
+				      u32 *srsize)
+{
+	u32 coreinfo;
+	uint nb, banksize, lss;
+	bool retent;
+	int i;
+
+	*ramsize = 0;
+	*srsize = 0;
+
+	if (WARN_ON(sr->pub.rev < 4))
+		return;
+
+	if (!brcmf_chip_iscoreup(&sr->pub))
+		brcmf_chip_resetcore(&sr->pub, 0, 0, 0);
+
+	/* Get info for determining size */
+	coreinfo = brcmf_chip_core_read32(sr, SOCRAMREGOFFS(coreinfo));
+	nb = (coreinfo & SRCI_SRNB_MASK) >> SRCI_SRNB_SHIFT;
+
+	if ((sr->pub.rev <= 7) || (sr->pub.rev == 12)) {
+		banksize = (coreinfo & SRCI_SRBSZ_MASK);
+		lss = (coreinfo & SRCI_LSS_MASK) >> SRCI_LSS_SHIFT;
+		if (lss != 0)
+			nb--;
+		*ramsize = nb * (1 << (banksize + SR_BSZ_BASE));
+		if (lss != 0)
+			*ramsize += (1 << ((lss - 1) + SR_BSZ_BASE));
+	} else {
+		nb = (coreinfo & SRCI_SRNB_MASK) >> SRCI_SRNB_SHIFT;
+		for (i = 0; i < nb; i++) {
+			retent = brcmf_chip_socram_banksize(sr, i, &banksize);
+			*ramsize += banksize;
+			if (retent)
+				*srsize += banksize;
+		}
+	}
+
+	/* hardcoded save&restore memory sizes */
+	switch (sr->chip->pub.chip) {
 	case BRCM_CC_4334_CHIP_ID:
-	case BRCM_CC_43340_CHIP_ID:
-		ci->pub.ramsize = 0x80000;
+		if (sr->chip->pub.chiprev < 2)
+			*srsize = (32 * 1024);
 		break;
-	case BRCM_CC_4335_CHIP_ID:
-		ci->pub.ramsize = 0xc0000;
-		ci->pub.rambase = 0x180000;
+	case BRCM_CC_43430_CHIP_ID:
+		/* assume sr for now as we can not check
+		 * firmware sr capability at this point.
+		 */
+		*srsize = (64 * 1024);
 		break;
-	case BRCM_CC_43362_CHIP_ID:
-		ci->pub.ramsize = 0x3c000;
+	default:
 		break;
+	}
+}
+
+/** Return the TCM-RAM size of the ARMCR4 core. */
+static u32 brcmf_chip_tcm_ramsize(struct brcmf_core_priv *cr4)
+{
+	u32 corecap;
+	u32 memsize = 0;
+	u32 nab;
+	u32 nbb;
+	u32 totb;
+	u32 bxinfo;
+	u32 idx;
+
+	corecap = brcmf_chip_core_read32(cr4, ARMCR4_CAP);
+
+	nab = (corecap & ARMCR4_TCBANB_MASK) >> ARMCR4_TCBANB_SHIFT;
+	nbb = (corecap & ARMCR4_TCBBNB_MASK) >> ARMCR4_TCBBNB_SHIFT;
+	totb = nab + nbb;
+
+	for (idx = 0; idx < totb; idx++) {
+		brcmf_chip_core_write32(cr4, ARMCR4_BANKIDX, idx);
+		bxinfo = brcmf_chip_core_read32(cr4, ARMCR4_BANKINFO);
+		memsize += ((bxinfo & ARMCR4_BSZ_MASK) + 1) * ARMCR4_BSZ_MULT;
+	}
+
+	return memsize;
+}
+
+static u32 brcmf_chip_tcm_rambase(struct brcmf_chip_priv *ci)
+{
+	switch (ci->pub.chip) {
+	case BRCM_CC_4345_CHIP_ID:
+		return 0x198000;
+	case BRCM_CC_4335_CHIP_ID:
 	case BRCM_CC_4339_CHIP_ID:
 	case BRCM_CC_4354_CHIP_ID:
 	case BRCM_CC_4356_CHIP_ID:
 	case BRCM_CC_43567_CHIP_ID:
 	case BRCM_CC_43569_CHIP_ID:
 	case BRCM_CC_43570_CHIP_ID:
-		ci->pub.ramsize = 0xc0000;
-		ci->pub.rambase = 0x180000;
-		break;
 	case BRCM_CC_43602_CHIP_ID:
-		ci->pub.ramsize = 0xf0000;
-		ci->pub.rambase = 0x180000;
-		break;
+		return 0x180000;
 	default:
 		brcmf_err("unknown chip: %s\n", ci->pub.name);
 		break;
 	}
+	return 0;
+}
+
+static int brcmf_chip_get_raminfo(struct brcmf_chip_priv *ci)
+{
+	struct brcmf_core_priv *mem_core;
+	struct brcmf_core *mem;
+
+	mem = brcmf_chip_get_core(&ci->pub, BCMA_CORE_ARM_CR4);
+	if (mem) {
+		mem_core = container_of(mem, struct brcmf_core_priv, pub);
+		ci->pub.ramsize = brcmf_chip_tcm_ramsize(mem_core);
+		ci->pub.rambase = brcmf_chip_tcm_rambase(ci);
+		if (!ci->pub.rambase) {
+			brcmf_err("RAM base not provided with ARM CR4 core\n");
+			return -EINVAL;
+		}
+	} else {
+		mem = brcmf_chip_get_core(&ci->pub, BCMA_CORE_INTERNAL_MEM);
+		mem_core = container_of(mem, struct brcmf_core_priv, pub);
+		brcmf_chip_socram_ramsize(mem_core, &ci->pub.ramsize,
+					  &ci->pub.srsize);
+	}
+	brcmf_dbg(INFO, "RAM: base=0x%x size=%d (0x%x) sr=%d (0x%x)\n",
+		  ci->pub.rambase, ci->pub.ramsize, ci->pub.ramsize,
+		  ci->pub.srsize, ci->pub.srsize);
+
+	if (!ci->pub.ramsize) {
+		brcmf_err("RAM size is undetermined\n");
+		return -ENOMEM;
+	}
+	return 0;
 }
 
 static u32 brcmf_chip_dmp_get_desc(struct brcmf_chip_priv *ci, u32 *eromaddr,
@@ -660,6 +839,7 @@ static int brcmf_chip_recognition(struct brcmf_chip_priv *ci)
 	struct brcmf_core *core;
 	u32 regdata;
 	u32 socitype;
+	int ret;
 
 	/* Get CC core rev
 	 * Chipid is assume to be at offset 0 from SI_ENUM_BASE
@@ -712,9 +892,13 @@ static int brcmf_chip_recognition(struct brcmf_chip_priv *ci)
 		return -ENODEV;
 	}
 
-	brcmf_chip_get_raminfo(ci);
+	ret = brcmf_chip_cores_check(ci);
+	if (ret)
+		return ret;
 
-	return brcmf_chip_cores_check(ci);
+	/* assure chip is passive for core access */
+	brcmf_chip_set_passive(&ci->pub);
+	return brcmf_chip_get_raminfo(ci);
 }
 
 static void brcmf_chip_disable_arm(struct brcmf_chip_priv *chip, u16 id)
@@ -778,12 +962,6 @@ static int brcmf_chip_setup(struct brcmf_chip_priv *chip)
 	if (chip->ops->setup)
 		ret = chip->ops->setup(chip->ctx, pub);
 
-	/*
-	 * Make sure any on-chip ARM is off (in case strapping is wrong),
-	 * or downloaded code was already running.
-	 */
-	brcmf_chip_disable_arm(chip, BCMA_CORE_ARM_CM3);
-	brcmf_chip_disable_arm(chip, BCMA_CORE_ARM_CR4);
 	return ret;
 }
 
@@ -799,7 +977,7 @@ struct brcmf_chip *brcmf_chip_attach(void *ctx,
 		err = -EINVAL;
 	if (WARN_ON(!ops->prepare))
 		err = -EINVAL;
-	if (WARN_ON(!ops->exit_dl))
+	if (WARN_ON(!ops->activate))
 		err = -EINVAL;
 	if (err < 0)
 		return ERR_PTR(-EINVAL);
@@ -897,9 +1075,10 @@ void brcmf_chip_resetcore(struct brcmf_core *pub, u32 prereset, u32 reset,
 }
 
 static void
-brcmf_chip_cm3_enterdl(struct brcmf_chip_priv *chip)
+brcmf_chip_cm3_set_passive(struct brcmf_chip_priv *chip)
 {
 	struct brcmf_core *core;
+	struct brcmf_core_priv *sr;
 
 	brcmf_chip_disable_arm(chip, BCMA_CORE_ARM_CM3);
 	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_80211);
@@ -909,9 +1088,16 @@ brcmf_chip_cm3_enterdl(struct brcmf_chip_priv *chip)
 			     D11_BCMA_IOCTL_PHYCLOCKEN);
 	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_INTERNAL_MEM);
 	brcmf_chip_resetcore(core, 0, 0, 0);
+
+	/* disable bank #3 remap for this device */
+	if (chip->pub.chip == BRCM_CC_43430_CHIP_ID) {
+		sr = container_of(core, struct brcmf_core_priv, pub);
+		brcmf_chip_core_write32(sr, SOCRAMREGOFFS(bankidx), 3);
+		brcmf_chip_core_write32(sr, SOCRAMREGOFFS(bankpda), 0);
+	}
 }
 
-static bool brcmf_chip_cm3_exitdl(struct brcmf_chip_priv *chip)
+static bool brcmf_chip_cm3_set_active(struct brcmf_chip_priv *chip)
 {
 	struct brcmf_core *core;
 
@@ -921,7 +1107,7 @@ static bool brcmf_chip_cm3_exitdl(struct brcmf_chip_priv *chip)
 		return false;
 	}
 
-	chip->ops->exit_dl(chip->ctx, &chip->pub, 0);
+	chip->ops->activate(chip->ctx, &chip->pub, 0);
 
 	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_ARM_CM3);
 	brcmf_chip_resetcore(core, 0, 0, 0);
@@ -930,7 +1116,7 @@ static bool brcmf_chip_cm3_exitdl(struct brcmf_chip_priv *chip)
 }
 
 static inline void
-brcmf_chip_cr4_enterdl(struct brcmf_chip_priv *chip)
+brcmf_chip_cr4_set_passive(struct brcmf_chip_priv *chip)
 {
 	struct brcmf_core *core;
 
@@ -943,11 +1129,11 @@ brcmf_chip_cr4_enterdl(struct brcmf_chip_priv *chip)
 			     D11_BCMA_IOCTL_PHYCLOCKEN);
 }
 
-static bool brcmf_chip_cr4_exitdl(struct brcmf_chip_priv *chip, u32 rstvec)
+static bool brcmf_chip_cr4_set_active(struct brcmf_chip_priv *chip, u32 rstvec)
 {
 	struct brcmf_core *core;
 
-	chip->ops->exit_dl(chip->ctx, &chip->pub, rstvec);
+	chip->ops->activate(chip->ctx, &chip->pub, rstvec);
 
 	/* restore ARM */
 	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_ARM_CR4);
@@ -956,7 +1142,7 @@ static bool brcmf_chip_cr4_exitdl(struct brcmf_chip_priv *chip, u32 rstvec)
 	return true;
 }
 
-void brcmf_chip_enter_download(struct brcmf_chip *pub)
+void brcmf_chip_set_passive(struct brcmf_chip *pub)
 {
 	struct brcmf_chip_priv *chip;
 	struct brcmf_core *arm;
@@ -966,14 +1152,14 @@ void brcmf_chip_enter_download(struct brcmf_chip *pub)
 	chip = container_of(pub, struct brcmf_chip_priv, pub);
 	arm = brcmf_chip_get_core(pub, BCMA_CORE_ARM_CR4);
 	if (arm) {
-		brcmf_chip_cr4_enterdl(chip);
+		brcmf_chip_cr4_set_passive(chip);
 		return;
 	}
 
-	brcmf_chip_cm3_enterdl(chip);
+	brcmf_chip_cm3_set_passive(chip);
 }
 
-bool brcmf_chip_exit_download(struct brcmf_chip *pub, u32 rstvec)
+bool brcmf_chip_set_active(struct brcmf_chip *pub, u32 rstvec)
 {
 	struct brcmf_chip_priv *chip;
 	struct brcmf_core *arm;
@@ -983,9 +1169,9 @@ bool brcmf_chip_exit_download(struct brcmf_chip *pub, u32 rstvec)
 	chip = container_of(pub, struct brcmf_chip_priv, pub);
 	arm = brcmf_chip_get_core(pub, BCMA_CORE_ARM_CR4);
 	if (arm)
-		return brcmf_chip_cr4_exitdl(chip, rstvec);
+		return brcmf_chip_cr4_set_active(chip, rstvec);
 
-	return brcmf_chip_cm3_exitdl(chip);
+	return brcmf_chip_cm3_set_active(chip);
 }
 
 bool brcmf_chip_sr_capable(struct brcmf_chip *pub)
@@ -1016,6 +1202,10 @@ bool brcmf_chip_sr_capable(struct brcmf_chip *pub)
 		addr = CORE_CC_REG(base, chipcontrol_data);
 		reg = chip->ops->read32(chip->ctx, addr);
 		return (reg & pmu_cc3_mask) != 0;
+	case BRCM_CC_43430_CHIP_ID:
+		addr = CORE_CC_REG(base, sr_control1);
+		reg = chip->ops->read32(chip->ctx, addr);
+		return reg != 0;
 	default:
 		addr = CORE_CC_REG(base, pmucapabilities_ext);
 		reg = chip->ops->read32(chip->ctx, addr);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.h b/drivers/net/wireless/brcm80211/brcmfmac/chip.h
index c32908da90c8..60dcb38fc77a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/chip.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/chip.h
@@ -30,7 +30,8 @@
  * @pmucaps: PMU capabilities.
  * @pmurev: PMU revision.
  * @rambase: RAM base address (only applicable for ARM CR4 chips).
- * @ramsize: amount of RAM on chip.
+ * @ramsize: amount of RAM on chip including retention.
+ * @srsize: amount of retention RAM on chip.
  * @name: string representation of the chip identifier.
  */
 struct brcmf_chip {
@@ -41,6 +42,7 @@ struct brcmf_chip {
 	u32 pmurev;
 	u32 rambase;
 	u32 ramsize;
+	u32 srsize;
 	char name[8];
 };
 
@@ -64,7 +66,7 @@ struct brcmf_core {
  * @write32: write 32-bit value over bus.
  * @prepare: prepare bus for core configuration.
  * @setup: bus-specific core setup.
- * @exit_dl: exit download state.
+ * @active: chip becomes active.
  *	The callback should use the provided @rstvec when non-zero.
  */
 struct brcmf_buscore_ops {
@@ -72,7 +74,7 @@ struct brcmf_buscore_ops {
 	void (*write32)(void *ctx, u32 addr, u32 value);
 	int (*prepare)(void *ctx);
 	int (*setup)(void *ctx, struct brcmf_chip *chip);
-	void (*exit_dl)(void *ctx, struct brcmf_chip *chip, u32 rstvec);
+	void (*activate)(void *ctx, struct brcmf_chip *chip, u32 rstvec);
 };
 
 struct brcmf_chip *brcmf_chip_attach(void *ctx,
@@ -84,8 +86,8 @@ bool brcmf_chip_iscoreup(struct brcmf_core *core);
 void brcmf_chip_coredisable(struct brcmf_core *core, u32 prereset, u32 reset);
 void brcmf_chip_resetcore(struct brcmf_core *core, u32 prereset, u32 reset,
 			  u32 postreset);
-void brcmf_chip_enter_download(struct brcmf_chip *ci);
-bool brcmf_chip_exit_download(struct brcmf_chip *ci, u32 rstvec);
+void brcmf_chip_set_passive(struct brcmf_chip *ci);
+bool brcmf_chip_set_active(struct brcmf_chip *ci, u32 rstvec);
 bool brcmf_chip_sr_capable(struct brcmf_chip *pub);
 
 #endif /* BRCMF_AXIDMP_H */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/core.c b/drivers/net/wireless/brcm80211/brcmfmac/core.c
index 2d6e2cc1b12c..f8f47dcfa886 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/core.c
@@ -944,6 +944,34 @@ fail:
 	return ret;
 }
 
+static int brcmf_revinfo_read(struct seq_file *s, void *data)
+{
+	struct brcmf_bus *bus_if = dev_get_drvdata(s->private);
+	struct brcmf_rev_info *ri = &bus_if->drvr->revinfo;
+	char drev[BRCMU_DOTREV_LEN];
+	char brev[BRCMU_BOARDREV_LEN];
+
+	seq_printf(s, "vendorid: 0x%04x\n", ri->vendorid);
+	seq_printf(s, "deviceid: 0x%04x\n", ri->deviceid);
+	seq_printf(s, "radiorev: %s\n", brcmu_dotrev_str(ri->radiorev, drev));
+	seq_printf(s, "chipnum: %u (%x)\n", ri->chipnum, ri->chipnum);
+	seq_printf(s, "chiprev: %u\n", ri->chiprev);
+	seq_printf(s, "chippkg: %u\n", ri->chippkg);
+	seq_printf(s, "corerev: %u\n", ri->corerev);
+	seq_printf(s, "boardid: 0x%04x\n", ri->boardid);
+	seq_printf(s, "boardvendor: 0x%04x\n", ri->boardvendor);
+	seq_printf(s, "boardrev: %s\n", brcmu_boardrev_str(ri->boardrev, brev));
+	seq_printf(s, "driverrev: %s\n", brcmu_dotrev_str(ri->driverrev, drev));
+	seq_printf(s, "ucoderev: %u\n", ri->ucoderev);
+	seq_printf(s, "bus: %u\n", ri->bus);
+	seq_printf(s, "phytype: %u\n", ri->phytype);
+	seq_printf(s, "phyrev: %u\n", ri->phyrev);
+	seq_printf(s, "anarev: %u\n", ri->anarev);
+	seq_printf(s, "nvramrev: %08x\n", ri->nvramrev);
+
+	return 0;
+}
+
 int brcmf_bus_start(struct device *dev)
 {
 	int ret = -1;
@@ -974,6 +1002,8 @@ int brcmf_bus_start(struct device *dev)
 	if (ret < 0)
 		goto fail;
 
+	brcmf_debugfs_add_entry(drvr, "revinfo", brcmf_revinfo_read);
+
 	/* assure we have chipid before feature attach */
 	if (!bus_if->chip) {
 		bus_if->chip = drvr->revinfo.chipnum;
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/flowring.c b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
index 910fbb561469..eb1325371d3a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
@@ -236,7 +236,7 @@ void brcmf_flowring_delete(struct brcmf_flowring *flow, u8 flowid)
 	brcmf_flowring_block(flow, flowid, false);
 	hash_idx = ring->hash_id;
 	flow->hash[hash_idx].ifidx = BRCMF_FLOWRING_INVALID_IFIDX;
-	memset(flow->hash[hash_idx].mac, 0, ETH_ALEN);
+	eth_zero_addr(flow->hash[hash_idx].mac);
 	flow->rings[flowid] = NULL;
 
 	skb = skb_dequeue(&ring->skblist);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
index 6262612dec45..4ec9811f49c8 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
@@ -481,10 +481,9 @@ static int brcmf_msgbuf_ioctl_resp_wait(struct brcmf_msgbuf *msgbuf)
 
 static void brcmf_msgbuf_ioctl_resp_wake(struct brcmf_msgbuf *msgbuf)
 {
-	if (waitqueue_active(&msgbuf->ioctl_resp_wait)) {
-		msgbuf->ctl_completed = true;
+	msgbuf->ctl_completed = true;
+	if (waitqueue_active(&msgbuf->ioctl_resp_wait))
 		wake_up(&msgbuf->ioctl_resp_wait);
-	}
 }
 
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h
index 77a51b8c1e12..3d513e407e3d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h
@@ -17,11 +17,11 @@
 
 #ifdef CONFIG_BRCMFMAC_PROTO_MSGBUF
 
-#define BRCMF_H2D_MSGRING_CONTROL_SUBMIT_MAX_ITEM	20
-#define BRCMF_H2D_MSGRING_RXPOST_SUBMIT_MAX_ITEM	256
-#define BRCMF_D2H_MSGRING_CONTROL_COMPLETE_MAX_ITEM	20
+#define BRCMF_H2D_MSGRING_CONTROL_SUBMIT_MAX_ITEM	64
+#define BRCMF_H2D_MSGRING_RXPOST_SUBMIT_MAX_ITEM	512
+#define BRCMF_D2H_MSGRING_CONTROL_COMPLETE_MAX_ITEM	64
 #define BRCMF_D2H_MSGRING_TX_COMPLETE_MAX_ITEM		1024
-#define BRCMF_D2H_MSGRING_RX_COMPLETE_MAX_ITEM		256
+#define BRCMF_D2H_MSGRING_RX_COMPLETE_MAX_ITEM		512
 #define BRCMF_H2D_TXFLOWRING_MAX_ITEM			512
 
 #define BRCMF_H2D_MSGRING_CONTROL_SUBMIT_ITEMSIZE	40
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
index effb48ebd864..710fbe570eb2 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
@@ -697,7 +697,7 @@ static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
 	else
 		sparams->scan_type = 1;
 
-	memset(&sparams->bssid, 0xFF, ETH_ALEN);
+	eth_broadcast_addr(sparams->bssid);
 	if (ssid.SSID_len)
 		memcpy(sparams->ssid_le.SSID, ssid.SSID, ssid.SSID_len);
 	sparams->ssid_le.SSID_len = cpu_to_le32(ssid.SSID_len);
@@ -2246,11 +2246,13 @@ static void brcmf_p2p_delete_p2pdev(struct brcmf_p2p_info *p2p,
  *
  * @wiphy: wiphy device of new interface.
  * @name: name of the new interface.
+ * @name_assign_type: origin of the interface name
  * @type: nl80211 interface type.
  * @flags: not used.
  * @params: contains mac address for P2P device.
  */
 struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
+				       unsigned char name_assign_type,
 				       enum nl80211_iftype type, u32 *flags,
 				       struct vif_params *params)
 {
@@ -2310,6 +2312,7 @@ struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
 	}
 
 	strncpy(ifp->ndev->name, name, sizeof(ifp->ndev->name) - 1);
+	ifp->ndev->name_assign_type = name_assign_type;
 	err = brcmf_net_attach(ifp, true);
 	if (err) {
 		brcmf_err("Registering netdevice failed\n");
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/p2p.h b/drivers/net/wireless/brcm80211/brcmfmac/p2p.h
index 6821b26224be..872f382d9e49 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/p2p.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/p2p.h
@@ -149,6 +149,7 @@ struct brcmf_p2p_info {
 s32 brcmf_p2p_attach(struct brcmf_cfg80211_info *cfg);
 void brcmf_p2p_detach(struct brcmf_p2p_info *p2p);
 struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
+				       unsigned char name_assign_type,
 				       enum nl80211_iftype type, u32 *flags,
 				       struct vif_params *params);
 int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
index 61c053a729be..1831ecd0813e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
@@ -47,8 +47,6 @@ enum brcmf_pcie_state {
 
 #define BRCMF_PCIE_43602_FW_NAME		"brcm/brcmfmac43602-pcie.bin"
 #define BRCMF_PCIE_43602_NVRAM_NAME		"brcm/brcmfmac43602-pcie.txt"
-#define BRCMF_PCIE_4354_FW_NAME			"brcm/brcmfmac4354-pcie.bin"
-#define BRCMF_PCIE_4354_NVRAM_NAME		"brcm/brcmfmac4354-pcie.txt"
 #define BRCMF_PCIE_4356_FW_NAME			"brcm/brcmfmac4356-pcie.bin"
 #define BRCMF_PCIE_4356_NVRAM_NAME		"brcm/brcmfmac4356-pcie.txt"
 #define BRCMF_PCIE_43570_FW_NAME		"brcm/brcmfmac43570-pcie.bin"
@@ -187,8 +185,8 @@ enum brcmf_pcie_state {
 
 MODULE_FIRMWARE(BRCMF_PCIE_43602_FW_NAME);
 MODULE_FIRMWARE(BRCMF_PCIE_43602_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4354_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4354_NVRAM_NAME);
+MODULE_FIRMWARE(BRCMF_PCIE_4356_FW_NAME);
+MODULE_FIRMWARE(BRCMF_PCIE_4356_NVRAM_NAME);
 MODULE_FIRMWARE(BRCMF_PCIE_43570_FW_NAME);
 MODULE_FIRMWARE(BRCMF_PCIE_43570_NVRAM_NAME);
 
@@ -509,8 +507,6 @@ static void brcmf_pcie_attach(struct brcmf_pciedev_info *devinfo)
 
 static int brcmf_pcie_enter_download_state(struct brcmf_pciedev_info *devinfo)
 {
-	brcmf_chip_enter_download(devinfo->ci);
-
 	if (devinfo->ci->chip == BRCM_CC_43602_CHIP_ID) {
 		brcmf_pcie_select_core(devinfo, BCMA_CORE_ARM_CR4);
 		brcmf_pcie_write_reg32(devinfo, BRCMF_PCIE_ARMCR4REG_BANKIDX,
@@ -536,7 +532,7 @@ static int brcmf_pcie_exit_download_state(struct brcmf_pciedev_info *devinfo,
 		brcmf_chip_resetcore(core, 0, 0, 0);
 	}
 
-	return !brcmf_chip_exit_download(devinfo->ci, resetintr);
+	return !brcmf_chip_set_active(devinfo->ci, resetintr);
 }
 
 
@@ -653,10 +649,9 @@ static void brcmf_pcie_bus_console_read(struct brcmf_pciedev_info *devinfo)
 			console->log_str[console->log_idx] = ch;
 			console->log_idx++;
 		}
-
 		if (ch == '\n') {
 			console->log_str[console->log_idx] = 0;
-			brcmf_dbg(PCIE, "CONSOLE: %s\n", console->log_str);
+			brcmf_dbg(PCIE, "CONSOLE: %s", console->log_str);
 			console->log_idx = 0;
 		}
 	}
@@ -1328,10 +1323,6 @@ static int brcmf_pcie_get_fwnames(struct brcmf_pciedev_info *devinfo)
 		fw_name = BRCMF_PCIE_43602_FW_NAME;
 		nvram_name = BRCMF_PCIE_43602_NVRAM_NAME;
 		break;
-	case BRCM_CC_4354_CHIP_ID:
-		fw_name = BRCMF_PCIE_4354_FW_NAME;
-		nvram_name = BRCMF_PCIE_4354_NVRAM_NAME;
-		break;
 	case BRCM_CC_4356_CHIP_ID:
 		fw_name = BRCMF_PCIE_4356_FW_NAME;
 		nvram_name = BRCMF_PCIE_4356_NVRAM_NAME;
@@ -1566,8 +1557,8 @@ static int brcmf_pcie_buscoreprep(void *ctx)
 }
 
 
-static void brcmf_pcie_buscore_exitdl(void *ctx, struct brcmf_chip *chip,
-				      u32 rstvec)
+static void brcmf_pcie_buscore_activate(void *ctx, struct brcmf_chip *chip,
+					u32 rstvec)
 {
 	struct brcmf_pciedev_info *devinfo = (struct brcmf_pciedev_info *)ctx;
 
@@ -1577,7 +1568,7 @@ static void brcmf_pcie_buscore_exitdl(void *ctx, struct brcmf_chip *chip,
 
 static const struct brcmf_buscore_ops brcmf_pcie_buscore_ops = {
 	.prepare = brcmf_pcie_buscoreprep,
-	.exit_dl = brcmf_pcie_buscore_exitdl,
+	.activate = brcmf_pcie_buscore_activate,
 	.read32 = brcmf_pcie_buscore_read32,
 	.write32 = brcmf_pcie_buscore_write32,
 };
@@ -1856,7 +1847,6 @@ cleanup:
 	PCI_ANY_ID, PCI_ANY_ID, PCI_CLASS_NETWORK_OTHER << 8, 0xffff00, 0 }
 
 static struct pci_device_id brcmf_pcie_devid_table[] = {
-	BRCMF_PCIE_DEVICE(BRCM_PCIE_4354_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_4356_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43567_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43570_DEVICE_ID),
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
index faec35c899ec..ab0c89833013 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
@@ -432,8 +432,6 @@ struct brcmf_sdio {
 	struct brcmf_sdio_dev *sdiodev;	/* sdio device handler */
 	struct brcmf_chip *ci;	/* Chip info struct */
 
-	u32 ramsize;		/* Size of RAM in SOCRAM (bytes) */
-
 	u32 hostintmask;	/* Copy of Host Interrupt Mask */
 	atomic_t intstatus;	/* Intstatus bits (events) pending */
 	atomic_t fcstate;	/* State of dongle flow-control */
@@ -485,10 +483,9 @@ struct brcmf_sdio {
 #endif				/* DEBUG */
 
 	uint clkstate;		/* State of sd and backplane clock(s) */
-	bool activity;		/* Activity flag for clock down */
 	s32 idletime;		/* Control for activity timeout */
-	s32 idlecount;	/* Activity timeout counter */
-	s32 idleclock;	/* How to set bus driver when idle */
+	s32 idlecount;		/* Activity timeout counter */
+	s32 idleclock;		/* How to set bus driver when idle */
 	bool rxflow_mode;	/* Rx flow control mode */
 	bool rxflow;		/* Is rx flow control on */
 	bool alp_only;		/* Don't use HT clock (ALP only) */
@@ -510,11 +507,13 @@ struct brcmf_sdio {
 
 	struct workqueue_struct *brcmf_wq;
 	struct work_struct datawork;
-	atomic_t dpc_tskcnt;
+	bool dpc_triggered;
+	bool dpc_running;
 
 	bool txoff;		/* Transmit flow-controlled */
 	struct brcmf_sdio_count sdcnt;
 	bool sr_enabled; /* SaveRestore enabled */
+	bool sleeping;
 
 	u8 tx_hdrlen;		/* sdio bus header length for tx packet */
 	bool txglom;		/* host tx glomming enable flag */
@@ -616,6 +615,10 @@ static const struct sdiod_drive_str sdiod_drvstr_tab2_3v3[] = {
 #define BCM43362_NVRAM_NAME		"brcm/brcmfmac43362-sdio.txt"
 #define BCM4339_FIRMWARE_NAME		"brcm/brcmfmac4339-sdio.bin"
 #define BCM4339_NVRAM_NAME		"brcm/brcmfmac4339-sdio.txt"
+#define BCM43430_FIRMWARE_NAME		"brcm/brcmfmac43430-sdio.bin"
+#define BCM43430_NVRAM_NAME		"brcm/brcmfmac43430-sdio.txt"
+#define BCM43455_FIRMWARE_NAME		"brcm/brcmfmac43455-sdio.bin"
+#define BCM43455_NVRAM_NAME		"brcm/brcmfmac43455-sdio.txt"
 #define BCM4354_FIRMWARE_NAME		"brcm/brcmfmac4354-sdio.bin"
 #define BCM4354_NVRAM_NAME		"brcm/brcmfmac4354-sdio.txt"
 
@@ -639,6 +642,10 @@ MODULE_FIRMWARE(BCM43362_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM43362_NVRAM_NAME);
 MODULE_FIRMWARE(BCM4339_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM4339_NVRAM_NAME);
+MODULE_FIRMWARE(BCM43430_FIRMWARE_NAME);
+MODULE_FIRMWARE(BCM43430_NVRAM_NAME);
+MODULE_FIRMWARE(BCM43455_FIRMWARE_NAME);
+MODULE_FIRMWARE(BCM43455_NVRAM_NAME);
 MODULE_FIRMWARE(BCM4354_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM4354_NVRAM_NAME);
 
@@ -668,6 +675,8 @@ static const struct brcmf_firmware_names brcmf_fwname_data[] = {
 	{ BRCM_CC_4335_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4335) },
 	{ BRCM_CC_43362_CHIP_ID, 0xFFFFFFFE, BRCMF_FIRMWARE_NVRAM(BCM43362) },
 	{ BRCM_CC_4339_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4339) },
+	{ BRCM_CC_43430_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM43430) },
+	{ BRCM_CC_4345_CHIP_ID, 0xFFFFFFC0, BRCMF_FIRMWARE_NVRAM(BCM43455) },
 	{ BRCM_CC_4354_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4354) }
 };
 
@@ -958,13 +967,8 @@ static int brcmf_sdio_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 	brcmf_dbg(SDIO, "Enter\n");
 
 	/* Early exit if we're already there */
-	if (bus->clkstate == target) {
-		if (target == CLK_AVAIL) {
-			brcmf_sdio_wd_timer(bus, BRCMF_WD_POLL_MS);
-			bus->activity = true;
-		}
+	if (bus->clkstate == target)
 		return 0;
-	}
 
 	switch (target) {
 	case CLK_AVAIL:
@@ -973,8 +977,6 @@ static int brcmf_sdio_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 			brcmf_sdio_sdclk(bus, true);
 		/* Now request HT Avail on the backplane */
 		brcmf_sdio_htclk(bus, true, pendok);
-		brcmf_sdio_wd_timer(bus, BRCMF_WD_POLL_MS);
-		bus->activity = true;
 		break;
 
 	case CLK_SDONLY:
@@ -986,7 +988,6 @@ static int brcmf_sdio_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 		else
 			brcmf_err("request for %d -> %d\n",
 				  bus->clkstate, target);
-		brcmf_sdio_wd_timer(bus, BRCMF_WD_POLL_MS);
 		break;
 
 	case CLK_NONE:
@@ -995,7 +996,6 @@ static int brcmf_sdio_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 			brcmf_sdio_htclk(bus, false, false);
 		/* Now remove the SD clock */
 		brcmf_sdio_sdclk(bus, false);
-		brcmf_sdio_wd_timer(bus, 0);
 		break;
 	}
 #ifdef DEBUG
@@ -1013,26 +1013,16 @@ brcmf_sdio_bus_sleep(struct brcmf_sdio *bus, bool sleep, bool pendok)
 
 	brcmf_dbg(SDIO, "Enter: request %s currently %s\n",
 		  (sleep ? "SLEEP" : "WAKE"),
-		  (bus->sdiodev->sleeping ? "SLEEP" : "WAKE"));
+		  (bus->sleeping ? "SLEEP" : "WAKE"));
 
 	/* If SR is enabled control bus state with KSO */
 	if (bus->sr_enabled) {
 		/* Done if we're already in the requested state */
-		if (sleep == bus->sdiodev->sleeping)
+		if (sleep == bus->sleeping)
 			goto end;
 
 		/* Going to sleep */
 		if (sleep) {
-			/* Don't sleep if something is pending */
-			if (atomic_read(&bus->intstatus) ||
-			    atomic_read(&bus->ipend) > 0 ||
-			    (!atomic_read(&bus->fcstate) &&
-			    brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol) &&
-			    data_ok(bus))) {
-				 err = -EBUSY;
-				 goto done;
-			}
-
 			clkcsr = brcmf_sdiod_regrb(bus->sdiodev,
 						   SBSDIO_FUNC1_CHIPCLKCSR,
 						   &err);
@@ -1043,11 +1033,7 @@ brcmf_sdio_bus_sleep(struct brcmf_sdio *bus, bool sleep, bool pendok)
 						  SBSDIO_ALP_AVAIL_REQ, &err);
 			}
 			err = brcmf_sdio_kso_control(bus, false);
-			/* disable watchdog */
-			if (!err)
-				brcmf_sdio_wd_timer(bus, 0);
 		} else {
-			bus->idlecount = 0;
 			err = brcmf_sdio_kso_control(bus, true);
 		}
 		if (err) {
@@ -1064,10 +1050,9 @@ end:
 			brcmf_sdio_clkctl(bus, CLK_NONE, pendok);
 	} else {
 		brcmf_sdio_clkctl(bus, CLK_AVAIL, pendok);
+		brcmf_sdio_wd_timer(bus, BRCMF_WD_POLL_MS);
 	}
-	bus->sdiodev->sleeping = sleep;
-	if (sleep)
-		wake_up(&bus->sdiodev->idle_wait);
+	bus->sleeping = sleep;
 	brcmf_dbg(SDIO, "new state %s\n",
 		  (sleep ? "SLEEP" : "WAKE"));
 done:
@@ -1085,44 +1070,47 @@ static inline bool brcmf_sdio_valid_shared_address(u32 addr)
 static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
 				 struct sdpcm_shared *sh)
 {
-	u32 addr;
+	u32 addr = 0;
 	int rv;
 	u32 shaddr = 0;
 	struct sdpcm_shared_le sh_le;
 	__le32 addr_le;
 
-	shaddr = bus->ci->rambase + bus->ramsize - 4;
+	sdio_claim_host(bus->sdiodev->func[1]);
+	brcmf_sdio_bus_sleep(bus, false, false);
 
 	/*
 	 * Read last word in socram to determine
 	 * address of sdpcm_shared structure
 	 */
-	sdio_claim_host(bus->sdiodev->func[1]);
-	brcmf_sdio_bus_sleep(bus, false, false);
-	rv = brcmf_sdiod_ramrw(bus->sdiodev, false, shaddr, (u8 *)&addr_le, 4);
-	sdio_release_host(bus->sdiodev->func[1]);
+	shaddr = bus->ci->rambase + bus->ci->ramsize - 4;
+	if (!bus->ci->rambase && brcmf_chip_sr_capable(bus->ci))
+		shaddr -= bus->ci->srsize;
+	rv = brcmf_sdiod_ramrw(bus->sdiodev, false, shaddr,
+			       (u8 *)&addr_le, 4);
 	if (rv < 0)
-		return rv;
-
-	addr = le32_to_cpu(addr_le);
-
-	brcmf_dbg(SDIO, "sdpcm_shared address 0x%08X\n", addr);
+		goto fail;
 
 	/*
 	 * Check if addr is valid.
 	 * NVRAM length at the end of memory should have been overwritten.
 	 */
+	addr = le32_to_cpu(addr_le);
 	if (!brcmf_sdio_valid_shared_address(addr)) {
-			brcmf_err("invalid sdpcm_shared address 0x%08X\n",
-				  addr);
-			return -EINVAL;
+		brcmf_err("invalid sdpcm_shared address 0x%08X\n", addr);
+		rv = -EINVAL;
+		goto fail;
 	}
 
+	brcmf_dbg(INFO, "sdpcm_shared address 0x%08X\n", addr);
+
 	/* Read hndrte_shared structure */
 	rv = brcmf_sdiod_ramrw(bus->sdiodev, false, addr, (u8 *)&sh_le,
 			       sizeof(struct sdpcm_shared_le));
 	if (rv < 0)
-		return rv;
+		goto fail;
+
+	sdio_release_host(bus->sdiodev->func[1]);
 
 	/* Endianness */
 	sh->flags = le32_to_cpu(sh_le.flags);
@@ -1139,8 +1127,13 @@ static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
 			  sh->flags & SDPCM_SHARED_VERSION_MASK);
 		return -EPROTO;
 	}
-
 	return 0;
+
+fail:
+	brcmf_err("unable to obtain sdpcm_shared info: rv=%d (addr=0x%x)\n",
+		  rv, addr);
+	sdio_release_host(bus->sdiodev->func[1]);
+	return rv;
 }
 
 static void brcmf_sdio_get_console_addr(struct brcmf_sdio *bus)
@@ -1909,7 +1902,7 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 	bus->rxpending = true;
 
 	for (rd->seq_num = bus->rx_seq, rxleft = maxframes;
-	     !bus->rxskip && rxleft && bus->sdiodev->state == BRCMF_STATE_DATA;
+	     !bus->rxskip && rxleft && bus->sdiodev->state == BRCMF_SDIOD_DATA;
 	     rd->seq_num++, rxleft--) {
 
 		/* Handle glomming separately */
@@ -2415,7 +2408,7 @@ static uint brcmf_sdio_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 	}
 
 	/* Deflow-control stack if needed */
-	if ((bus->sdiodev->state == BRCMF_STATE_DATA) &&
+	if ((bus->sdiodev->state == BRCMF_SDIOD_DATA) &&
 	    bus->txoff && (pktq_len(&bus->txq) < TXLOW)) {
 		bus->txoff = false;
 		brcmf_txflowblock(bus->sdiodev->dev, false);
@@ -2503,7 +2496,7 @@ static void brcmf_sdio_bus_stop(struct device *dev)
 		bus->watchdog_tsk = NULL;
 	}
 
-	if (sdiodev->state != BRCMF_STATE_NOMEDIUM) {
+	if (sdiodev->state != BRCMF_SDIOD_NOMEDIUM) {
 		sdio_claim_host(sdiodev->func[1]);
 
 		/* Enable clock for device interrupts */
@@ -2603,21 +2596,6 @@ static int brcmf_sdio_intr_rstatus(struct brcmf_sdio *bus)
 	return ret;
 }
 
-static int brcmf_sdio_pm_resume_wait(struct brcmf_sdio_dev *sdiodev)
-{
-#ifdef CONFIG_PM_SLEEP
-	int retry;
-
-	/* Wait for possible resume to complete */
-	retry = 0;
-	while ((atomic_read(&sdiodev->suspend)) && (retry++ != 50))
-		msleep(20);
-	if (atomic_read(&sdiodev->suspend))
-		return -EIO;
-#endif
-	return 0;
-}
-
 static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 {
 	u32 newstatus = 0;
@@ -2628,9 +2606,6 @@ static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 
 	brcmf_dbg(TRACE, "Enter\n");
 
-	if (brcmf_sdio_pm_resume_wait(bus->sdiodev))
-		return;
-
 	sdio_claim_host(bus->sdiodev->func[1]);
 
 	/* If waiting for HTAVAIL, check status */
@@ -2739,11 +2714,14 @@ static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 	if (bus->ctrl_frame_stat && (bus->clkstate == CLK_AVAIL) &&
 	    data_ok(bus)) {
 		sdio_claim_host(bus->sdiodev->func[1]);
-		err = brcmf_sdio_tx_ctrlframe(bus,  bus->ctrl_frame_buf,
-					      bus->ctrl_frame_len);
+		if (bus->ctrl_frame_stat) {
+			err = brcmf_sdio_tx_ctrlframe(bus,  bus->ctrl_frame_buf,
+						      bus->ctrl_frame_len);
+			bus->ctrl_frame_err = err;
+			wmb();
+			bus->ctrl_frame_stat = false;
+		}
 		sdio_release_host(bus->sdiodev->func[1]);
-		bus->ctrl_frame_err = err;
-		bus->ctrl_frame_stat = false;
 		brcmf_sdio_wait_event_wakeup(bus);
 	}
 	/* Send queued frames (limit 1 if rx may still be pending) */
@@ -2755,15 +2733,25 @@ static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 		brcmf_sdio_sendfromq(bus, framecnt);
 	}
 
-	if ((bus->sdiodev->state != BRCMF_STATE_DATA) || (err != 0)) {
+	if ((bus->sdiodev->state != BRCMF_SDIOD_DATA) || (err != 0)) {
 		brcmf_err("failed backplane access over SDIO, halting operation\n");
 		atomic_set(&bus->intstatus, 0);
+		if (bus->ctrl_frame_stat) {
+			sdio_claim_host(bus->sdiodev->func[1]);
+			if (bus->ctrl_frame_stat) {
+				bus->ctrl_frame_err = -ENODEV;
+				wmb();
+				bus->ctrl_frame_stat = false;
+				brcmf_sdio_wait_event_wakeup(bus);
+			}
+			sdio_release_host(bus->sdiodev->func[1]);
+		}
 	} else if (atomic_read(&bus->intstatus) ||
 		   atomic_read(&bus->ipend) > 0 ||
 		   (!atomic_read(&bus->fcstate) &&
 		    brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol) &&
 		    data_ok(bus))) {
-		atomic_inc(&bus->dpc_tskcnt);
+		bus->dpc_triggered = true;
 	}
 }
 
@@ -2862,11 +2850,7 @@ static int brcmf_sdio_bus_txdata(struct device *dev, struct sk_buff *pkt)
 		qcount[prec] = pktq_plen(&bus->txq, prec);
 #endif
 
-	if (atomic_read(&bus->dpc_tskcnt) == 0) {
-		atomic_inc(&bus->dpc_tskcnt);
-		queue_work(bus->brcmf_wq, &bus->datawork);
-	}
-
+	brcmf_sdio_trigger_dpc(bus);
 	return ret;
 }
 
@@ -2963,23 +2947,27 @@ brcmf_sdio_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 	/* Send from dpc */
 	bus->ctrl_frame_buf = msg;
 	bus->ctrl_frame_len = msglen;
+	wmb();
 	bus->ctrl_frame_stat = true;
-	if (atomic_read(&bus->dpc_tskcnt) == 0) {
-		atomic_inc(&bus->dpc_tskcnt);
-		queue_work(bus->brcmf_wq, &bus->datawork);
-	}
 
+	brcmf_sdio_trigger_dpc(bus);
 	wait_event_interruptible_timeout(bus->ctrl_wait, !bus->ctrl_frame_stat,
 					 msecs_to_jiffies(CTL_DONE_TIMEOUT));
-
-	if (!bus->ctrl_frame_stat) {
+	ret = 0;
+	if (bus->ctrl_frame_stat) {
+		sdio_claim_host(bus->sdiodev->func[1]);
+		if (bus->ctrl_frame_stat) {
+			brcmf_dbg(SDIO, "ctrl_frame timeout\n");
+			bus->ctrl_frame_stat = false;
+			ret = -ETIMEDOUT;
+		}
+		sdio_release_host(bus->sdiodev->func[1]);
+	}
+	if (!ret) {
 		brcmf_dbg(SDIO, "ctrl_frame complete, err=%d\n",
 			  bus->ctrl_frame_err);
+		rmb();
 		ret = bus->ctrl_frame_err;
-	} else {
-		brcmf_dbg(SDIO, "ctrl_frame timeout\n");
-		bus->ctrl_frame_stat = false;
-		ret = -ETIMEDOUT;
 	}
 
 	if (ret)
@@ -3383,9 +3371,6 @@ static int brcmf_sdio_download_firmware(struct brcmf_sdio *bus,
 	sdio_claim_host(bus->sdiodev->func[1]);
 	brcmf_sdio_clkctl(bus, CLK_AVAIL, false);
 
-	/* Keep arm in reset */
-	brcmf_chip_enter_download(bus->ci);
-
 	rstvec = get_unaligned_le32(fw->data);
 	brcmf_dbg(SDIO, "firmware rstvec: %x\n", rstvec);
 
@@ -3405,13 +3390,13 @@ static int brcmf_sdio_download_firmware(struct brcmf_sdio *bus,
 	}
 
 	/* Take arm out of reset */
-	if (!brcmf_chip_exit_download(bus->ci, rstvec)) {
+	if (!brcmf_chip_set_active(bus->ci, rstvec)) {
 		brcmf_err("error getting out of ARM core reset\n");
 		goto err;
 	}
 
 	/* Allow full data communication using DPC from now on. */
-	bus->sdiodev->state = BRCMF_STATE_DATA;
+	brcmf_sdiod_change_state(bus->sdiodev, BRCMF_SDIOD_DATA);
 	bcmerror = 0;
 
 err:
@@ -3548,6 +3533,14 @@ done:
 	return err;
 }
 
+void brcmf_sdio_trigger_dpc(struct brcmf_sdio *bus)
+{
+	if (!bus->dpc_triggered) {
+		bus->dpc_triggered = true;
+		queue_work(bus->brcmf_wq, &bus->datawork);
+	}
+}
+
 void brcmf_sdio_isr(struct brcmf_sdio *bus)
 {
 	brcmf_dbg(TRACE, "Enter\n");
@@ -3557,7 +3550,7 @@ void brcmf_sdio_isr(struct brcmf_sdio *bus)
 		return;
 	}
 
-	if (bus->sdiodev->state != BRCMF_STATE_DATA) {
+	if (bus->sdiodev->state != BRCMF_SDIOD_DATA) {
 		brcmf_err("bus is down. we have nothing to do\n");
 		return;
 	}
@@ -3574,11 +3567,11 @@ void brcmf_sdio_isr(struct brcmf_sdio *bus)
 	if (!bus->intr)
 		brcmf_err("isr w/o interrupt configured!\n");
 
-	atomic_inc(&bus->dpc_tskcnt);
+	bus->dpc_triggered = true;
 	queue_work(bus->brcmf_wq, &bus->datawork);
 }
 
-static bool brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
+static void brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 {
 	brcmf_dbg(TIMER, "Enter\n");
 
@@ -3594,7 +3587,7 @@ static bool brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 		if (!bus->intr ||
 		    (bus->sdcnt.intrcount == bus->sdcnt.lastintrs)) {
 
-			if (atomic_read(&bus->dpc_tskcnt) == 0) {
+			if (!bus->dpc_triggered) {
 				u8 devpend;
 
 				sdio_claim_host(bus->sdiodev->func[1]);
@@ -3602,9 +3595,8 @@ static bool brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 							    SDIO_CCCR_INTx,
 							    NULL);
 				sdio_release_host(bus->sdiodev->func[1]);
-				intstatus =
-				    devpend & (INTR_STATUS_FUNC1 |
-					       INTR_STATUS_FUNC2);
+				intstatus = devpend & (INTR_STATUS_FUNC1 |
+						       INTR_STATUS_FUNC2);
 			}
 
 			/* If there is something, make like the ISR and
@@ -3613,7 +3605,7 @@ static bool brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 				bus->sdcnt.pollcnt++;
 				atomic_set(&bus->ipend, 1);
 
-				atomic_inc(&bus->dpc_tskcnt);
+				bus->dpc_triggered = true;
 				queue_work(bus->brcmf_wq, &bus->datawork);
 			}
 		}
@@ -3623,7 +3615,7 @@ static bool brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 	}
 #ifdef DEBUG
 	/* Poll for console output periodically */
-	if (bus->sdiodev->state == BRCMF_STATE_DATA &&
+	if (bus->sdiodev->state == BRCMF_SDIOD_DATA &&
 	    bus->console_interval != 0) {
 		bus->console.count += BRCMF_WD_POLL_MS;
 		if (bus->console.count >= bus->console_interval) {
@@ -3640,22 +3632,25 @@ static bool brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 #endif				/* DEBUG */
 
 	/* On idle timeout clear activity flag and/or turn off clock */
-	if ((bus->idletime > 0) && (bus->clkstate == CLK_AVAIL)) {
-		if (++bus->idlecount >= bus->idletime) {
-			bus->idlecount = 0;
-			if (bus->activity) {
-				bus->activity = false;
-				brcmf_sdio_wd_timer(bus, BRCMF_WD_POLL_MS);
-			} else {
+	if (!bus->dpc_triggered) {
+		rmb();
+		if ((!bus->dpc_running) && (bus->idletime > 0) &&
+		    (bus->clkstate == CLK_AVAIL)) {
+			bus->idlecount++;
+			if (bus->idlecount > bus->idletime) {
 				brcmf_dbg(SDIO, "idle\n");
 				sdio_claim_host(bus->sdiodev->func[1]);
+				brcmf_sdio_wd_timer(bus, 0);
+				bus->idlecount = 0;
 				brcmf_sdio_bus_sleep(bus, true, false);
 				sdio_release_host(bus->sdiodev->func[1]);
 			}
+		} else {
+			bus->idlecount = 0;
 		}
+	} else {
+		bus->idlecount = 0;
 	}
-
-	return (atomic_read(&bus->ipend) > 0);
 }
 
 static void brcmf_sdio_dataworker(struct work_struct *work)
@@ -3663,9 +3658,18 @@ static void brcmf_sdio_dataworker(struct work_struct *work)
 	struct brcmf_sdio *bus = container_of(work, struct brcmf_sdio,
 					      datawork);
 
-	while (atomic_read(&bus->dpc_tskcnt)) {
-		atomic_set(&bus->dpc_tskcnt, 0);
+	bus->dpc_running = true;
+	wmb();
+	while (ACCESS_ONCE(bus->dpc_triggered)) {
+		bus->dpc_triggered = false;
 		brcmf_sdio_dpc(bus);
+		bus->idlecount = 0;
+	}
+	bus->dpc_running = false;
+	if (brcmf_sdiod_freezing(bus->sdiodev)) {
+		brcmf_sdiod_change_state(bus->sdiodev, BRCMF_SDIOD_DOWN);
+		brcmf_sdiod_try_freeze(bus->sdiodev);
+		brcmf_sdiod_change_state(bus->sdiodev, BRCMF_SDIOD_DATA);
 	}
 }
 
@@ -3784,8 +3788,8 @@ static int brcmf_sdio_buscoreprep(void *ctx)
 	return 0;
 }
 
-static void brcmf_sdio_buscore_exitdl(void *ctx, struct brcmf_chip *chip,
-				      u32 rstvec)
+static void brcmf_sdio_buscore_activate(void *ctx, struct brcmf_chip *chip,
+					u32 rstvec)
 {
 	struct brcmf_sdio_dev *sdiodev = ctx;
 	struct brcmf_core *core;
@@ -3828,7 +3832,7 @@ static void brcmf_sdio_buscore_write32(void *ctx, u32 addr, u32 val)
 
 static const struct brcmf_buscore_ops brcmf_sdio_buscore_ops = {
 	.prepare = brcmf_sdio_buscoreprep,
-	.exit_dl = brcmf_sdio_buscore_exitdl,
+	.activate = brcmf_sdio_buscore_activate,
 	.read32 = brcmf_sdio_buscore_read32,
 	.write32 = brcmf_sdio_buscore_write32,
 };
@@ -3882,13 +3886,6 @@ brcmf_sdio_probe_attach(struct brcmf_sdio *bus)
 		drivestrength = DEFAULT_SDIO_DRIVE_STRENGTH;
 	brcmf_sdio_drivestrengthinit(bus->sdiodev, bus->ci, drivestrength);
 
-	/* Get info on the SOCRAM cores... */
-	bus->ramsize = bus->ci->ramsize;
-	if (!(bus->ramsize)) {
-		brcmf_err("failed to find SOCRAM memory!\n");
-		goto fail;
-	}
-
 	/* Set card control so an SDIO card reset does a WLAN backplane reset */
 	reg_val = brcmf_sdiod_regrb(bus->sdiodev,
 				    SDIO_CCCR_BRCM_CARDCTRL, &err);
@@ -3944,13 +3941,19 @@ static int
 brcmf_sdio_watchdog_thread(void *data)
 {
 	struct brcmf_sdio *bus = (struct brcmf_sdio *)data;
+	int wait;
 
 	allow_signal(SIGTERM);
 	/* Run until signal received */
+	brcmf_sdiod_freezer_count(bus->sdiodev);
 	while (1) {
 		if (kthread_should_stop())
 			break;
-		if (!wait_for_completion_interruptible(&bus->watchdog_wait)) {
+		brcmf_sdiod_freezer_uncount(bus->sdiodev);
+		wait = wait_for_completion_interruptible(&bus->watchdog_wait);
+		brcmf_sdiod_freezer_count(bus->sdiodev);
+		brcmf_sdiod_try_freeze(bus->sdiodev);
+		if (!wait) {
 			brcmf_sdio_bus_watchdog(bus);
 			/* Count the tick for reference */
 			bus->sdcnt.tickcnt++;
@@ -3971,7 +3974,7 @@ brcmf_sdio_watchdog(unsigned long data)
 		/* Reschedule the watchdog */
 		if (bus->wd_timer_valid)
 			mod_timer(&bus->timer,
-				  jiffies + BRCMF_WD_POLL_MS * HZ / 1000);
+				  jiffies + msecs_to_jiffies(BRCMF_WD_POLL_MS));
 	}
 }
 
@@ -4089,6 +4092,7 @@ struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
 {
 	int ret;
 	struct brcmf_sdio *bus;
+	struct workqueue_struct *wq;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
@@ -4117,12 +4121,16 @@ struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
 			bus->sgentry_align = sdiodev->pdata->sd_sgentry_align;
 	}
 
-	INIT_WORK(&bus->datawork, brcmf_sdio_dataworker);
-	bus->brcmf_wq = create_singlethread_workqueue("brcmf_wq");
-	if (bus->brcmf_wq == NULL) {
+	/* single-threaded workqueue */
+	wq = alloc_ordered_workqueue("brcmf_wq/%s", WQ_MEM_RECLAIM,
+				     dev_name(&sdiodev->func[1]->dev));
+	if (!wq) {
 		brcmf_err("insufficient memory to create txworkqueue\n");
 		goto fail;
 	}
+	brcmf_sdiod_freezer_count(sdiodev);
+	INIT_WORK(&bus->datawork, brcmf_sdio_dataworker);
+	bus->brcmf_wq = wq;
 
 	/* attempt to attach to the dongle */
 	if (!(brcmf_sdio_probe_attach(bus))) {
@@ -4143,13 +4151,15 @@ struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
 	/* Initialize watchdog thread */
 	init_completion(&bus->watchdog_wait);
 	bus->watchdog_tsk = kthread_run(brcmf_sdio_watchdog_thread,
-					bus, "brcmf_watchdog");
+					bus, "brcmf_wdog/%s",
+					dev_name(&sdiodev->func[1]->dev));
 	if (IS_ERR(bus->watchdog_tsk)) {
 		pr_warn("brcmf_watchdog thread failed to start\n");
 		bus->watchdog_tsk = NULL;
 	}
 	/* Initialize DPC thread */
-	atomic_set(&bus->dpc_tskcnt, 0);
+	bus->dpc_triggered = false;
+	bus->dpc_running = false;
 
 	/* Assign bus interface call back */
 	bus->sdiodev->bus_if->dev = bus->sdiodev->dev;
@@ -4242,16 +4252,16 @@ void brcmf_sdio_remove(struct brcmf_sdio *bus)
 			destroy_workqueue(bus->brcmf_wq);
 
 		if (bus->ci) {
-			if (bus->sdiodev->state != BRCMF_STATE_NOMEDIUM) {
+			if (bus->sdiodev->state != BRCMF_SDIOD_NOMEDIUM) {
 				sdio_claim_host(bus->sdiodev->func[1]);
+				brcmf_sdio_wd_timer(bus, 0);
 				brcmf_sdio_clkctl(bus, CLK_AVAIL, false);
 				/* Leave the device in state where it is
-				 * 'quiet'. This is done by putting it in
-				 * download_state which essentially resets
-				 * all necessary cores.
+				 * 'passive'. This is done by resetting all
+				 * necessary cores.
 				 */
 				msleep(20);
-				brcmf_chip_enter_download(bus->ci);
+				brcmf_chip_set_passive(bus->ci);
 				brcmf_sdio_clkctl(bus, CLK_NONE, false);
 				sdio_release_host(bus->sdiodev->func[1]);
 			}
@@ -4277,7 +4287,7 @@ void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, uint wdtick)
 	}
 
 	/* don't start the wd until fw is loaded */
-	if (bus->sdiodev->state != BRCMF_STATE_DATA)
+	if (bus->sdiodev->state != BRCMF_SDIOD_DATA)
 		return;
 
 	if (wdtick) {
@@ -4290,16 +4300,28 @@ void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, uint wdtick)
 			   dynamically changed or in the first instance
 			 */
 			bus->timer.expires =
-				jiffies + BRCMF_WD_POLL_MS * HZ / 1000;
+				jiffies + msecs_to_jiffies(BRCMF_WD_POLL_MS);
 			add_timer(&bus->timer);
 
 		} else {
 			/* Re arm the timer, at last watchdog period */
 			mod_timer(&bus->timer,
-				jiffies + BRCMF_WD_POLL_MS * HZ / 1000);
+				jiffies + msecs_to_jiffies(BRCMF_WD_POLL_MS));
 		}
 
 		bus->wd_timer_valid = true;
 		bus->save_ms = wdtick;
 	}
 }
+
+int brcmf_sdio_sleep(struct brcmf_sdio *bus, bool sleep)
+{
+	int ret;
+
+	sdio_claim_host(bus->sdiodev->func[1]);
+	ret = brcmf_sdio_bus_sleep(bus, sleep, false);
+	sdio_release_host(bus->sdiodev->func[1]);
+
+	return ret;
+}
+
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio.h
index ec2586a8425c..7328478b2d7b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio.h
@@ -155,11 +155,17 @@
 /* watchdog polling interval in ms */
 #define BRCMF_WD_POLL_MS	10
 
-/* The state of the bus */
-enum brcmf_sdio_state {
-	BRCMF_STATE_DOWN,	/* Device available, still initialising */
-	BRCMF_STATE_DATA,	/* Ready for data transfers, DPC enabled */
-	BRCMF_STATE_NOMEDIUM	/* No medium access to dongle possible */
+/**
+ * enum brcmf_sdiod_state - the state of the bus.
+ *
+ * @BRCMF_SDIOD_DOWN: Device can be accessed, no DPC.
+ * @BRCMF_SDIOD_DATA: Ready for data transfers, DPC enabled.
+ * @BRCMF_SDIOD_NOMEDIUM: No medium access to dongle possible.
+ */
+enum brcmf_sdiod_state {
+	BRCMF_SDIOD_DOWN,
+	BRCMF_SDIOD_DATA,
+	BRCMF_SDIOD_NOMEDIUM
 };
 
 struct brcmf_sdreg {
@@ -169,15 +175,13 @@ struct brcmf_sdreg {
 };
 
 struct brcmf_sdio;
+struct brcmf_sdiod_freezer;
 
 struct brcmf_sdio_dev {
 	struct sdio_func *func[SDIO_MAX_FUNCS];
 	u8 num_funcs;			/* Supported funcs on client */
 	u32 sbwad;			/* Save backplane window address */
 	struct brcmf_sdio *bus;
-	atomic_t suspend;		/* suspend flag */
-	bool sleeping;
-	wait_queue_head_t idle_wait;
 	struct device *dev;
 	struct brcmf_bus *bus_if;
 	struct brcmfmac_sdio_platform_data *pdata;
@@ -194,7 +198,8 @@ struct brcmf_sdio_dev {
 	char fw_name[BRCMF_FW_PATH_LEN + BRCMF_FW_NAME_LEN];
 	char nvram_name[BRCMF_FW_PATH_LEN + BRCMF_FW_NAME_LEN];
 	bool wowl_enabled;
-	enum brcmf_sdio_state state;
+	enum brcmf_sdiod_state state;
+	struct brcmf_sdiod_freezer *freezer;
 };
 
 /* sdio core registers */
@@ -337,6 +342,28 @@ int brcmf_sdiod_ramrw(struct brcmf_sdio_dev *sdiodev, bool write, u32 address,
 
 /* Issue an abort to the specified function */
 int brcmf_sdiod_abort(struct brcmf_sdio_dev *sdiodev, uint fn);
+void brcmf_sdiod_change_state(struct brcmf_sdio_dev *sdiodev,
+			      enum brcmf_sdiod_state state);
+#ifdef CONFIG_PM_SLEEP
+bool brcmf_sdiod_freezing(struct brcmf_sdio_dev *sdiodev);
+void brcmf_sdiod_try_freeze(struct brcmf_sdio_dev *sdiodev);
+void brcmf_sdiod_freezer_count(struct brcmf_sdio_dev *sdiodev);
+void brcmf_sdiod_freezer_uncount(struct brcmf_sdio_dev *sdiodev);
+#else
+static inline bool brcmf_sdiod_freezing(struct brcmf_sdio_dev *sdiodev)
+{
+	return false;
+}
+static inline void brcmf_sdiod_try_freeze(struct brcmf_sdio_dev *sdiodev)
+{
+}
+static inline void brcmf_sdiod_freezer_count(struct brcmf_sdio_dev *sdiodev)
+{
+}
+static inline void brcmf_sdiod_freezer_uncount(struct brcmf_sdio_dev *sdiodev)
+{
+}
+#endif /* CONFIG_PM_SLEEP */
 
 struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev);
 void brcmf_sdio_remove(struct brcmf_sdio *bus);
@@ -344,5 +371,7 @@ void brcmf_sdio_isr(struct brcmf_sdio *bus);
 
 void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, uint wdtick);
 void brcmf_sdio_wowl_config(struct device *dev, bool enabled);
+int brcmf_sdio_sleep(struct brcmf_sdio *bus, bool sleep);
+void brcmf_sdio_trigger_dpc(struct brcmf_sdio *bus);
 
 #endif /* BRCMFMAC_SDIO_H */
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/brcm80211/brcmsmac/brcms_trace_brcmsmac.h b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac.h
new file mode 100644
index 000000000000..a0da3248b942
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2011 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#if !defined(__TRACE_BRCMSMAC_H) || defined(TRACE_HEADER_MULTI_READ)
+#define __TRACE_BRCMSMAC_H
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM brcmsmac
+
+/*
+ * We define a tracepoint, its arguments, its printk format and its
+ * 'fast binary record' layout.
+ */
+TRACE_EVENT(brcms_timer,
+	/* TPPROTO is the prototype of the function called by this tracepoint */
+	TP_PROTO(struct brcms_timer *t),
+	/*
+	 * TPARGS(firstarg, p) are the parameters names, same as found in the
+	 * prototype.
+	 */
+	TP_ARGS(t),
+	/*
+	 * Fast binary tracing: define the trace record via TP_STRUCT__entry().
+	 * You can think about it like a regular C structure local variable
+	 * definition.
+	 */
+	TP_STRUCT__entry(
+		__field(uint, ms)
+		__field(uint, set)
+		__field(uint, periodic)
+	),
+	TP_fast_assign(
+		__entry->ms = t->ms;
+		__entry->set = t->set;
+		__entry->periodic = t->periodic;
+	),
+	TP_printk(
+		"ms=%u set=%u periodic=%u",
+		__entry->ms, __entry->set, __entry->periodic
+	)
+);
+
+TRACE_EVENT(brcms_dpc,
+	TP_PROTO(unsigned long data),
+	TP_ARGS(data),
+	TP_STRUCT__entry(
+		__field(unsigned long, data)
+	),
+	TP_fast_assign(
+		__entry->data = data;
+	),
+	TP_printk(
+		"data=%p",
+		(void *)__entry->data
+	)
+);
+
+TRACE_EVENT(brcms_macintstatus,
+	TP_PROTO(const struct device *dev, int in_isr, u32 macintstatus,
+		 u32 mask),
+	TP_ARGS(dev, in_isr, macintstatus, mask),
+	TP_STRUCT__entry(
+		__string(dev, dev_name(dev))
+		__field(int, in_isr)
+		__field(u32, macintstatus)
+		__field(u32, mask)
+	),
+	TP_fast_assign(
+		__assign_str(dev, dev_name(dev));
+		__entry->in_isr = in_isr;
+		__entry->macintstatus = macintstatus;
+		__entry->mask = mask;
+	),
+	TP_printk("[%s] in_isr=%d macintstatus=%#x mask=%#x", __get_str(dev),
+		  __entry->in_isr, __entry->macintstatus, __entry->mask)
+);
+#endif /* __TRACE_BRCMSMAC_H */
+
+#ifdef CONFIG_BRCM_TRACING
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE brcms_trace_brcmsmac
+#include <trace/define_trace.h>
+
+#endif /* CONFIG_BRCM_TRACING */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_msg.h b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_msg.h
new file mode 100644
index 000000000000..0e8a69ab909f
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_msg.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2011 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#if !defined(__TRACE_BRCMSMAC_MSG_H) || defined(TRACE_HEADER_MULTI_READ)
+#define __TRACE_BRCMSMAC_MSG_H
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM brcmsmac_msg
+
+#define MAX_MSG_LEN	100
+
+DECLARE_EVENT_CLASS(brcms_msg_event,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf),
+	TP_STRUCT__entry(
+		__dynamic_array(char, msg, MAX_MSG_LEN)
+	),
+	TP_fast_assign(
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       MAX_MSG_LEN, vaf->fmt,
+				       *vaf->va) >= MAX_MSG_LEN);
+	),
+	TP_printk("%s", __get_str(msg))
+);
+
+DEFINE_EVENT(brcms_msg_event, brcms_info,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(brcms_msg_event, brcms_warn,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(brcms_msg_event, brcms_err,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(brcms_msg_event, brcms_crit,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+TRACE_EVENT(brcms_dbg,
+	TP_PROTO(u32 level, const char *func, struct va_format *vaf),
+	TP_ARGS(level, func, vaf),
+	TP_STRUCT__entry(
+		__field(u32, level)
+		__string(func, func)
+		__dynamic_array(char, msg, MAX_MSG_LEN)
+	),
+	TP_fast_assign(
+		__entry->level = level;
+		__assign_str(func, func);
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       MAX_MSG_LEN, vaf->fmt,
+				       *vaf->va) >= MAX_MSG_LEN);
+	),
+	TP_printk("%s: %s", __get_str(func), __get_str(msg))
+);
+#endif /* __TRACE_BRCMSMAC_MSG_H */
+
+#ifdef CONFIG_BRCM_TRACING
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE brcms_trace_brcmsmac_msg
+#include <trace/define_trace.h>
+
+#endif /* CONFIG_BRCM_TRACING */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_tx.h b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_tx.h
new file mode 100644
index 000000000000..cf2cc070f1e5
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_tx.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2011 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#if !defined(__TRACE_BRCMSMAC_TX_H) || defined(TRACE_HEADER_MULTI_READ)
+#define __TRACE_BRCMSMAC_TX_H
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM brcmsmac_tx
+
+TRACE_EVENT(brcms_txdesc,
+	TP_PROTO(const struct device *dev,
+		 void *txh, size_t txh_len),
+	TP_ARGS(dev, txh, txh_len),
+	TP_STRUCT__entry(
+		__string(dev, dev_name(dev))
+		__dynamic_array(u8, txh, txh_len)
+	),
+	TP_fast_assign(
+		__assign_str(dev, dev_name(dev));
+		memcpy(__get_dynamic_array(txh), txh, txh_len);
+	),
+	TP_printk("[%s] txdesc", __get_str(dev))
+);
+
+TRACE_EVENT(brcms_txstatus,
+	TP_PROTO(const struct device *dev, u16 framelen, u16 frameid,
+		 u16 status, u16 lasttxtime, u16 sequence, u16 phyerr,
+		 u16 ackphyrxsh),
+	TP_ARGS(dev, framelen, frameid, status, lasttxtime, sequence, phyerr,
+		ackphyrxsh),
+	TP_STRUCT__entry(
+		__string(dev, dev_name(dev))
+		__field(u16, framelen)
+		__field(u16, frameid)
+		__field(u16, status)
+		__field(u16, lasttxtime)
+		__field(u16, sequence)
+		__field(u16, phyerr)
+		__field(u16, ackphyrxsh)
+	),
+	TP_fast_assign(
+		__assign_str(dev, dev_name(dev));
+		__entry->framelen = framelen;
+		__entry->frameid = frameid;
+		__entry->status = status;
+		__entry->lasttxtime = lasttxtime;
+		__entry->sequence = sequence;
+		__entry->phyerr = phyerr;
+		__entry->ackphyrxsh = ackphyrxsh;
+	),
+	TP_printk("[%s] FrameId %#04x TxStatus %#04x LastTxTime %#04x "
+		  "Seq %#04x PHYTxStatus %#04x RxAck %#04x",
+		  __get_str(dev), __entry->frameid, __entry->status,
+		  __entry->lasttxtime, __entry->sequence, __entry->phyerr,
+		  __entry->ackphyrxsh)
+);
+
+TRACE_EVENT(brcms_ampdu_session,
+	TP_PROTO(const struct device *dev, unsigned max_ampdu_len,
+		 u16 max_ampdu_frames, u16 ampdu_len, u16 ampdu_frames,
+		 u16 dma_len),
+	TP_ARGS(dev, max_ampdu_len, max_ampdu_frames, ampdu_len, ampdu_frames,
+		dma_len),
+	TP_STRUCT__entry(
+		__string(dev, dev_name(dev))
+		__field(unsigned, max_ampdu_len)
+		__field(u16, max_ampdu_frames)
+		__field(u16, ampdu_len)
+		__field(u16, ampdu_frames)
+		__field(u16, dma_len)
+	),
+	TP_fast_assign(
+		__assign_str(dev, dev_name(dev));
+		__entry->max_ampdu_len = max_ampdu_len;
+		__entry->max_ampdu_frames = max_ampdu_frames;
+		__entry->ampdu_len = ampdu_len;
+		__entry->ampdu_frames = ampdu_frames;
+		__entry->dma_len = dma_len;
+	),
+	TP_printk("[%s] ampdu session max_len=%u max_frames=%u len=%u frames=%u dma_len=%u",
+		  __get_str(dev), __entry->max_ampdu_len,
+		  __entry->max_ampdu_frames, __entry->ampdu_len,
+		  __entry->ampdu_frames, __entry->dma_len)
+);
+#endif /* __TRACE_BRCMSMAC_TX_H */
+
+#ifdef CONFIG_BRCM_TRACING
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE brcms_trace_brcmsmac_tx
+#include <trace/define_trace.h>
+
+#endif /* CONFIG_BRCM_TRACING */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.h b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.h
index 871781e6a713..cbf2f06436fc 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.h
@@ -14,9 +14,8 @@
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-#if !defined(__TRACE_BRCMSMAC_H) || defined(TRACE_HEADER_MULTI_READ)
-
-#define __TRACE_BRCMSMAC_H
+#ifndef __BRCMS_TRACE_EVENTS_H
+#define __BRCMS_TRACE_EVENTS_H
 
 #include <linux/types.h>
 #include <linux/device.h>
@@ -34,222 +33,8 @@ static inline void trace_ ## name(proto) {}
 static inline void trace_ ## name(proto) {}
 #endif
 
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM brcmsmac
-
-/*
- * We define a tracepoint, its arguments, its printk format and its
- * 'fast binary record' layout.
- */
-TRACE_EVENT(brcms_timer,
-	/* TPPROTO is the prototype of the function called by this tracepoint */
-	TP_PROTO(struct brcms_timer *t),
-	/*
-	 * TPARGS(firstarg, p) are the parameters names, same as found in the
-	 * prototype.
-	 */
-	TP_ARGS(t),
-	/*
-	 * Fast binary tracing: define the trace record via TP_STRUCT__entry().
-	 * You can think about it like a regular C structure local variable
-	 * definition.
-	 */
-	TP_STRUCT__entry(
-		__field(uint, ms)
-		__field(uint, set)
-		__field(uint, periodic)
-	),
-	TP_fast_assign(
-		__entry->ms = t->ms;
-		__entry->set = t->set;
-		__entry->periodic = t->periodic;
-	),
-	TP_printk(
-		"ms=%u set=%u periodic=%u",
-		__entry->ms, __entry->set, __entry->periodic
-	)
-);
-
-TRACE_EVENT(brcms_dpc,
-	TP_PROTO(unsigned long data),
-	TP_ARGS(data),
-	TP_STRUCT__entry(
-		__field(unsigned long, data)
-	),
-	TP_fast_assign(
-		__entry->data = data;
-	),
-	TP_printk(
-		"data=%p",
-		(void *)__entry->data
-	)
-);
-
-TRACE_EVENT(brcms_macintstatus,
-	TP_PROTO(const struct device *dev, int in_isr, u32 macintstatus,
-		 u32 mask),
-	TP_ARGS(dev, in_isr, macintstatus, mask),
-	TP_STRUCT__entry(
-		__string(dev, dev_name(dev))
-		__field(int, in_isr)
-		__field(u32, macintstatus)
-		__field(u32, mask)
-	),
-	TP_fast_assign(
-		__assign_str(dev, dev_name(dev));
-		__entry->in_isr = in_isr;
-		__entry->macintstatus = macintstatus;
-		__entry->mask = mask;
-	),
-	TP_printk("[%s] in_isr=%d macintstatus=%#x mask=%#x", __get_str(dev),
-		  __entry->in_isr, __entry->macintstatus, __entry->mask)
-);
-
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM brcmsmac_tx
-
-TRACE_EVENT(brcms_txdesc,
-	TP_PROTO(const struct device *dev,
-		 void *txh, size_t txh_len),
-	TP_ARGS(dev, txh, txh_len),
-	TP_STRUCT__entry(
-		__string(dev, dev_name(dev))
-		__dynamic_array(u8, txh, txh_len)
-	),
-	TP_fast_assign(
-		__assign_str(dev, dev_name(dev));
-		memcpy(__get_dynamic_array(txh), txh, txh_len);
-	),
-	TP_printk("[%s] txdesc", __get_str(dev))
-);
-
-TRACE_EVENT(brcms_txstatus,
-	TP_PROTO(const struct device *dev, u16 framelen, u16 frameid,
-		 u16 status, u16 lasttxtime, u16 sequence, u16 phyerr,
-		 u16 ackphyrxsh),
-	TP_ARGS(dev, framelen, frameid, status, lasttxtime, sequence, phyerr,
-		ackphyrxsh),
-	TP_STRUCT__entry(
-		__string(dev, dev_name(dev))
-		__field(u16, framelen)
-		__field(u16, frameid)
-		__field(u16, status)
-		__field(u16, lasttxtime)
-		__field(u16, sequence)
-		__field(u16, phyerr)
-		__field(u16, ackphyrxsh)
-	),
-	TP_fast_assign(
-		__assign_str(dev, dev_name(dev));
-		__entry->framelen = framelen;
-		__entry->frameid = frameid;
-		__entry->status = status;
-		__entry->lasttxtime = lasttxtime;
-		__entry->sequence = sequence;
-		__entry->phyerr = phyerr;
-		__entry->ackphyrxsh = ackphyrxsh;
-	),
-	TP_printk("[%s] FrameId %#04x TxStatus %#04x LastTxTime %#04x "
-		  "Seq %#04x PHYTxStatus %#04x RxAck %#04x",
-		  __get_str(dev), __entry->frameid, __entry->status,
-		  __entry->lasttxtime, __entry->sequence, __entry->phyerr,
-		  __entry->ackphyrxsh)
-);
-
-TRACE_EVENT(brcms_ampdu_session,
-	TP_PROTO(const struct device *dev, unsigned max_ampdu_len,
-		 u16 max_ampdu_frames, u16 ampdu_len, u16 ampdu_frames,
-		 u16 dma_len),
-	TP_ARGS(dev, max_ampdu_len, max_ampdu_frames, ampdu_len, ampdu_frames,
-		dma_len),
-	TP_STRUCT__entry(
-		__string(dev, dev_name(dev))
-		__field(unsigned, max_ampdu_len)
-		__field(u16, max_ampdu_frames)
-		__field(u16, ampdu_len)
-		__field(u16, ampdu_frames)
-		__field(u16, dma_len)
-	),
-	TP_fast_assign(
-		__assign_str(dev, dev_name(dev));
-		__entry->max_ampdu_len = max_ampdu_len;
-		__entry->max_ampdu_frames = max_ampdu_frames;
-		__entry->ampdu_len = ampdu_len;
-		__entry->ampdu_frames = ampdu_frames;
-		__entry->dma_len = dma_len;
-	),
-	TP_printk("[%s] ampdu session max_len=%u max_frames=%u len=%u frames=%u dma_len=%u",
-		  __get_str(dev), __entry->max_ampdu_len,
-		  __entry->max_ampdu_frames, __entry->ampdu_len,
-		  __entry->ampdu_frames, __entry->dma_len)
-);
-
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM brcmsmac_msg
-
-#define MAX_MSG_LEN	100
-
-DECLARE_EVENT_CLASS(brcms_msg_event,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf),
-	TP_STRUCT__entry(
-		__dynamic_array(char, msg, MAX_MSG_LEN)
-	),
-	TP_fast_assign(
-		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
-				       MAX_MSG_LEN, vaf->fmt,
-				       *vaf->va) >= MAX_MSG_LEN);
-	),
-	TP_printk("%s", __get_str(msg))
-);
-
-DEFINE_EVENT(brcms_msg_event, brcms_info,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-DEFINE_EVENT(brcms_msg_event, brcms_warn,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-DEFINE_EVENT(brcms_msg_event, brcms_err,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-DEFINE_EVENT(brcms_msg_event, brcms_crit,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-TRACE_EVENT(brcms_dbg,
-	TP_PROTO(u32 level, const char *func, struct va_format *vaf),
-	TP_ARGS(level, func, vaf),
-	TP_STRUCT__entry(
-		__field(u32, level)
-		__string(func, func)
-		__dynamic_array(char, msg, MAX_MSG_LEN)
-	),
-	TP_fast_assign(
-		__entry->level = level;
-		__assign_str(func, func);
-		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
-				       MAX_MSG_LEN, vaf->fmt,
-				       *vaf->va) >= MAX_MSG_LEN);
-	),
-	TP_printk("%s: %s", __get_str(func), __get_str(msg))
-);
+#include "brcms_trace_brcmsmac.h"
+#include "brcms_trace_brcmsmac_tx.h"
+#include "brcms_trace_brcmsmac_msg.h"
 
 #endif /* __TRACE_BRCMSMAC_H */
-
-#ifdef CONFIG_BRCM_TRACING
-
-#undef TRACE_INCLUDE_PATH
-#define TRACE_INCLUDE_PATH .
-#undef TRACE_INCLUDE_FILE
-#define TRACE_INCLUDE_FILE brcms_trace_events
-
-#include <trace/define_trace.h>
-
-#endif /* CONFIG_BRCM_TRACING */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index eb8584a9c49a..369527e27689 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -4668,7 +4668,7 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	brcms_c_coredisable(wlc_hw);
 
 	/* Match driver "down" state */
-	bcma_core_pci_down(wlc_hw->d11core->bus);
+	bcma_host_pci_down(wlc_hw->d11core->bus);
 
 	/* turn off pll and xtal to match driver "down" state */
 	brcms_b_xtal(wlc_hw, OFF);
@@ -4959,7 +4959,7 @@ static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
 	 * Configure pci/pcmcia here instead of in brcms_c_attach()
 	 * to allow mfg hotswap:  down, hotswap (chip power cycle), up.
 	 */
-	bcma_core_pci_irq_ctl(&wlc_hw->d11core->bus->drv_pci[0], wlc_hw->d11core,
+	bcma_host_pci_irq_ctl(wlc_hw->d11core->bus, wlc_hw->d11core,
 			      true);
 
 	/*
@@ -4969,12 +4969,12 @@ static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
 	 */
 	if (brcms_b_radio_read_hwdisabled(wlc_hw)) {
 		/* put SB PCI in down state again */
-		bcma_core_pci_down(wlc_hw->d11core->bus);
+		bcma_host_pci_down(wlc_hw->d11core->bus);
 		brcms_b_xtal(wlc_hw, OFF);
 		return -ENOMEDIUM;
 	}
 
-	bcma_core_pci_up(wlc_hw->d11core->bus);
+	bcma_host_pci_up(wlc_hw->d11core->bus);
 
 	/* reset the d11 core */
 	brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
@@ -5171,7 +5171,7 @@ static int brcms_b_down_finish(struct brcms_hardware *wlc_hw)
 
 		/* turn off primary xtal and pll */
 		if (!wlc_hw->noreset) {
-			bcma_core_pci_down(wlc_hw->d11core->bus);
+			bcma_host_pci_down(wlc_hw->d11core->bus);
 			brcms_b_xtal(wlc_hw, OFF);
 		}
 	}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
index 941b1e41f366..1c4e9dd57960 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
@@ -2949,5 +2949,5 @@ bool wlc_phy_txpower_ipa_ison(struct brcms_phy_pub *ppi)
 	if (ISNPHY(pi))
 		return wlc_phy_n_txpower_ipa_ison(pi);
 	else
-		return 0;
+		return false;
 }
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
index 5f1366234a0d..93d4cde0eb31 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
@@ -4999,7 +4999,7 @@ void wlc_2064_vco_cal(struct brcms_phy *pi)
 bool wlc_phy_tpc_isenabled_lcnphy(struct brcms_phy *pi)
 {
 	if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
-		return 0;
+		return false;
 	else
 		return (LCNPHY_TX_PWR_CTRL_HW ==
 			wlc_lcnphy_get_tx_pwr_ctrl((pi)));
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
index 084f18f4f950..99dac9b8a082 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
@@ -23041,10 +23041,7 @@ static void wlc_phy_rssi_cal_nphy_rev2(struct brcms_phy *pi, u8 rssi_type)
 	else if (rssi_ctrl_state[0] == RADIO_2055_WBRSSI_G1_SEL)
 		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
 				     NPHY_RSSI_SEL_W1);
-	else if (rssi_ctrl_state[0] == RADIO_2055_WBRSSI_G2_SEL)
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
-				     NPHY_RSSI_SEL_W2);
-	else
+	else /* RADIO_2055_WBRSSI_G2_SEL */
 		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
 				     NPHY_RSSI_SEL_W2);
 	if (rssi_ctrl_state[1] == RADIO_2055_NBRSSI_SEL)
@@ -23053,13 +23050,9 @@ static void wlc_phy_rssi_cal_nphy_rev2(struct brcms_phy *pi, u8 rssi_type)
 	else if (rssi_ctrl_state[1] == RADIO_2055_WBRSSI_G1_SEL)
 		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
 				     NPHY_RSSI_SEL_W1);
-	else if (rssi_ctrl_state[1] == RADIO_2055_WBRSSI_G2_SEL)
+	else /* RADIO_2055_WBRSSI_G1_SEL */
 		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
 				     NPHY_RSSI_SEL_W2);
-	else
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
-				     NPHY_RSSI_SEL_W2);
-
 	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_OFF, rssi_type);
 
 	write_phy_reg(pi, 0x91, rfctrlintc_state[0]);
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
index 2124a17d0bfd..4efdd51af9c8 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
@@ -37,6 +37,8 @@
 #define BRCM_CC_43362_CHIP_ID		43362
 #define BRCM_CC_4335_CHIP_ID		0x4335
 #define BRCM_CC_4339_CHIP_ID		0x4339
+#define BRCM_CC_43430_CHIP_ID		43430
+#define BRCM_CC_4345_CHIP_ID		0x4345
 #define BRCM_CC_4354_CHIP_ID		0x4354
 #define BRCM_CC_4356_CHIP_ID		0x4356
 #define BRCM_CC_43566_CHIP_ID		43566
diff --git a/drivers/net/wireless/brcm80211/include/chipcommon.h b/drivers/net/wireless/brcm80211/include/chipcommon.h
index d242333b7559..e1fd499930a0 100644
--- a/drivers/net/wireless/brcm80211/include/chipcommon.h
+++ b/drivers/net/wireless/brcm80211/include/chipcommon.h
@@ -183,7 +183,14 @@ struct chipcregs {
 	u8 uart1lsr;
 	u8 uart1msr;
 	u8 uart1scratch;
-	u32 PAD[126];
+	u32 PAD[62];
+
+	/* save/restore, corerev >= 48 */
+	u32 sr_capability;          /* 0x500 */
+	u32 sr_control0;            /* 0x504 */
+	u32 sr_control1;            /* 0x508 */
+	u32 gpio_control;           /* 0x50C */
+	u32 PAD[60];
 
 	/* PMU registers (corerev >= 20) */
 	u32 pmucontrol;	/* 0x600 */
diff --git a/drivers/net/wireless/cw1200/cw1200_spi.c b/drivers/net/wireless/cw1200/cw1200_spi.c
index 964b64ab7fe3..7603546d2de3 100644
--- a/drivers/net/wireless/cw1200/cw1200_spi.c
+++ b/drivers/net/wireless/cw1200/cw1200_spi.c
@@ -447,7 +447,7 @@ static int cw1200_spi_disconnect(struct spi_device *func)
 }
 
 #ifdef CONFIG_PM
-static int cw1200_spi_suspend(struct device *dev, pm_message_t state)
+static int cw1200_spi_suspend(struct device *dev)
 {
 	struct hwbus_priv *self = spi_get_drvdata(to_spi_device(dev));
 
@@ -458,10 +458,8 @@ static int cw1200_spi_suspend(struct device *dev, pm_message_t state)
 	return 0;
 }
 
-static int cw1200_spi_resume(struct device *dev)
-{
-	return 0;
-}
+static SIMPLE_DEV_PM_OPS(cw1200_pm_ops, cw1200_spi_suspend, NULL);
+
 #endif
 
 static struct spi_driver spi_driver = {
@@ -472,8 +470,7 @@ static struct spi_driver spi_driver = {
 		.bus            = &spi_bus_type,
 		.owner          = THIS_MODULE,
 #ifdef CONFIG_PM
-		.suspend        = cw1200_spi_suspend,
-		.resume         = cw1200_spi_resume,
+		.pm		= &cw1200_pm_ops,
 #endif
 	},
 };
diff --git a/drivers/net/wireless/cw1200/sta.c b/drivers/net/wireless/cw1200/sta.c
index 4a47c7f8a246..b0f65fa09428 100644
--- a/drivers/net/wireless/cw1200/sta.c
+++ b/drivers/net/wireless/cw1200/sta.c
@@ -293,7 +293,7 @@ void cw1200_remove_interface(struct ieee80211_hw *dev,
 	}
 	priv->vif = NULL;
 	priv->mode = NL80211_IFTYPE_MONITOR;
-	memset(priv->mac_addr, 0, ETH_ALEN);
+	eth_zero_addr(priv->mac_addr);
 	memset(&priv->p2p_ps_modeinfo, 0, sizeof(priv->p2p_ps_modeinfo));
 	cw1200_free_keys(priv);
 	cw1200_setup_mac(priv);
@@ -1240,8 +1240,8 @@ static void cw1200_do_join(struct cw1200_common *priv)
 
 	bssid = priv->vif->bss_conf.bssid;
 
-	bss = cfg80211_get_bss(priv->hw->wiphy, priv->channel,
-			bssid, NULL, 0, 0, 0);
+	bss = cfg80211_get_bss(priv->hw->wiphy, priv->channel, bssid, NULL, 0,
+			       IEEE80211_BSS_TYPE_ANY, IEEE80211_PRIVACY_ANY);
 
 	if (!bss && !conf->ibss_joined) {
 		wsm_unlock_tx(priv);
diff --git a/drivers/net/wireless/cw1200/txrx.c b/drivers/net/wireless/cw1200/txrx.c
index 0bd541175ecd..d28bd49cb5fd 100644
--- a/drivers/net/wireless/cw1200/txrx.c
+++ b/drivers/net/wireless/cw1200/txrx.c
@@ -1429,7 +1429,7 @@ void cw1200_link_id_gc_work(struct work_struct *work)
 				priv->link_id_map &= ~mask;
 				priv->sta_asleep_mask &= ~mask;
 				priv->pspoll_mask &= ~mask;
-				memset(map_link.mac_addr, 0, ETH_ALEN);
+				eth_zero_addr(map_link.mac_addr);
 				spin_unlock_bh(&priv->ps_state_lock);
 				reset.link_id = i + 1;
 				wsm_reset(priv, &reset);
diff --git a/drivers/net/wireless/hostap/hostap_80211_tx.c b/drivers/net/wireless/hostap/hostap_80211_tx.c
index 8bde77689469..055e11d353ca 100644
--- a/drivers/net/wireless/hostap/hostap_80211_tx.c
+++ b/drivers/net/wireless/hostap/hostap_80211_tx.c
@@ -174,8 +174,8 @@ netdev_tx_t hostap_data_start_xmit(struct sk_buff *skb,
 		/* send broadcast and multicast frames to broadcast RA, if
 		 * configured; otherwise, use unicast RA of the WDS link */
 		if ((local->wds_type & HOSTAP_WDS_BROADCAST_RA) &&
-		    skb->data[0] & 0x01)
-			memset(&hdr.addr1, 0xff, ETH_ALEN);
+		    is_multicast_ether_addr(skb->data))
+			eth_broadcast_addr(hdr.addr1);
 		else if (iface->type == HOSTAP_INTERFACE_WDS)
 			memcpy(&hdr.addr1, iface->u.wds.remote_addr,
 			       ETH_ALEN);
diff --git a/drivers/net/wireless/hostap/hostap_ap.c b/drivers/net/wireless/hostap/hostap_ap.c
index fd8d83dd4f62..c995ace153ee 100644
--- a/drivers/net/wireless/hostap/hostap_ap.c
+++ b/drivers/net/wireless/hostap/hostap_ap.c
@@ -309,7 +309,7 @@ void hostap_deauth_all_stas(struct net_device *dev, struct ap_data *ap,
 	int i;
 
 	PDEBUG(DEBUG_AP, "%s: Deauthenticate all stations\n", dev->name);
-	memset(addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(addr);
 
 	resp = cpu_to_le16(WLAN_REASON_PREV_AUTH_NOT_VALID);
 
@@ -1015,8 +1015,8 @@ static void prism2_send_mgmt(struct net_device *dev,
 		memcpy(hdr->addr3, dev->dev_addr, ETH_ALEN); /* SA */
 	} else if (ieee80211_is_ctl(hdr->frame_control)) {
 		/* control:ACK does not have addr2 or addr3 */
-		memset(hdr->addr2, 0, ETH_ALEN);
-		memset(hdr->addr3, 0, ETH_ALEN);
+		eth_zero_addr(hdr->addr2);
+		eth_zero_addr(hdr->addr3);
 	} else {
 		memcpy(hdr->addr2, dev->dev_addr, ETH_ALEN); /* SA */
 		memcpy(hdr->addr3, dev->dev_addr, ETH_ALEN); /* BSSID */
@@ -1601,7 +1601,7 @@ static void handle_assoc(local_info_t *local, struct sk_buff *skb,
 		memcpy(prev_ap, pos, ETH_ALEN);
 		pos++; pos++; pos++; left -= 6;
 	} else
-		memset(prev_ap, 0, ETH_ALEN);
+		eth_zero_addr(prev_ap);
 
 	if (left >= 2) {
 		unsigned int ileft;
diff --git a/drivers/net/wireless/hostap/hostap_info.c b/drivers/net/wireless/hostap/hostap_info.c
index de7c4ffec309..7635ac4f6679 100644
--- a/drivers/net/wireless/hostap/hostap_info.c
+++ b/drivers/net/wireless/hostap/hostap_info.c
@@ -442,7 +442,7 @@ static void handle_info_queue_linkstatus(local_info_t *local)
 	} else {
 		netif_carrier_off(local->dev);
 		netif_carrier_off(local->ddev);
-		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu.ap_addr.sa_data);
 	}
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
 
diff --git a/drivers/net/wireless/hostap/hostap_main.c b/drivers/net/wireless/hostap/hostap_main.c
index 52919ad42726..01de1a3bf94e 100644
--- a/drivers/net/wireless/hostap/hostap_main.c
+++ b/drivers/net/wireless/hostap/hostap_main.c
@@ -224,7 +224,7 @@ int prism2_wds_del(local_info_t *local, u8 *remote_addr,
 
 	if (selected) {
 		if (do_not_remove)
-			memset(selected->u.wds.remote_addr, 0, ETH_ALEN);
+			eth_zero_addr(selected->u.wds.remote_addr);
 		else {
 			hostap_remove_interface(selected->dev, rtnl_locked, 0);
 			local->wds_connections--;
@@ -798,7 +798,6 @@ static void prism2_tx_timeout(struct net_device *dev)
 
 const struct header_ops hostap_80211_ops = {
 	.create		= eth_header,
-	.rebuild	= eth_rebuild_header,
 	.cache		= eth_header_cache,
 	.cache_update	= eth_header_cache_update,
 	.parse		= hostap_80211_header_parse,
@@ -1088,7 +1087,7 @@ int prism2_sta_deauth(local_info_t *local, u16 reason)
 
 	ret = prism2_sta_send_mgmt(local, local->bssid, IEEE80211_STYPE_DEAUTH,
 				   (u8 *) &val, 2);
-	memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+	eth_zero_addr(wrqu.ap_addr.sa_data);
 	wireless_send_event(local->dev, SIOCGIWAP, &wrqu, NULL);
 	return ret;
 }
diff --git a/drivers/net/wireless/hostap/hostap_wlan.h b/drivers/net/wireless/hostap/hostap_wlan.h
index 57904015380f..ca25283e1c92 100644
--- a/drivers/net/wireless/hostap/hostap_wlan.h
+++ b/drivers/net/wireless/hostap/hostap_wlan.h
@@ -4,6 +4,7 @@
 #include <linux/interrupt.h>
 #include <linux/wireless.h>
 #include <linux/netdevice.h>
+#include <linux/etherdevice.h>
 #include <linux/mutex.h>
 #include <net/iw_handler.h>
 #include <net/ieee80211_radiotap.h>
@@ -85,16 +86,16 @@ struct hfa384x_rx_frame {
 	/* 802.11 */
 	__le16 frame_control;
 	__le16 duration_id;
-	u8 addr1[6];
-	u8 addr2[6];
-	u8 addr3[6];
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
 	__le16 seq_ctrl;
-	u8 addr4[6];
+	u8 addr4[ETH_ALEN];
 	__le16 data_len;
 
 	/* 802.3 */
-	u8 dst_addr[6];
-	u8 src_addr[6];
+	u8 dst_addr[ETH_ALEN];
+	u8 src_addr[ETH_ALEN];
 	__be16 len;
 
 	/* followed by frame data; max 2304 bytes */
@@ -114,16 +115,16 @@ struct hfa384x_tx_frame {
 	/* 802.11 */
 	__le16 frame_control; /* parts not used */
 	__le16 duration_id;
-	u8 addr1[6];
-	u8 addr2[6]; /* filled by firmware */
-	u8 addr3[6];
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN]; /* filled by firmware */
+	u8 addr3[ETH_ALEN];
 	__le16 seq_ctrl; /* filled by firmware */
-	u8 addr4[6];
+	u8 addr4[ETH_ALEN];
 	__le16 data_len;
 
 	/* 802.3 */
-	u8 dst_addr[6];
-	u8 src_addr[6];
+	u8 dst_addr[ETH_ALEN];
+	u8 src_addr[ETH_ALEN];
 	__be16 len;
 
 	/* followed by frame data; max 2304 bytes */
@@ -156,7 +157,7 @@ struct hfa384x_hostscan_request {
 } __packed;
 
 struct hfa384x_join_request {
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	__le16 channel;
 } __packed;
 
@@ -228,7 +229,7 @@ struct hfa384x_scan_result {
 	__le16 chid;
 	__le16 anl;
 	__le16 sl;
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	__le16 beacon_interval;
 	__le16 capability;
 	__le16 ssid_len;
@@ -241,7 +242,7 @@ struct hfa384x_hostscan_result {
 	__le16 chid;
 	__le16 anl;
 	__le16 sl;
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	__le16 beacon_interval;
 	__le16 capability;
 	__le16 ssid_len;
@@ -824,7 +825,7 @@ struct local_info {
 #define PRISM2_INFO_PENDING_SCANRESULTS 1
 	int prev_link_status; /* previous received LinkStatus info */
 	int prev_linkstatus_connected;
-	u8 preferred_ap[6]; /* use this AP if possible */
+	u8 preferred_ap[ETH_ALEN]; /* use this AP if possible */
 
 #ifdef PRISM2_CALLBACK
 	void *callback_data; /* Can be used in callbacks; e.g., allocate
diff --git a/drivers/net/wireless/ipw2x00/Kconfig b/drivers/net/wireless/ipw2x00/Kconfig
index 21de4fe6cf2d..d6ec44d7a391 100644
--- a/drivers/net/wireless/ipw2x00/Kconfig
+++ b/drivers/net/wireless/ipw2x00/Kconfig
@@ -66,7 +66,7 @@ config IPW2100_DEBUG
 config IPW2200
 	tristate "Intel PRO/Wireless 2200BG and 2915ABG Network Connection"
 	depends on PCI && CFG80211
-	select CFG80211_WEXT
+	select CFG80211_WEXT_EXPORT
 	select WIRELESS_EXT
 	select WEXT_SPY
 	select WEXT_PRIV
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index 6fabea0309dd..08eb229e7816 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -2147,8 +2147,8 @@ static void isr_indicate_association_lost(struct ipw2100_priv *priv, u32 status)
 		return;
 	}
 
-	memset(priv->bssid, 0, ETH_ALEN);
-	memset(priv->ieee->bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->bssid);
+	eth_zero_addr(priv->ieee->bssid);
 
 	netif_carrier_off(priv->net_dev);
 	netif_stop_queue(priv->net_dev);
@@ -6956,7 +6956,7 @@ static int ipw2100_wx_get_wap(struct net_device *dev,
 		wrqu->ap_addr.sa_family = ARPHRD_ETHER;
 		memcpy(wrqu->ap_addr.sa_data, priv->bssid, ETH_ALEN);
 	} else
-		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu->ap_addr.sa_data);
 
 	IPW_DEBUG_WX("Getting WAP BSSID: %pM\n", wrqu->ap_addr.sa_data);
 	return 0;
@@ -8300,7 +8300,7 @@ static void ipw2100_wx_event_work(struct work_struct *work)
 	    priv->status & STATUS_RF_KILL_MASK ||
 	    ipw2100_get_ordinal(priv, IPW_ORD_STAT_ASSN_AP_BSSID,
 				&priv->bssid, &len)) {
-		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu.ap_addr.sa_data);
 	} else {
 		/* We now have the BSSID, so can finish setting to the full
 		 * associated state */
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index 67cad9b05ad8..39f3e6f5cbcd 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -1964,7 +1964,7 @@ static void notify_wx_assoc_event(struct ipw_priv *priv)
 	if (priv->status & STATUS_ASSOCIATED)
 		memcpy(wrqu.ap_addr.sa_data, priv->bssid, ETH_ALEN);
 	else
-		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu.ap_addr.sa_data);
 	wireless_send_event(priv->net_dev, SIOCGIWAP, &wrqu, NULL);
 }
 
@@ -7400,7 +7400,7 @@ static int ipw_associate_network(struct ipw_priv *priv,
 	memcpy(priv->assoc_request.bssid, network->bssid, ETH_ALEN);
 
 	if (priv->ieee->iw_mode == IW_MODE_ADHOC) {
-		memset(&priv->assoc_request.dest, 0xFF, ETH_ALEN);
+		eth_broadcast_addr(priv->assoc_request.dest);
 		priv->assoc_request.atim_window = cpu_to_le16(network->atim_window);
 	} else {
 		memcpy(priv->assoc_request.dest, network->bssid, ETH_ALEN);
@@ -8986,7 +8986,7 @@ static int ipw_wx_get_wap(struct net_device *dev,
 		wrqu->ap_addr.sa_family = ARPHRD_ETHER;
 		memcpy(wrqu->ap_addr.sa_data, priv->bssid, ETH_ALEN);
 	} else
-		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+		eth_zero_addr(wrqu->ap_addr.sa_data);
 
 	IPW_DEBUG_WX("Getting WAP BSSID: %pM\n",
 		     wrqu->ap_addr.sa_data);
diff --git a/drivers/net/wireless/iwlegacy/4965-rs.c b/drivers/net/wireless/iwlegacy/4965-rs.c
index eaaeea19d8c5..bac60b2bc3f0 100644
--- a/drivers/net/wireless/iwlegacy/4965-rs.c
+++ b/drivers/net/wireless/iwlegacy/4965-rs.c
@@ -1678,7 +1678,7 @@ il4965_rs_stay_in_table(struct il_lq_sta *lq_sta, bool force_search)
 		    lq_sta->total_success > lq_sta->max_success_limit ||
 		    (!lq_sta->search_better_tbl && lq_sta->flush_timer &&
 		     flush_interval_passed)) {
-			D_RATE("LQ: stay is expired %d %d %d\n:",
+			D_RATE("LQ: stay is expired %d %d %d\n",
 			       lq_sta->total_failed, lq_sta->total_success,
 			       flush_interval_passed);
 
diff --git a/drivers/net/wireless/iwlegacy/common.c b/drivers/net/wireless/iwlegacy/common.c
index 2c4fa49686ef..887114582583 100644
--- a/drivers/net/wireless/iwlegacy/common.c
+++ b/drivers/net/wireless/iwlegacy/common.c
@@ -4634,7 +4634,7 @@ il_mac_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	il->vif = NULL;
 	il->iw_mode = NL80211_IFTYPE_UNSPECIFIED;
 	il_teardown_interface(il, vif);
-	memset(il->bssid, 0, ETH_ALEN);
+	eth_zero_addr(il->bssid);
 
 	D_MAC80211("leave\n");
 	mutex_unlock(&il->mutex);
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..5abd62ed8cb4 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -1114,35 +1114,39 @@ 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");
 }
 
-static void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
-				     struct ieee80211_vif *vif,
-				     enum ieee80211_rssi_event rssi_event)
+static void iwlagn_mac_event_callback(struct ieee80211_hw *hw,
+				      struct ieee80211_vif *vif,
+				      const struct ieee80211_event *event)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
+	if (event->type != RSSI_EVENT)
+		return;
+
 	IWL_DEBUG_MAC80211(priv, "enter\n");
 	mutex_lock(&priv->mutex);
 
 	if (priv->lib->bt_params &&
 	    priv->lib->bt_params->advanced_bt_coexist) {
-		if (rssi_event == RSSI_EVENT_LOW)
+		if (event->u.rssi.data == RSSI_EVENT_LOW)
 			priv->bt_enable_pspoll = true;
-		else if (rssi_event == RSSI_EVENT_HIGH)
+		else if (event->u.rssi.data == RSSI_EVENT_HIGH)
 			priv->bt_enable_pspoll = false;
 
 		iwlagn_send_advance_bt_config(priv);
@@ -1613,7 +1617,7 @@ const struct ieee80211_ops iwlagn_hw_ops = {
 	.channel_switch = iwlagn_mac_channel_switch,
 	.flush = iwlagn_mac_flush,
 	.tx_last_beacon = iwlagn_mac_tx_last_beacon,
-	.rssi_callback = iwlagn_mac_rssi_callback,
+	.event_callback = iwlagn_mac_event_callback,
 	.set_tim = iwlagn_mac_set_tim,
 };
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/main.c b/drivers/net/wireless/iwlwifi/dvm/main.c
index c4d6dd7402d9..234e30f498b2 100644
--- a/drivers/net/wireless/iwlwifi/dvm/main.c
+++ b/drivers/net/wireless/iwlwifi/dvm/main.c
@@ -1549,7 +1549,7 @@ static void iwl_dump_nic_error_log(struct iwl_priv *priv)
 				      table.blink1, table.blink2, table.ilink1,
 				      table.ilink2, table.bcon_time, table.gp1,
 				      table.gp2, table.gp3, table.ucode_ver,
-				      table.hw_ver, table.brd_ver);
+				      table.hw_ver, 0, table.brd_ver);
 	IWL_ERR(priv, "0x%08X | %-28s\n", table.error_id,
 		desc_lookup(table.error_id));
 	IWL_ERR(priv, "0x%08X | uPc\n", table.pc);
diff --git a/drivers/net/wireless/iwlwifi/dvm/rs.c b/drivers/net/wireless/iwlwifi/dvm/rs.c
index 32b78a66536d..3bd7c86e90d9 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.c
@@ -3153,12 +3153,13 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 	desc += sprintf(buff+desc, "lq type %s\n",
 	   (is_legacy(tbl->lq_type)) ? "legacy" : "HT");
 	if (is_Ht(tbl->lq_type)) {
-		desc += sprintf(buff+desc, " %s",
+		desc += sprintf(buff + desc, " %s",
 		   (is_siso(tbl->lq_type)) ? "SISO" :
 		   ((is_mimo2(tbl->lq_type)) ? "MIMO2" : "MIMO3"));
-		   desc += sprintf(buff+desc, " %s",
+		desc += sprintf(buff + desc, " %s",
 		   (tbl->is_ht40) ? "40MHz" : "20MHz");
-		   desc += sprintf(buff+desc, " %s %s %s\n", (tbl->is_SGI) ? "SGI" : "",
+		desc += sprintf(buff + desc, " %s %s %s\n",
+		   (tbl->is_SGI) ? "SGI" : "",
 		   (lq_sta->is_green) ? "GF enabled" : "",
 		   (lq_sta->is_agg) ? "AGG on" : "");
 	}
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index 1e40a12de077..275df12a6045 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -189,9 +189,9 @@ static void iwlagn_tx_cmd_build_rate(struct iwl_priv *priv,
 		rate_flags |= RATE_MCS_CCK_MSK;
 
 	/* Set up antennas */
-	 if (priv->lib->bt_params &&
-	     priv->lib->bt_params->advanced_bt_coexist &&
-	     priv->bt_full_concurrent) {
+	if (priv->lib->bt_params &&
+	    priv->lib->bt_params->advanced_bt_coexist &&
+	    priv->bt_full_concurrent) {
 		/* operated as 1x1 in full concurrency mode */
 		priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
 				first_antenna(priv->nvm_data->valid_tx_ant));
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-7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index 97e38d2e2983..36e786f0387b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -69,16 +69,16 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL7260_UCODE_API_MAX	12
-#define IWL3160_UCODE_API_MAX	12
+#define IWL7260_UCODE_API_MAX	13
+#define IWL3160_UCODE_API_MAX	13
 
 /* Oldest version we won't warn about */
-#define IWL7260_UCODE_API_OK	10
-#define IWL3160_UCODE_API_OK	10
+#define IWL7260_UCODE_API_OK	12
+#define IWL3160_UCODE_API_OK	12
 
 /* Lowest firmware API version supported */
-#define IWL7260_UCODE_API_MIN	9
-#define IWL3160_UCODE_API_MIN	9
+#define IWL7260_UCODE_API_MIN	10
+#define IWL3160_UCODE_API_MIN	10
 
 /* NVM versions */
 #define IWL7260_NVM_VERSION		0x0a1d
diff --git a/drivers/net/wireless/iwlwifi/iwl-8000.c b/drivers/net/wireless/iwlwifi/iwl-8000.c
index 2f7fe8167dc9..ce6321b7d241 100644
--- a/drivers/net/wireless/iwlwifi/iwl-8000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-8000.c
@@ -69,13 +69,13 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL8000_UCODE_API_MAX	12
+#define IWL8000_UCODE_API_MAX	13
 
 /* Oldest version we won't warn about */
-#define IWL8000_UCODE_API_OK	10
+#define IWL8000_UCODE_API_OK	12
 
 /* Lowest firmware API version supported */
-#define IWL8000_UCODE_API_MIN	9
+#define IWL8000_UCODE_API_MIN	10
 
 /* NVM versions */
 #define IWL8000_NVM_VERSION		0x0a1d
@@ -94,8 +94,8 @@
 	IWL8000_FW_PRE "-" __stringify(api) ".ucode"
 
 #define NVM_HW_SECTION_NUM_FAMILY_8000		10
-#define DEFAULT_NVM_FILE_FAMILY_8000A		"iwl_nvm_8000.bin"
-#define DEFAULT_NVM_FILE_FAMILY_8000		"iwl_nvm_8000B.bin"
+#define DEFAULT_NVM_FILE_FAMILY_8000B		"nvmData-8000B"
+#define DEFAULT_NVM_FILE_FAMILY_8000C		"nvmData-8000C"
 
 /* Max SDIO RX aggregation size of the ADDBA request/response */
 #define MAX_RX_AGG_SIZE_8260_SDIO	28
@@ -177,8 +177,8 @@ const struct iwl_cfg iwl8260_2ac_sdio_cfg = {
 	.ht_params = &iwl8000_ht_params,
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
-	.default_nvm_file = DEFAULT_NVM_FILE_FAMILY_8000,
-	.default_nvm_file_8000A = DEFAULT_NVM_FILE_FAMILY_8000A,
+	.default_nvm_file_B_step = DEFAULT_NVM_FILE_FAMILY_8000B,
+	.default_nvm_file_C_step = DEFAULT_NVM_FILE_FAMILY_8000C,
 	.max_rx_agg_size = MAX_RX_AGG_SIZE_8260_SDIO,
 	.disable_dummy_notification = true,
 	.max_ht_ampdu_exponent  = MAX_HT_AMPDU_EXPONENT_8260_SDIO,
@@ -192,8 +192,8 @@ const struct iwl_cfg iwl4165_2ac_sdio_cfg = {
 	.ht_params = &iwl8000_ht_params,
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
-	.default_nvm_file = DEFAULT_NVM_FILE_FAMILY_8000,
-	.default_nvm_file_8000A = DEFAULT_NVM_FILE_FAMILY_8000A,
+	.default_nvm_file_B_step = DEFAULT_NVM_FILE_FAMILY_8000B,
+	.default_nvm_file_C_step = DEFAULT_NVM_FILE_FAMILY_8000C,
 	.max_rx_agg_size = MAX_RX_AGG_SIZE_8260_SDIO,
 	.bt_shared_single_ant = true,
 	.disable_dummy_notification = true,
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index 4b190d98a1ec..3f33f753ce2f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -92,9 +92,9 @@ static inline bool iwl_has_secure_boot(u32 hw_rev,
 {
 	/* return 1 only for family 8000 B0 */
 	if ((family == IWL_DEVICE_FAMILY_8000) && (hw_rev & 0xC))
-		return 1;
+		return true;
 
-	return 0;
+	return false;
 }
 
 /*
@@ -228,7 +228,7 @@ struct iwl_pwr_tx_backoff {
 
 /**
  * struct iwl_cfg
- * @name: Offical name of the device
+ * @name: Official name of the device
  * @fw_name_pre: Firmware filename prefix. The api version and extension
  *	(.ucode) will be added to filename before loading from disk. The
  *	filename is constructed as fw_name_pre<api>.ucode.
@@ -303,8 +303,8 @@ struct iwl_cfg {
 	bool lp_xtal_workaround;
 	const struct iwl_pwr_tx_backoff *pwr_tx_backoffs;
 	bool no_power_up_nic_in_init;
-	const char *default_nvm_file;
-	const char *default_nvm_file_8000A;
+	const char *default_nvm_file_B_step;
+	const char *default_nvm_file_C_step;
 	unsigned int max_rx_agg_size;
 	bool disable_dummy_notification;
 	unsigned int max_tx_agg_size;
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index 684254553558..9bb36d79c2bd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -157,6 +157,7 @@ do {                                            			\
 /* 0x0000F000 - 0x00001000 */
 #define IWL_DL_ASSOC		0x00001000
 #define IWL_DL_DROP		0x00002000
+#define IWL_DL_LAR		0x00004000
 #define IWL_DL_COEX		0x00008000
 /* 0x000F0000 - 0x00010000 */
 #define IWL_DL_FW		0x00010000
@@ -219,5 +220,6 @@ do {                                            			\
 #define IWL_DEBUG_POWER(p, f, a...)	IWL_DEBUG(p, IWL_DL_POWER, f, ## a)
 #define IWL_DEBUG_11H(p, f, a...)	IWL_DEBUG(p, IWL_DL_11H, f, ## a)
 #define IWL_DEBUG_RPM(p, f, a...)	IWL_DEBUG(p, IWL_DL_RPM, f, ## a)
+#define IWL_DEBUG_LAR(p, f, a...)	IWL_DEBUG(p, IWL_DL_LAR, f, ## a)
 
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-data.h b/drivers/net/wireless/iwlwifi/iwl-devtrace-data.h
new file mode 100644
index 000000000000..04e6649340b8
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace-data.h
@@ -0,0 +1,79 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#if !defined(__IWLWIFI_DEVICE_TRACE_DATA) || defined(TRACE_HEADER_MULTI_READ)
+#define __IWLWIFI_DEVICE_TRACE_DATA
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi_data
+
+TRACE_EVENT(iwlwifi_dev_tx_data,
+	TP_PROTO(const struct device *dev,
+		 struct sk_buff *skb,
+		 void *data, size_t data_len),
+	TP_ARGS(dev, skb, data, data_len),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+
+		__dynamic_array(u8, data, iwl_trace_data(skb) ? data_len : 0)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		if (iwl_trace_data(skb))
+			memcpy(__get_dynamic_array(data), data, data_len);
+	),
+	TP_printk("[%s] TX frame data", __get_str(dev))
+);
+
+TRACE_EVENT(iwlwifi_dev_rx_data,
+	TP_PROTO(const struct device *dev,
+		 const struct iwl_trans *trans,
+		 void *rxbuf, size_t len),
+	TP_ARGS(dev, trans, rxbuf, len),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+
+		__dynamic_array(u8, data,
+				len - iwl_rx_trace_len(trans, rxbuf, len))
+	),
+	TP_fast_assign(
+		size_t offs = iwl_rx_trace_len(trans, rxbuf, len);
+		DEV_ASSIGN;
+		if (offs < len)
+			memcpy(__get_dynamic_array(data),
+			       ((u8 *)rxbuf) + offs, len - offs);
+	),
+	TP_printk("[%s] RX frame data", __get_str(dev))
+);
+#endif /* __IWLWIFI_DEVICE_TRACE_DATA */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE iwl-devtrace-data
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-io.h b/drivers/net/wireless/iwlwifi/iwl-devtrace-io.h
new file mode 100644
index 000000000000..f62c54485852
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace-io.h
@@ -0,0 +1,155 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#if !defined(__IWLWIFI_DEVICE_TRACE_IO) || defined(TRACE_HEADER_MULTI_READ)
+#define __IWLWIFI_DEVICE_TRACE_IO
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi_io
+
+TRACE_EVENT(iwlwifi_dev_ioread32,
+	TP_PROTO(const struct device *dev, u32 offs, u32 val),
+	TP_ARGS(dev, offs, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, offs)
+		__field(u32, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%s] read io[%#x] = %#x",
+		  __get_str(dev), __entry->offs, __entry->val)
+);
+
+TRACE_EVENT(iwlwifi_dev_iowrite8,
+	TP_PROTO(const struct device *dev, u32 offs, u8 val),
+	TP_ARGS(dev, offs, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, offs)
+		__field(u8, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%s] write io[%#x] = %#x)",
+		  __get_str(dev), __entry->offs, __entry->val)
+);
+
+TRACE_EVENT(iwlwifi_dev_iowrite32,
+	TP_PROTO(const struct device *dev, u32 offs, u32 val),
+	TP_ARGS(dev, offs, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, offs)
+		__field(u32, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%s] write io[%#x] = %#x)",
+		  __get_str(dev), __entry->offs, __entry->val)
+);
+
+TRACE_EVENT(iwlwifi_dev_iowrite_prph32,
+	TP_PROTO(const struct device *dev, u32 offs, u32 val),
+	TP_ARGS(dev, offs, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, offs)
+		__field(u32, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%s] write PRPH[%#x] = %#x)",
+		  __get_str(dev), __entry->offs, __entry->val)
+);
+
+TRACE_EVENT(iwlwifi_dev_ioread_prph32,
+	TP_PROTO(const struct device *dev, u32 offs, u32 val),
+	TP_ARGS(dev, offs, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, offs)
+		__field(u32, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%s] read PRPH[%#x] = %#x",
+		  __get_str(dev), __entry->offs, __entry->val)
+);
+
+TRACE_EVENT(iwlwifi_dev_irq,
+	TP_PROTO(const struct device *dev),
+	TP_ARGS(dev),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+	),
+	/* TP_printk("") doesn't compile */
+	TP_printk("%d", 0)
+);
+
+TRACE_EVENT(iwlwifi_dev_ict_read,
+	TP_PROTO(const struct device *dev, u32 index, u32 value),
+	TP_ARGS(dev, index, value),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, index)
+		__field(u32, value)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->index = index;
+		__entry->value = value;
+	),
+	TP_printk("[%s] read ict[%d] = %#.8x",
+		  __get_str(dev), __entry->index, __entry->value)
+);
+#endif /* __IWLWIFI_DEVICE_TRACE_IO */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE iwl-devtrace-io
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h b/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h
new file mode 100644
index 000000000000..223b8752f924
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h
@@ -0,0 +1,202 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#if !defined(__IWLWIFI_DEVICE_TRACE_IWLWIFI) || defined(TRACE_HEADER_MULTI_READ)
+#define __IWLWIFI_DEVICE_TRACE_IWLWIFI
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi
+
+TRACE_EVENT(iwlwifi_dev_hcmd,
+	TP_PROTO(const struct device *dev,
+		 struct iwl_host_cmd *cmd, u16 total_size,
+		 struct iwl_cmd_header *hdr),
+	TP_ARGS(dev, cmd, total_size, hdr),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__dynamic_array(u8, hcmd, total_size)
+		__field(u32, flags)
+	),
+	TP_fast_assign(
+		int i, offset = sizeof(*hdr);
+
+		DEV_ASSIGN;
+		__entry->flags = cmd->flags;
+		memcpy(__get_dynamic_array(hcmd), hdr, sizeof(*hdr));
+
+		for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
+			if (!cmd->len[i])
+				continue;
+			memcpy((u8 *)__get_dynamic_array(hcmd) + offset,
+			       cmd->data[i], cmd->len[i]);
+			offset += cmd->len[i];
+		}
+	),
+	TP_printk("[%s] hcmd %#.2x (%ssync)",
+		  __get_str(dev), ((u8 *)__get_dynamic_array(hcmd))[0],
+		  __entry->flags & CMD_ASYNC ? "a" : "")
+);
+
+TRACE_EVENT(iwlwifi_dev_rx,
+	TP_PROTO(const struct device *dev, const struct iwl_trans *trans,
+		 void *rxbuf, size_t len),
+	TP_ARGS(dev, trans, rxbuf, len),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__dynamic_array(u8, rxbuf, iwl_rx_trace_len(trans, rxbuf, len))
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		memcpy(__get_dynamic_array(rxbuf), rxbuf,
+		       iwl_rx_trace_len(trans, rxbuf, len));
+	),
+	TP_printk("[%s] RX cmd %#.2x",
+		  __get_str(dev), ((u8 *)__get_dynamic_array(rxbuf))[4])
+);
+
+TRACE_EVENT(iwlwifi_dev_tx,
+	TP_PROTO(const struct device *dev, struct sk_buff *skb,
+		 void *tfd, size_t tfdlen,
+		 void *buf0, size_t buf0_len,
+		 void *buf1, size_t buf1_len),
+	TP_ARGS(dev, skb, tfd, tfdlen, buf0, buf0_len, buf1, buf1_len),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+
+		__field(size_t, framelen)
+		__dynamic_array(u8, tfd, tfdlen)
+
+		/*
+		 * Do not insert between or below these items,
+		 * we want to keep the frame together (except
+		 * for the possible padding).
+		 */
+		__dynamic_array(u8, buf0, buf0_len)
+		__dynamic_array(u8, buf1, iwl_trace_data(skb) ? 0 : buf1_len)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->framelen = buf0_len + buf1_len;
+		memcpy(__get_dynamic_array(tfd), tfd, tfdlen);
+		memcpy(__get_dynamic_array(buf0), buf0, buf0_len);
+		if (!iwl_trace_data(skb))
+			memcpy(__get_dynamic_array(buf1), buf1, buf1_len);
+	),
+	TP_printk("[%s] TX %.2x (%zu bytes)",
+		  __get_str(dev), ((u8 *)__get_dynamic_array(buf0))[0],
+		  __entry->framelen)
+);
+
+TRACE_EVENT(iwlwifi_dev_ucode_error,
+	TP_PROTO(const struct device *dev, u32 desc, u32 tsf_low,
+		 u32 data1, u32 data2, u32 line, u32 blink1,
+		 u32 blink2, u32 ilink1, u32 ilink2, u32 bcon_time,
+		 u32 gp1, u32 gp2, u32 gp3, u32 major, u32 minor, u32 hw_ver,
+		 u32 brd_ver),
+	TP_ARGS(dev, desc, tsf_low, data1, data2, line,
+		blink1, blink2, ilink1, ilink2, bcon_time, gp1, gp2,
+		gp3, major, minor, hw_ver, brd_ver),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, desc)
+		__field(u32, tsf_low)
+		__field(u32, data1)
+		__field(u32, data2)
+		__field(u32, line)
+		__field(u32, blink1)
+		__field(u32, blink2)
+		__field(u32, ilink1)
+		__field(u32, ilink2)
+		__field(u32, bcon_time)
+		__field(u32, gp1)
+		__field(u32, gp2)
+		__field(u32, gp3)
+		__field(u32, major)
+		__field(u32, minor)
+		__field(u32, hw_ver)
+		__field(u32, brd_ver)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->desc = desc;
+		__entry->tsf_low = tsf_low;
+		__entry->data1 = data1;
+		__entry->data2 = data2;
+		__entry->line = line;
+		__entry->blink1 = blink1;
+		__entry->blink2 = blink2;
+		__entry->ilink1 = ilink1;
+		__entry->ilink2 = ilink2;
+		__entry->bcon_time = bcon_time;
+		__entry->gp1 = gp1;
+		__entry->gp2 = gp2;
+		__entry->gp3 = gp3;
+		__entry->major = major;
+		__entry->minor = minor;
+		__entry->hw_ver = hw_ver;
+		__entry->brd_ver = brd_ver;
+	),
+	TP_printk("[%s] #%02d %010u data 0x%08X 0x%08X line %u, "
+		  "blink 0x%05X 0x%05X ilink 0x%05X 0x%05X "
+		  "bcon_tm %010u gp 0x%08X 0x%08X 0x%08X major 0x%08X "
+		  "minor 0x%08X hw 0x%08X brd 0x%08X",
+		  __get_str(dev), __entry->desc, __entry->tsf_low,
+		  __entry->data1,
+		  __entry->data2, __entry->line, __entry->blink1,
+		  __entry->blink2, __entry->ilink1, __entry->ilink2,
+		  __entry->bcon_time, __entry->gp1, __entry->gp2,
+		  __entry->gp3, __entry->major, __entry->minor,
+		  __entry->hw_ver, __entry->brd_ver)
+);
+
+TRACE_EVENT(iwlwifi_dev_ucode_event,
+	TP_PROTO(const struct device *dev, u32 time, u32 data, u32 ev),
+	TP_ARGS(dev, time, data, ev),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+
+		__field(u32, time)
+		__field(u32, data)
+		__field(u32, ev)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->time = time;
+		__entry->data = data;
+		__entry->ev = ev;
+	),
+	TP_printk("[%s] EVT_LOGT:%010u:0x%08x:%04u",
+		  __get_str(dev), __entry->time, __entry->data, __entry->ev)
+);
+#endif /* __IWLWIFI_DEVICE_TRACE_IWLWIFI */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE iwl-devtrace-iwlwifi
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-msg.h b/drivers/net/wireless/iwlwifi/iwl-devtrace-msg.h
new file mode 100644
index 000000000000..a3b3c2465f89
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace-msg.h
@@ -0,0 +1,97 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#if !defined(__IWLWIFI_DEVICE_TRACE_MSG) || defined(TRACE_HEADER_MULTI_READ)
+#define __IWLWIFI_DEVICE_TRACE_MSG
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi_msg
+
+#define MAX_MSG_LEN	110
+
+DECLARE_EVENT_CLASS(iwlwifi_msg_event,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf),
+	TP_STRUCT__entry(
+		__dynamic_array(char, msg, MAX_MSG_LEN)
+	),
+	TP_fast_assign(
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       MAX_MSG_LEN, vaf->fmt,
+				       *vaf->va) >= MAX_MSG_LEN);
+	),
+	TP_printk("%s", __get_str(msg))
+);
+
+DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_err,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_warn,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_info,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_crit,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+TRACE_EVENT(iwlwifi_dbg,
+	TP_PROTO(u32 level, bool in_interrupt, const char *function,
+		 struct va_format *vaf),
+	TP_ARGS(level, in_interrupt, function, vaf),
+	TP_STRUCT__entry(
+		__field(u32, level)
+		__field(u8, in_interrupt)
+		__string(function, function)
+		__dynamic_array(char, msg, MAX_MSG_LEN)
+	),
+	TP_fast_assign(
+		__entry->level = level;
+		__entry->in_interrupt = in_interrupt;
+		__assign_str(function, function);
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       MAX_MSG_LEN, vaf->fmt,
+				       *vaf->va) >= MAX_MSG_LEN);
+	),
+	TP_printk("%s", __get_str(msg))
+);
+#endif /* __IWLWIFI_DEVICE_TRACE_MSG */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE iwl-devtrace-msg
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-ucode.h b/drivers/net/wireless/iwlwifi/iwl-devtrace-ucode.h
new file mode 100644
index 000000000000..10839fae9cd9
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace-ucode.h
@@ -0,0 +1,81 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#if !defined(__IWLWIFI_DEVICE_TRACE_UCODE) || defined(TRACE_HEADER_MULTI_READ)
+#define __IWLWIFI_DEVICE_TRACE_UCODE
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi_ucode
+
+TRACE_EVENT(iwlwifi_dev_ucode_cont_event,
+	TP_PROTO(const struct device *dev, u32 time, u32 data, u32 ev),
+	TP_ARGS(dev, time, data, ev),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+
+		__field(u32, time)
+		__field(u32, data)
+		__field(u32, ev)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->time = time;
+		__entry->data = data;
+		__entry->ev = ev;
+	),
+	TP_printk("[%s] EVT_LOGT:%010u:0x%08x:%04u",
+		  __get_str(dev), __entry->time, __entry->data, __entry->ev)
+);
+
+TRACE_EVENT(iwlwifi_dev_ucode_wrap_event,
+	TP_PROTO(const struct device *dev, u32 wraps, u32 n_entry, u32 p_entry),
+	TP_ARGS(dev, wraps, n_entry, p_entry),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+
+		__field(u32, wraps)
+		__field(u32, n_entry)
+		__field(u32, p_entry)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->wraps = wraps;
+		__entry->n_entry = n_entry;
+		__entry->p_entry = p_entry;
+	),
+	TP_printk("[%s] wraps=#%02d n=0x%X p=0x%X",
+		  __get_str(dev), __entry->wraps, __entry->n_entry,
+		  __entry->p_entry)
+);
+#endif /* __IWLWIFI_DEVICE_TRACE_UCODE */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE iwl-devtrace-ucode
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.h b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
index 78bd41bf34b0..b87acd6a229b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.h
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
@@ -24,7 +24,7 @@
  *
  *****************************************************************************/
 
-#if !defined(__IWLWIFI_DEVICE_TRACE) || defined(TRACE_HEADER_MULTI_READ)
+#ifndef __IWLWIFI_DEVICE_TRACE
 #include <linux/skbuff.h>
 #include <linux/ieee80211.h>
 #include <net/cfg80211.h>
@@ -80,436 +80,10 @@ static inline void trace_ ## name(proto) {}
 #define DEV_ENTRY	__string(dev, dev_name(dev))
 #define DEV_ASSIGN	__assign_str(dev, dev_name(dev))
 
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM iwlwifi_io
+#include "iwl-devtrace-io.h"
+#include "iwl-devtrace-ucode.h"
+#include "iwl-devtrace-msg.h"
+#include "iwl-devtrace-data.h"
+#include "iwl-devtrace-iwlwifi.h"
 
-TRACE_EVENT(iwlwifi_dev_ioread32,
-	TP_PROTO(const struct device *dev, u32 offs, u32 val),
-	TP_ARGS(dev, offs, val),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__field(u32, offs)
-		__field(u32, val)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->offs = offs;
-		__entry->val = val;
-	),
-	TP_printk("[%s] read io[%#x] = %#x",
-		  __get_str(dev), __entry->offs, __entry->val)
-);
-
-TRACE_EVENT(iwlwifi_dev_iowrite8,
-	TP_PROTO(const struct device *dev, u32 offs, u8 val),
-	TP_ARGS(dev, offs, val),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__field(u32, offs)
-		__field(u8, val)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->offs = offs;
-		__entry->val = val;
-	),
-	TP_printk("[%s] write io[%#x] = %#x)",
-		  __get_str(dev), __entry->offs, __entry->val)
-);
-
-TRACE_EVENT(iwlwifi_dev_iowrite32,
-	TP_PROTO(const struct device *dev, u32 offs, u32 val),
-	TP_ARGS(dev, offs, val),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__field(u32, offs)
-		__field(u32, val)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->offs = offs;
-		__entry->val = val;
-	),
-	TP_printk("[%s] write io[%#x] = %#x)",
-		  __get_str(dev), __entry->offs, __entry->val)
-);
-
-TRACE_EVENT(iwlwifi_dev_iowrite_prph32,
-	TP_PROTO(const struct device *dev, u32 offs, u32 val),
-	TP_ARGS(dev, offs, val),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__field(u32, offs)
-		__field(u32, val)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->offs = offs;
-		__entry->val = val;
-	),
-	TP_printk("[%s] write PRPH[%#x] = %#x)",
-		  __get_str(dev), __entry->offs, __entry->val)
-);
-
-TRACE_EVENT(iwlwifi_dev_ioread_prph32,
-	TP_PROTO(const struct device *dev, u32 offs, u32 val),
-	TP_ARGS(dev, offs, val),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__field(u32, offs)
-		__field(u32, val)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->offs = offs;
-		__entry->val = val;
-	),
-	TP_printk("[%s] read PRPH[%#x] = %#x",
-		  __get_str(dev), __entry->offs, __entry->val)
-);
-
-TRACE_EVENT(iwlwifi_dev_irq,
-	TP_PROTO(const struct device *dev),
-	TP_ARGS(dev),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-	),
-	/* TP_printk("") doesn't compile */
-	TP_printk("%d", 0)
-);
-
-TRACE_EVENT(iwlwifi_dev_ict_read,
-	TP_PROTO(const struct device *dev, u32 index, u32 value),
-	TP_ARGS(dev, index, value),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__field(u32, index)
-		__field(u32, value)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->index = index;
-		__entry->value = value;
-	),
-	TP_printk("[%s] read ict[%d] = %#.8x",
-		  __get_str(dev), __entry->index, __entry->value)
-);
-
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM iwlwifi_ucode
-
-TRACE_EVENT(iwlwifi_dev_ucode_cont_event,
-	TP_PROTO(const struct device *dev, u32 time, u32 data, u32 ev),
-	TP_ARGS(dev, time, data, ev),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-
-		__field(u32, time)
-		__field(u32, data)
-		__field(u32, ev)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->time = time;
-		__entry->data = data;
-		__entry->ev = ev;
-	),
-	TP_printk("[%s] EVT_LOGT:%010u:0x%08x:%04u",
-		  __get_str(dev), __entry->time, __entry->data, __entry->ev)
-);
-
-TRACE_EVENT(iwlwifi_dev_ucode_wrap_event,
-	TP_PROTO(const struct device *dev, u32 wraps, u32 n_entry, u32 p_entry),
-	TP_ARGS(dev, wraps, n_entry, p_entry),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-
-		__field(u32, wraps)
-		__field(u32, n_entry)
-		__field(u32, p_entry)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->wraps = wraps;
-		__entry->n_entry = n_entry;
-		__entry->p_entry = p_entry;
-	),
-	TP_printk("[%s] wraps=#%02d n=0x%X p=0x%X",
-		  __get_str(dev), __entry->wraps, __entry->n_entry,
-		  __entry->p_entry)
-);
-
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM iwlwifi_msg
-
-#define MAX_MSG_LEN	110
-
-DECLARE_EVENT_CLASS(iwlwifi_msg_event,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf),
-	TP_STRUCT__entry(
-		__dynamic_array(char, msg, MAX_MSG_LEN)
-	),
-	TP_fast_assign(
-		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
-				       MAX_MSG_LEN, vaf->fmt,
-				       *vaf->va) >= MAX_MSG_LEN);
-	),
-	TP_printk("%s", __get_str(msg))
-);
-
-DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_err,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_warn,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_info,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_crit,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf)
-);
-
-TRACE_EVENT(iwlwifi_dbg,
-	TP_PROTO(u32 level, bool in_interrupt, const char *function,
-		 struct va_format *vaf),
-	TP_ARGS(level, in_interrupt, function, vaf),
-	TP_STRUCT__entry(
-		__field(u32, level)
-		__field(u8, in_interrupt)
-		__string(function, function)
-		__dynamic_array(char, msg, MAX_MSG_LEN)
-	),
-	TP_fast_assign(
-		__entry->level = level;
-		__entry->in_interrupt = in_interrupt;
-		__assign_str(function, function);
-		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
-				       MAX_MSG_LEN, vaf->fmt,
-				       *vaf->va) >= MAX_MSG_LEN);
-	),
-	TP_printk("%s", __get_str(msg))
-);
-
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM iwlwifi_data
-
-TRACE_EVENT(iwlwifi_dev_tx_data,
-	TP_PROTO(const struct device *dev,
-		 struct sk_buff *skb,
-		 void *data, size_t data_len),
-	TP_ARGS(dev, skb, data, data_len),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-
-		__dynamic_array(u8, data, iwl_trace_data(skb) ? data_len : 0)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		if (iwl_trace_data(skb))
-			memcpy(__get_dynamic_array(data), data, data_len);
-	),
-	TP_printk("[%s] TX frame data", __get_str(dev))
-);
-
-TRACE_EVENT(iwlwifi_dev_rx_data,
-	TP_PROTO(const struct device *dev,
-		 const struct iwl_trans *trans,
-		 void *rxbuf, size_t len),
-	TP_ARGS(dev, trans, rxbuf, len),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-
-		__dynamic_array(u8, data,
-				len - iwl_rx_trace_len(trans, rxbuf, len))
-	),
-	TP_fast_assign(
-		size_t offs = iwl_rx_trace_len(trans, rxbuf, len);
-		DEV_ASSIGN;
-		if (offs < len)
-			memcpy(__get_dynamic_array(data),
-			       ((u8 *)rxbuf) + offs, len - offs);
-	),
-	TP_printk("[%s] RX frame data", __get_str(dev))
-);
-
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM iwlwifi
-
-TRACE_EVENT(iwlwifi_dev_hcmd,
-	TP_PROTO(const struct device *dev,
-		 struct iwl_host_cmd *cmd, u16 total_size,
-		 struct iwl_cmd_header *hdr),
-	TP_ARGS(dev, cmd, total_size, hdr),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__dynamic_array(u8, hcmd, total_size)
-		__field(u32, flags)
-	),
-	TP_fast_assign(
-		int i, offset = sizeof(*hdr);
-
-		DEV_ASSIGN;
-		__entry->flags = cmd->flags;
-		memcpy(__get_dynamic_array(hcmd), hdr, sizeof(*hdr));
-
-		for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
-			if (!cmd->len[i])
-				continue;
-			memcpy((u8 *)__get_dynamic_array(hcmd) + offset,
-			       cmd->data[i], cmd->len[i]);
-			offset += cmd->len[i];
-		}
-	),
-	TP_printk("[%s] hcmd %#.2x (%ssync)",
-		  __get_str(dev), ((u8 *)__get_dynamic_array(hcmd))[0],
-		  __entry->flags & CMD_ASYNC ? "a" : "")
-);
-
-TRACE_EVENT(iwlwifi_dev_rx,
-	TP_PROTO(const struct device *dev, const struct iwl_trans *trans,
-		 void *rxbuf, size_t len),
-	TP_ARGS(dev, trans, rxbuf, len),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__dynamic_array(u8, rxbuf, iwl_rx_trace_len(trans, rxbuf, len))
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		memcpy(__get_dynamic_array(rxbuf), rxbuf,
-		       iwl_rx_trace_len(trans, rxbuf, len));
-	),
-	TP_printk("[%s] RX cmd %#.2x",
-		  __get_str(dev), ((u8 *)__get_dynamic_array(rxbuf))[4])
-);
-
-TRACE_EVENT(iwlwifi_dev_tx,
-	TP_PROTO(const struct device *dev, struct sk_buff *skb,
-		 void *tfd, size_t tfdlen,
-		 void *buf0, size_t buf0_len,
-		 void *buf1, size_t buf1_len),
-	TP_ARGS(dev, skb, tfd, tfdlen, buf0, buf0_len, buf1, buf1_len),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-
-		__field(size_t, framelen)
-		__dynamic_array(u8, tfd, tfdlen)
-
-		/*
-		 * Do not insert between or below these items,
-		 * we want to keep the frame together (except
-		 * for the possible padding).
-		 */
-		__dynamic_array(u8, buf0, buf0_len)
-		__dynamic_array(u8, buf1, iwl_trace_data(skb) ? 0 : buf1_len)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->framelen = buf0_len + buf1_len;
-		memcpy(__get_dynamic_array(tfd), tfd, tfdlen);
-		memcpy(__get_dynamic_array(buf0), buf0, buf0_len);
-		if (!iwl_trace_data(skb))
-			memcpy(__get_dynamic_array(buf1), buf1, buf1_len);
-	),
-	TP_printk("[%s] TX %.2x (%zu bytes)",
-		  __get_str(dev), ((u8 *)__get_dynamic_array(buf0))[0],
-		  __entry->framelen)
-);
-
-TRACE_EVENT(iwlwifi_dev_ucode_error,
-	TP_PROTO(const struct device *dev, u32 desc, u32 tsf_low,
-		 u32 data1, u32 data2, u32 line, u32 blink1,
-		 u32 blink2, u32 ilink1, u32 ilink2, u32 bcon_time,
-		 u32 gp1, u32 gp2, u32 gp3, u32 ucode_ver, u32 hw_ver,
-		 u32 brd_ver),
-	TP_ARGS(dev, desc, tsf_low, data1, data2, line,
-		blink1, blink2, ilink1, ilink2, bcon_time, gp1, gp2,
-		gp3, ucode_ver, hw_ver, brd_ver),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-		__field(u32, desc)
-		__field(u32, tsf_low)
-		__field(u32, data1)
-		__field(u32, data2)
-		__field(u32, line)
-		__field(u32, blink1)
-		__field(u32, blink2)
-		__field(u32, ilink1)
-		__field(u32, ilink2)
-		__field(u32, bcon_time)
-		__field(u32, gp1)
-		__field(u32, gp2)
-		__field(u32, gp3)
-		__field(u32, ucode_ver)
-		__field(u32, hw_ver)
-		__field(u32, brd_ver)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->desc = desc;
-		__entry->tsf_low = tsf_low;
-		__entry->data1 = data1;
-		__entry->data2 = data2;
-		__entry->line = line;
-		__entry->blink1 = blink1;
-		__entry->blink2 = blink2;
-		__entry->ilink1 = ilink1;
-		__entry->ilink2 = ilink2;
-		__entry->bcon_time = bcon_time;
-		__entry->gp1 = gp1;
-		__entry->gp2 = gp2;
-		__entry->gp3 = gp3;
-		__entry->ucode_ver = ucode_ver;
-		__entry->hw_ver = hw_ver;
-		__entry->brd_ver = brd_ver;
-	),
-	TP_printk("[%s] #%02d %010u data 0x%08X 0x%08X line %u, "
-		  "blink 0x%05X 0x%05X ilink 0x%05X 0x%05X "
-		  "bcon_tm %010u gp 0x%08X 0x%08X 0x%08X uCode 0x%08X "
-		  "hw 0x%08X brd 0x%08X",
-		  __get_str(dev), __entry->desc, __entry->tsf_low,
-		  __entry->data1,
-		  __entry->data2, __entry->line, __entry->blink1,
-		  __entry->blink2, __entry->ilink1, __entry->ilink2,
-		  __entry->bcon_time, __entry->gp1, __entry->gp2,
-		  __entry->gp3, __entry->ucode_ver, __entry->hw_ver,
-		  __entry->brd_ver)
-);
-
-TRACE_EVENT(iwlwifi_dev_ucode_event,
-	TP_PROTO(const struct device *dev, u32 time, u32 data, u32 ev),
-	TP_ARGS(dev, time, data, ev),
-	TP_STRUCT__entry(
-		DEV_ENTRY
-
-		__field(u32, time)
-		__field(u32, data)
-		__field(u32, ev)
-	),
-	TP_fast_assign(
-		DEV_ASSIGN;
-		__entry->time = time;
-		__entry->data = data;
-		__entry->ev = ev;
-	),
-	TP_printk("[%s] EVT_LOGT:%010u:0x%08x:%04u",
-		  __get_str(dev), __entry->time, __entry->data, __entry->ev)
-);
 #endif /* __IWLWIFI_DEVICE_TRACE */
-
-#undef TRACE_INCLUDE_PATH
-#define TRACE_INCLUDE_PATH .
-#undef TRACE_INCLUDE_FILE
-#define TRACE_INCLUDE_FILE iwl-devtrace
-#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 996e7f16adf9..7267152e7dc7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -145,7 +145,7 @@ static struct iwlwifi_opmode_table {
 #define IWL_DEFAULT_SCAN_CHANNELS 40
 
 /*
- * struct fw_sec: Just for the image parsing proccess.
+ * struct fw_sec: Just for the image parsing process.
  * For the fw storage we are using struct fw_desc.
  */
 struct fw_sec {
@@ -175,6 +175,8 @@ static void iwl_dealloc_ucode(struct iwl_drv *drv)
 	kfree(drv->fw.dbg_dest_tlv);
 	for (i = 0; i < ARRAY_SIZE(drv->fw.dbg_conf_tlv); i++)
 		kfree(drv->fw.dbg_conf_tlv[i]);
+	for (i = 0; i < ARRAY_SIZE(drv->fw.dbg_trigger_tlv); i++)
+		kfree(drv->fw.dbg_trigger_tlv[i]);
 
 	for (i = 0; i < IWL_UCODE_TYPE_MAX; i++)
 		iwl_free_fw_img(drv, drv->fw.img + i);
@@ -239,16 +241,10 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 	 * previous name and uses the new format.
 	 */
 	if (drv->trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
-		char rev_step[2] = {
-			'A' + CSR_HW_REV_STEP(drv->trans->hw_rev), 0
-		};
-
-		/* A-step doesn't have an indication */
-		if (CSR_HW_REV_STEP(drv->trans->hw_rev) == SILICON_A_STEP)
-			rev_step[0] = 0;
+		char rev_step = 'A' + CSR_HW_REV_STEP(drv->trans->hw_rev);
 
 		snprintf(drv->firmware_name, sizeof(drv->firmware_name),
-			 "%s%s-%s.ucode", name_pre, rev_step, tag);
+			 "%s%c-%s.ucode", name_pre, rev_step, tag);
 	}
 
 	IWL_DEBUG_INFO(drv, "attempting to load firmware %s'%s'\n",
@@ -293,8 +289,10 @@ struct iwl_firmware_pieces {
 
 	/* FW debug data parsed for driver usage */
 	struct iwl_fw_dbg_dest_tlv *dbg_dest_tlv;
-	struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_MAX];
-	size_t dbg_conf_tlv_len[FW_DBG_MAX];
+	struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_CONF_MAX];
+	size_t dbg_conf_tlv_len[FW_DBG_CONF_MAX];
+	struct iwl_fw_dbg_trigger_tlv *dbg_trigger_tlv[FW_DBG_TRIGGER_MAX];
+	size_t dbg_trigger_tlv_len[FW_DBG_TRIGGER_MAX];
 };
 
 /*
@@ -842,6 +840,23 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 			capa->n_scan_channels =
 				le32_to_cpup((__le32 *)tlv_data);
 			break;
+		case IWL_UCODE_TLV_FW_VERSION: {
+			__le32 *ptr = (void *)tlv_data;
+			u32 major, minor;
+			u8 local_comp;
+
+			if (tlv_len != sizeof(u32) * 3)
+				goto invalid_tlv_len;
+
+			major = le32_to_cpup(ptr++);
+			minor = le32_to_cpup(ptr++);
+			local_comp = le32_to_cpup(ptr);
+
+			snprintf(drv->fw.fw_version,
+				 sizeof(drv->fw.fw_version), "%u.%u.%u",
+				 major, minor, local_comp);
+			break;
+			}
 		case IWL_UCODE_TLV_FW_DBG_DEST: {
 			struct iwl_fw_dbg_dest_tlv *dest = (void *)tlv_data;
 
@@ -897,6 +912,31 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 			pieces->dbg_conf_tlv_len[conf->id] = tlv_len;
 			break;
 			}
+		case IWL_UCODE_TLV_FW_DBG_TRIGGER: {
+			struct iwl_fw_dbg_trigger_tlv *trigger =
+				(void *)tlv_data;
+			u32 trigger_id = le32_to_cpu(trigger->id);
+
+			if (trigger_id >= ARRAY_SIZE(drv->fw.dbg_trigger_tlv)) {
+				IWL_ERR(drv,
+					"Skip unknown trigger: %u\n",
+					trigger->id);
+				break;
+			}
+
+			if (pieces->dbg_trigger_tlv[trigger_id]) {
+				IWL_ERR(drv,
+					"Ignore duplicate dbg trigger %u\n",
+					trigger->id);
+				break;
+			}
+
+			IWL_INFO(drv, "Found debug trigger: %u\n", trigger->id);
+
+			pieces->dbg_trigger_tlv[trigger_id] = trigger;
+			pieces->dbg_trigger_tlv_len[trigger_id] = tlv_len;
+			break;
+			}
 		case IWL_UCODE_TLV_SEC_RT_USNIFFER:
 			usniffer_images = true;
 			iwl_store_ucode_sec(pieces, tlv_data,
@@ -968,34 +1008,34 @@ static int validate_sec_sizes(struct iwl_drv *drv,
 
 	/* Verify that uCode images will fit in card's SRAM. */
 	if (get_sec_size(pieces, IWL_UCODE_REGULAR, IWL_UCODE_SECTION_INST) >
-							cfg->max_inst_size) {
+	    cfg->max_inst_size) {
 		IWL_ERR(drv, "uCode instr len %Zd too large to fit in\n",
 			get_sec_size(pieces, IWL_UCODE_REGULAR,
-						IWL_UCODE_SECTION_INST));
+				     IWL_UCODE_SECTION_INST));
 		return -1;
 	}
 
 	if (get_sec_size(pieces, IWL_UCODE_REGULAR, IWL_UCODE_SECTION_DATA) >
-							cfg->max_data_size) {
+	    cfg->max_data_size) {
 		IWL_ERR(drv, "uCode data len %Zd too large to fit in\n",
 			get_sec_size(pieces, IWL_UCODE_REGULAR,
-						IWL_UCODE_SECTION_DATA));
+				     IWL_UCODE_SECTION_DATA));
 		return -1;
 	}
 
-	 if (get_sec_size(pieces, IWL_UCODE_INIT, IWL_UCODE_SECTION_INST) >
-							cfg->max_inst_size) {
+	if (get_sec_size(pieces, IWL_UCODE_INIT, IWL_UCODE_SECTION_INST) >
+	     cfg->max_inst_size) {
 		IWL_ERR(drv, "uCode init instr len %Zd too large to fit in\n",
 			get_sec_size(pieces, IWL_UCODE_INIT,
-						IWL_UCODE_SECTION_INST));
+				     IWL_UCODE_SECTION_INST));
 		return -1;
 	}
 
 	if (get_sec_size(pieces, IWL_UCODE_INIT, IWL_UCODE_SECTION_DATA) >
-							cfg->max_data_size) {
+	    cfg->max_data_size) {
 		IWL_ERR(drv, "uCode init data len %Zd too large to fit in\n",
 			get_sec_size(pieces, IWL_UCODE_REGULAR,
-						IWL_UCODE_SECTION_DATA));
+				     IWL_UCODE_SECTION_DATA));
 		return -1;
 	}
 	return 0;
@@ -1062,6 +1102,7 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 	const unsigned int api_max = drv->cfg->ucode_api_max;
 	unsigned int api_ok = drv->cfg->ucode_api_ok;
 	const unsigned int api_min = drv->cfg->ucode_api_min;
+	size_t trigger_tlv_sz[FW_DBG_TRIGGER_MAX];
 	u32 api_ver;
 	int i;
 	bool load_module = false;
@@ -1107,7 +1148,10 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 	if (err)
 		goto try_again;
 
-	api_ver = IWL_UCODE_API(drv->fw.ucode_ver);
+	if (drv->fw.ucode_capa.api[0] & IWL_UCODE_TLV_API_NEW_VERSION)
+		api_ver = drv->fw.ucode_ver;
+	else
+		api_ver = IWL_UCODE_API(drv->fw.ucode_ver);
 
 	/*
 	 * api_ver should match the api version forming part of the
@@ -1178,6 +1222,48 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 		}
 	}
 
+	memset(&trigger_tlv_sz, 0xff, sizeof(trigger_tlv_sz));
+
+	trigger_tlv_sz[FW_DBG_TRIGGER_MISSED_BEACONS] =
+		sizeof(struct iwl_fw_dbg_trigger_missed_bcon);
+	trigger_tlv_sz[FW_DBG_TRIGGER_CHANNEL_SWITCH] = 0;
+	trigger_tlv_sz[FW_DBG_TRIGGER_FW_NOTIF] =
+		sizeof(struct iwl_fw_dbg_trigger_cmd);
+	trigger_tlv_sz[FW_DBG_TRIGGER_MLME] =
+		sizeof(struct iwl_fw_dbg_trigger_mlme);
+	trigger_tlv_sz[FW_DBG_TRIGGER_STATS] =
+		sizeof(struct iwl_fw_dbg_trigger_stats);
+	trigger_tlv_sz[FW_DBG_TRIGGER_RSSI] =
+		sizeof(struct iwl_fw_dbg_trigger_low_rssi);
+	trigger_tlv_sz[FW_DBG_TRIGGER_TXQ_TIMERS] =
+		sizeof(struct iwl_fw_dbg_trigger_txq_timer);
+	trigger_tlv_sz[FW_DBG_TRIGGER_TIME_EVENT] =
+		sizeof(struct iwl_fw_dbg_trigger_time_event);
+
+	for (i = 0; i < ARRAY_SIZE(drv->fw.dbg_trigger_tlv); i++) {
+		if (pieces->dbg_trigger_tlv[i]) {
+			/*
+			 * If the trigger isn't long enough, WARN and exit.
+			 * Someone is trying to debug something and he won't
+			 * be able to catch the bug he is trying to chase.
+			 * We'd better be noisy to be sure he knows what's
+			 * going on.
+			 */
+			if (WARN_ON(pieces->dbg_trigger_tlv_len[i] <
+				    (trigger_tlv_sz[i] +
+				     sizeof(struct iwl_fw_dbg_trigger_tlv))))
+				goto out_free_fw;
+			drv->fw.dbg_trigger_tlv_len[i] =
+				pieces->dbg_trigger_tlv_len[i];
+			drv->fw.dbg_trigger_tlv[i] =
+				kmemdup(pieces->dbg_trigger_tlv[i],
+					drv->fw.dbg_trigger_tlv_len[i],
+					GFP_KERNEL);
+			if (!drv->fw.dbg_trigger_tlv[i])
+				goto out_free_fw;
+		}
+	}
+
 	/* Now that we can no longer fail, copy information */
 
 	/*
@@ -1257,6 +1343,7 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 				op->name, err);
 #endif
 	}
+	kfree(pieces);
 	return;
 
  try_again:
@@ -1484,6 +1571,10 @@ module_param_named(d0i3_disable, iwlwifi_mod_params.d0i3_disable,
 		   bool, S_IRUGO);
 MODULE_PARM_DESC(d0i3_disable, "disable d0i3 functionality (default: Y)");
 
+module_param_named(lar_disable, iwlwifi_mod_params.lar_disable,
+		   bool, S_IRUGO);
+MODULE_PARM_DESC(lar_disable, "disable LAR functionality (default: N)");
+
 module_param_named(uapsd_disable, iwlwifi_mod_params.uapsd_disable,
 		   bool, S_IRUGO | S_IWUSR);
 #ifdef CONFIG_IWLWIFI_UAPSD
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.h b/drivers/net/wireless/iwlwifi/iwl-drv.h
index adf522c756e6..cda746b33db1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.h
@@ -68,7 +68,7 @@
 
 /* for all modules */
 #define DRV_NAME        "iwlwifi"
-#define DRV_COPYRIGHT	"Copyright(c) 2003- 2014 Intel Corporation"
+#define DRV_COPYRIGHT	"Copyright(c) 2003- 2015 Intel Corporation"
 #define DRV_AUTHOR     "<ilw@linux.intel.com>"
 
 /* radio config bits (actual values from NVM definition) */
@@ -123,7 +123,7 @@ struct iwl_cfg;
  * starts the driver: fetches the firmware. This should be called by bus
  * specific system flows implementations. For example, the bus specific probe
  * function should do bus related operations only, and then call to this
- * function. It returns the driver object or %NULL if an error occured.
+ * function. It returns the driver object or %NULL if an error occurred.
  */
 struct iwl_drv *iwl_drv_start(struct iwl_trans *trans,
 			      const struct iwl_cfg *cfg);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
index f0548b8a64b0..5234a0bf11e4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
@@ -94,6 +94,7 @@ struct iwl_nvm_data {
 	u32 nvm_version;
 	s8 max_tx_pwr_half_dbm;
 
+	bool lar_enabled;
 	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
 	struct ieee80211_channel channels[];
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
index 25d0105741db..219ca8acca62 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
@@ -248,7 +248,7 @@ static int iwl_read_otp_word(struct iwl_trans *trans, u16 addr,
 	otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
 	if (otpgp & CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK) {
 		/* stop in this case */
-		/* set the uncorrectable OTP ECC bit for acknowledgement */
+		/* set the uncorrectable OTP ECC bit for acknowledgment */
 		iwl_set_bit(trans, CSR_OTP_GP_REG,
 			    CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
 		IWL_ERR(trans, "Uncorrectable OTP ECC error, abort OTP read\n");
@@ -256,7 +256,7 @@ static int iwl_read_otp_word(struct iwl_trans *trans, u16 addr,
 	}
 	if (otpgp & CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK) {
 		/* continue in this case */
-		/* set the correctable OTP ECC bit for acknowledgement */
+		/* set the correctable OTP ECC bit for acknowledgment */
 		iwl_set_bit(trans, CSR_OTP_GP_REG,
 			    CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK);
 		IWL_ERR(trans, "Correctable OTP ECC error, continue read\n");
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
index 1f7f15eb86da..d45dc021cda2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fh.h
@@ -445,7 +445,7 @@ static inline unsigned int FH_MEM_CBBC_QUEUE(unsigned int chnl)
 #define RX_LOW_WATERMARK 8
 
 /**
- * struct iwl_rb_status - reseve buffer status
+ * struct iwl_rb_status - reserve buffer status
  * 	host memory mapped FH registers
  * @closed_rb_num [0:11] - Indicates the index of the RB which was closed
  * @closed_fr_num [0:11] - Indicates the index of the RX Frame which was closed
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
index 919a2548a92c..251bf8dc4a12 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
@@ -82,6 +82,8 @@
  *	sections like this in a single file.
  * @IWL_FW_ERROR_DUMP_FH_REGS: range of FH registers
  * @IWL_FW_ERROR_DUMP_MEM: chunk of memory
+ * @IWL_FW_ERROR_DUMP_ERROR_INFO: description of what triggered this dump.
+ *	Structured as &struct iwl_fw_error_dump_trigger_desc.
  */
 enum iwl_fw_error_dump_type {
 	/* 0 is deprecated */
@@ -94,6 +96,7 @@ enum iwl_fw_error_dump_type {
 	IWL_FW_ERROR_DUMP_TXF = 7,
 	IWL_FW_ERROR_DUMP_FH_REGS = 8,
 	IWL_FW_ERROR_DUMP_MEM = 9,
+	IWL_FW_ERROR_DUMP_ERROR_INFO = 10,
 
 	IWL_FW_ERROR_DUMP_MAX,
 };
@@ -180,7 +183,7 @@ struct iwl_fw_error_dump_info {
  * struct iwl_fw_error_dump_fw_mon - FW monitor data
  * @fw_mon_wr_ptr: the position of the write pointer in the cyclic buffer
  * @fw_mon_base_ptr: base pointer of the data
- * @fw_mon_cycle_cnt: number of wrap arounds
+ * @fw_mon_cycle_cnt: number of wraparounds
  * @reserved: for future use
  * @data: captured data
  */
@@ -230,4 +233,53 @@ iwl_fw_error_next_data(struct iwl_fw_error_dump_data *data)
 	return (void *)(data->data + le32_to_cpu(data->len));
 }
 
+/**
+ * enum iwl_fw_dbg_trigger - triggers available
+ *
+ * @FW_DBG_TRIGGER_USER: trigger log collection by user
+ *	This should not be defined as a trigger to the driver, but a value the
+ *	driver should set to indicate that the trigger was initiated by the
+ *	user.
+ * @FW_DBG_TRIGGER_FW_ASSERT: trigger log collection when the firmware asserts
+ * @FW_DBG_TRIGGER_MISSED_BEACONS: trigger log collection when beacons are
+ *	missed.
+ * @FW_DBG_TRIGGER_CHANNEL_SWITCH: trigger log collection upon channel switch.
+ * @FW_DBG_TRIGGER_FW_NOTIF: trigger log collection when the firmware sends a
+ *	command response or a notification.
+ * @FW_DBG_TRIGGER_MLME: trigger log collection upon MLME event.
+ * @FW_DBG_TRIGGER_STATS: trigger log collection upon statistics threshold.
+ * @FW_DBG_TRIGGER_RSSI: trigger log collection when the rssi of the beacon
+ *	goes below a threshold.
+ * @FW_DBG_TRIGGER_TXQ_TIMERS: configures the timers for the Tx queue hang
+ *	detection.
+ * @FW_DBG_TRIGGER_TIME_EVENT: trigger log collection upon time events related
+ *	events.
+ */
+enum iwl_fw_dbg_trigger {
+	FW_DBG_TRIGGER_INVALID = 0,
+	FW_DBG_TRIGGER_USER,
+	FW_DBG_TRIGGER_FW_ASSERT,
+	FW_DBG_TRIGGER_MISSED_BEACONS,
+	FW_DBG_TRIGGER_CHANNEL_SWITCH,
+	FW_DBG_TRIGGER_FW_NOTIF,
+	FW_DBG_TRIGGER_MLME,
+	FW_DBG_TRIGGER_STATS,
+	FW_DBG_TRIGGER_RSSI,
+	FW_DBG_TRIGGER_TXQ_TIMERS,
+	FW_DBG_TRIGGER_TIME_EVENT,
+
+	/* must be last */
+	FW_DBG_TRIGGER_MAX,
+};
+
+/**
+ * struct iwl_fw_error_dump_trigger_desc - describes the trigger condition
+ * @type: %enum iwl_fw_dbg_trigger
+ * @data: raw data about what happened
+ */
+struct iwl_fw_error_dump_trigger_desc {
+	__le32 type;
+	u8 data[];
+};
+
 #endif /* __fw_error_dump_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index 016d91384681..bfdf3faa6c47 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -66,6 +66,7 @@
 #define __iwl_fw_file_h__
 
 #include <linux/netdevice.h>
+#include <linux/nl80211.h>
 
 /* v1/v2 uCode file layout */
 struct iwl_ucode_header {
@@ -133,8 +134,10 @@ enum iwl_ucode_tlv_type {
 	IWL_UCODE_TLV_N_SCAN_CHANNELS		= 31,
 	IWL_UCODE_TLV_SEC_RT_USNIFFER	= 34,
 	IWL_UCODE_TLV_SDIO_ADMA_ADDR	= 35,
+	IWL_UCODE_TLV_FW_VERSION	= 36,
 	IWL_UCODE_TLV_FW_DBG_DEST	= 38,
 	IWL_UCODE_TLV_FW_DBG_CONF	= 39,
+	IWL_UCODE_TLV_FW_DBG_TRIGGER	= 40,
 };
 
 struct iwl_ucode_tlv {
@@ -156,7 +159,8 @@ struct iwl_tlv_ucode_header {
 	__le32 zero;
 	__le32 magic;
 	u8 human_readable[FW_VER_HUMAN_READABLE_SZ];
-	__le32 ver;		/* major/minor/API/serial */
+	/* major/minor/API/serial or major in new format */
+	__le32 ver;
 	__le32 build;
 	__le64 ignore;
 	/*
@@ -187,7 +191,7 @@ struct iwl_ucode_capa {
  * enum iwl_ucode_tlv_flag - ucode API flags
  * @IWL_UCODE_TLV_FLAGS_PAN: This is PAN capable microcode; this previously
  *	was a separate TLV but moved here to save space.
- * @IWL_UCODE_TLV_FLAGS_NEWSCAN: new uCode scan behaviour on hidden SSID,
+ * @IWL_UCODE_TLV_FLAGS_NEWSCAN: new uCode scan behavior on hidden SSID,
  *	treats good CRC threshold as a boolean
  * @IWL_UCODE_TLV_FLAGS_MFP: This uCode image supports MFP (802.11w).
  * @IWL_UCODE_TLV_FLAGS_P2P: This uCode image supports P2P.
@@ -236,11 +240,9 @@ enum iwl_ucode_tlv_flag {
 /**
  * enum iwl_ucode_tlv_api - ucode api
  * @IWL_UCODE_TLV_API_BT_COEX_SPLIT: new API for BT Coex
- * @IWL_UCODE_TLV_API_DISABLE_STA_TX: ucode supports tx_disable bit.
- * @IWL_UCODE_TLV_API_LMAC_SCAN: This ucode uses LMAC unified scan API.
- * @IWL_UCODE_TLV_API_SF_NO_DUMMY_NOTIF: ucode supports disabling dummy notif.
  * @IWL_UCODE_TLV_API_FRAGMENTED_SCAN: This ucode supports active dwell time
  *	longer than the passive one, which is essential for fragmented scan.
+ * @IWL_UCODE_TLV_API_WIFI_MCC_UPDATE: ucode supports MCC updates with source.
  * IWL_UCODE_TLV_API_HDC_PHASE_0: ucode supports finer configuration of LTR
  * @IWL_UCODE_TLV_API_BASIC_DWELL: use only basic dwell time in scan command,
  *	regardless of the band or the number of the probes. FW will calculate
@@ -250,19 +252,21 @@ enum iwl_ucode_tlv_flag {
  * @IWL_UCODE_TLV_API_SINGLE_SCAN_EBS: EBS is supported for single scans too.
  * @IWL_UCODE_TLV_API_ASYNC_DTM: Async temperature notifications are supported.
  * @IWL_UCODE_TLV_API_LQ_SS_PARAMS: Configure STBC/BFER via LQ CMD ss_params
+ * @IWL_UCODE_TLV_API_STATS_V10: uCode supports/uses statistics API version 10
+ * @IWL_UCODE_TLV_API_NEW_VERSION: new versioning format
  */
 enum iwl_ucode_tlv_api {
 	IWL_UCODE_TLV_API_BT_COEX_SPLIT         = BIT(3),
-	IWL_UCODE_TLV_API_DISABLE_STA_TX	= BIT(5),
-	IWL_UCODE_TLV_API_LMAC_SCAN		= BIT(6),
-	IWL_UCODE_TLV_API_SF_NO_DUMMY_NOTIF	= BIT(7),
 	IWL_UCODE_TLV_API_FRAGMENTED_SCAN	= BIT(8),
+	IWL_UCODE_TLV_API_WIFI_MCC_UPDATE	= BIT(9),
 	IWL_UCODE_TLV_API_HDC_PHASE_0		= BIT(10),
 	IWL_UCODE_TLV_API_BASIC_DWELL		= BIT(13),
 	IWL_UCODE_TLV_API_SCD_CFG		= BIT(15),
 	IWL_UCODE_TLV_API_SINGLE_SCAN_EBS	= BIT(16),
 	IWL_UCODE_TLV_API_ASYNC_DTM		= BIT(17),
 	IWL_UCODE_TLV_API_LQ_SS_PARAMS		= BIT(18),
+	IWL_UCODE_TLV_API_STATS_V10		= BIT(19),
+	IWL_UCODE_TLV_API_NEW_VERSION		= BIT(20),
 };
 
 /**
@@ -284,6 +288,13 @@ enum iwl_ucode_tlv_api {
  *	which also implies support for the scheduler configuration command
  * @IWL_UCODE_TLV_CAPA_TDLS_CHANNEL_SWITCH: supports TDLS channel switching
  * @IWL_UCODE_TLV_CAPA_HOTSPOT_SUPPORT: supports Hot Spot Command
+ * @IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS: support radio and beacon statistics
+ * @IWL_UCODE_TLV_CAPA_BT_COEX_PLCR: enabled BT Coex packet level co-running
+ * @IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC: ucode supports LAR updates with different
+ *	sources for the MCC. This TLV bit is a future replacement to
+ *	IWL_UCODE_TLV_API_WIFI_MCC_UPDATE. When either is set, multi-source LAR
+ *	is supported.
+ * @IWL_UCODE_TLV_CAPA_BT_COEX_RRC: supports BT Coex RRC
  */
 enum iwl_ucode_tlv_capa {
 	IWL_UCODE_TLV_CAPA_D0I3_SUPPORT			= BIT(0),
@@ -298,6 +309,10 @@ enum iwl_ucode_tlv_capa {
 	IWL_UCODE_TLV_CAPA_DQA_SUPPORT			= BIT(12),
 	IWL_UCODE_TLV_CAPA_TDLS_CHANNEL_SWITCH		= BIT(13),
 	IWL_UCODE_TLV_CAPA_HOTSPOT_SUPPORT		= BIT(18),
+	IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS		= BIT(22),
+	IWL_UCODE_TLV_CAPA_BT_COEX_PLCR			= BIT(28),
+	IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC		= BIT(29),
+	IWL_UCODE_TLV_CAPA_BT_COEX_RRC			= BIT(30),
 };
 
 /* The default calibrate table size if not specified by firmware file */
@@ -450,44 +465,207 @@ struct iwl_fw_dbg_conf_hcmd {
 } __packed;
 
 /**
- * struct iwl_fw_dbg_trigger - a TLV that describes a debug configuration
+ * enum iwl_fw_dbg_trigger_mode - triggers functionalities
  *
- * @enabled: is this trigger enabled
- * @reserved:
- * @len: length, in bytes, of the %trigger field
- * @trigger: pointer to a trigger struct
+ * @IWL_FW_DBG_TRIGGER_START: when trigger occurs re-conf the dbg mechanism
+ * @IWL_FW_DBG_TRIGGER_STOP: when trigger occurs pull the dbg data
  */
-struct iwl_fw_dbg_trigger {
-	u8 enabled;
-	u8 reserved;
-	u8 len;
-	u8 trigger[0];
+enum iwl_fw_dbg_trigger_mode {
+	IWL_FW_DBG_TRIGGER_START = BIT(0),
+	IWL_FW_DBG_TRIGGER_STOP = BIT(1),
+};
+
+/**
+ * enum iwl_fw_dbg_trigger_vif_type - define the VIF type for a trigger
+ * @IWL_FW_DBG_CONF_VIF_ANY: any vif type
+ * @IWL_FW_DBG_CONF_VIF_IBSS: IBSS mode
+ * @IWL_FW_DBG_CONF_VIF_STATION: BSS mode
+ * @IWL_FW_DBG_CONF_VIF_AP: AP mode
+ * @IWL_FW_DBG_CONF_VIF_P2P_CLIENT: P2P Client mode
+ * @IWL_FW_DBG_CONF_VIF_P2P_GO: P2P GO mode
+ * @IWL_FW_DBG_CONF_VIF_P2P_DEVICE: P2P device
+ */
+enum iwl_fw_dbg_trigger_vif_type {
+	IWL_FW_DBG_CONF_VIF_ANY = NL80211_IFTYPE_UNSPECIFIED,
+	IWL_FW_DBG_CONF_VIF_IBSS = NL80211_IFTYPE_ADHOC,
+	IWL_FW_DBG_CONF_VIF_STATION = NL80211_IFTYPE_STATION,
+	IWL_FW_DBG_CONF_VIF_AP = NL80211_IFTYPE_AP,
+	IWL_FW_DBG_CONF_VIF_P2P_CLIENT = NL80211_IFTYPE_P2P_CLIENT,
+	IWL_FW_DBG_CONF_VIF_P2P_GO = NL80211_IFTYPE_P2P_GO,
+	IWL_FW_DBG_CONF_VIF_P2P_DEVICE = NL80211_IFTYPE_P2P_DEVICE,
+};
+
+/**
+ * struct iwl_fw_dbg_trigger_tlv - a TLV that describes the trigger
+ * @id: %enum iwl_fw_dbg_trigger
+ * @vif_type: %enum iwl_fw_dbg_trigger_vif_type
+ * @stop_conf_ids: bitmap of configurations this trigger relates to.
+ *	if the mode is %IWL_FW_DBG_TRIGGER_STOP, then if the bit corresponding
+ *	to the currently running configuration is set, the data should be
+ *	collected.
+ * @stop_delay: how many milliseconds to wait before collecting the data
+ *	after the STOP trigger fires.
+ * @mode: %enum iwl_fw_dbg_trigger_mode - can be stop / start of both
+ * @start_conf_id: if mode is %IWL_FW_DBG_TRIGGER_START, this defines what
+ *	configuration should be applied when the triggers kicks in.
+ * @occurrences: number of occurrences. 0 means the trigger will never fire.
+ */
+struct iwl_fw_dbg_trigger_tlv {
+	__le32 id;
+	__le32 vif_type;
+	__le32 stop_conf_ids;
+	__le32 stop_delay;
+	u8 mode;
+	u8 start_conf_id;
+	__le16 occurrences;
+	__le32 reserved[2];
+
+	u8 data[0];
 } __packed;
 
+#define FW_DBG_START_FROM_ALIVE	0
+#define FW_DBG_CONF_MAX		32
+#define FW_DBG_INVALID		0xff
+
 /**
- * enum iwl_fw_dbg_conf - configurations available
- *
- * @FW_DBG_CUSTOM: take this configuration from alive
- *	Note that the trigger is NO-OP for this configuration
+ * struct iwl_fw_dbg_trigger_missed_bcon - configures trigger for missed beacons
+ * @stop_consec_missed_bcon: stop recording if threshold is crossed.
+ * @stop_consec_missed_bcon_since_rx: stop recording if threshold is crossed.
+ * @start_consec_missed_bcon: start recording if threshold is crossed.
+ * @start_consec_missed_bcon_since_rx: start recording if threshold is crossed.
+ * @reserved1: reserved
+ * @reserved2: reserved
  */
-enum iwl_fw_dbg_conf {
-	FW_DBG_CUSTOM = 0,
+struct iwl_fw_dbg_trigger_missed_bcon {
+	__le32 stop_consec_missed_bcon;
+	__le32 stop_consec_missed_bcon_since_rx;
+	__le32 reserved2[2];
+	__le32 start_consec_missed_bcon;
+	__le32 start_consec_missed_bcon_since_rx;
+	__le32 reserved1[2];
+} __packed;
 
-	/* must be last */
-	FW_DBG_MAX,
-	FW_DBG_INVALID = 0xff,
-};
+/**
+ * struct iwl_fw_dbg_trigger_cmd - configures trigger for messages from FW.
+ * cmds: the list of commands to trigger the collection on
+ */
+struct iwl_fw_dbg_trigger_cmd {
+	struct cmd {
+		u8 cmd_id;
+		u8 group_id;
+	} __packed cmds[16];
+} __packed;
+
+/**
+ * iwl_fw_dbg_trigger_stats - configures trigger for statistics
+ * @stop_offset: the offset of the value to be monitored
+ * @stop_threshold: the threshold above which to collect
+ * @start_offset: the offset of the value to be monitored
+ * @start_threshold: the threshold above which to start recording
+ */
+struct iwl_fw_dbg_trigger_stats {
+	__le32 stop_offset;
+	__le32 stop_threshold;
+	__le32 start_offset;
+	__le32 start_threshold;
+} __packed;
+
+/**
+ * struct iwl_fw_dbg_trigger_low_rssi - trigger for low beacon RSSI
+ * @rssi: RSSI value to trigger at
+ */
+struct iwl_fw_dbg_trigger_low_rssi {
+	__le32 rssi;
+} __packed;
 
 /**
- * struct iwl_fw_dbg_conf_tlv - a TLV that describes a debug configuration
+ * struct iwl_fw_dbg_trigger_mlme - configures trigger for mlme events
+ * @stop_auth_denied: number of denied authentication to collect
+ * @stop_auth_timeout: number of authentication timeout to collect
+ * @stop_rx_deauth: number of Rx deauth before to collect
+ * @stop_tx_deauth: number of Tx deauth before to collect
+ * @stop_assoc_denied: number of denied association to collect
+ * @stop_assoc_timeout: number of association timeout to collect
+ * @stop_connection_loss: number of connection loss to collect
+ * @start_auth_denied: number of denied authentication to start recording
+ * @start_auth_timeout: number of authentication timeout to start recording
+ * @start_rx_deauth: number of Rx deauth to start recording
+ * @start_tx_deauth: number of Tx deauth to start recording
+ * @start_assoc_denied: number of denied association to start recording
+ * @start_assoc_timeout: number of association timeout to start recording
+ * @start_connection_loss: number of connection loss to start recording
+ */
+struct iwl_fw_dbg_trigger_mlme {
+	u8 stop_auth_denied;
+	u8 stop_auth_timeout;
+	u8 stop_rx_deauth;
+	u8 stop_tx_deauth;
+
+	u8 stop_assoc_denied;
+	u8 stop_assoc_timeout;
+	u8 stop_connection_loss;
+	u8 reserved;
+
+	u8 start_auth_denied;
+	u8 start_auth_timeout;
+	u8 start_rx_deauth;
+	u8 start_tx_deauth;
+
+	u8 start_assoc_denied;
+	u8 start_assoc_timeout;
+	u8 start_connection_loss;
+	u8 reserved2;
+} __packed;
+
+/**
+ * struct iwl_fw_dbg_trigger_txq_timer - configures the Tx queue's timer
+ * @command_queue: timeout for the command queue in ms
+ * @bss: timeout for the queues of a BSS (except for TDLS queues) in ms
+ * @softap: timeout for the queues of a softAP in ms
+ * @p2p_go: timeout for the queues of a P2P GO in ms
+ * @p2p_client: timeout for the queues of a P2P client in ms
+ * @p2p_device: timeout for the queues of a P2P device in ms
+ * @ibss: timeout for the queues of an IBSS in ms
+ * @tdls: timeout for the queues of a TDLS station in ms
+ */
+struct iwl_fw_dbg_trigger_txq_timer {
+	__le32 command_queue;
+	__le32 bss;
+	__le32 softap;
+	__le32 p2p_go;
+	__le32 p2p_client;
+	__le32 p2p_device;
+	__le32 ibss;
+	__le32 tdls;
+	__le32 reserved[4];
+} __packed;
+
+/**
+ * struct iwl_fw_dbg_trigger_time_event - configures a time event trigger
+ * time_Events: a list of tuples <id, action_bitmap>. The driver will issue a
+ *	trigger each time a time event notification that relates to time event
+ *	id with one of the actions in the bitmap is received and
+ *	BIT(notif->status) is set in status_bitmap.
  *
- * @id: %enum iwl_fw_dbg_conf
+ */
+struct iwl_fw_dbg_trigger_time_event {
+	struct {
+		__le32 id;
+		__le32 action_bitmap;
+		__le32 status_bitmap;
+	} __packed time_events[16];
+} __packed;
+
+/**
+ * struct iwl_fw_dbg_conf_tlv - a TLV that describes a debug configuration.
+ * @id: conf id
  * @usniffer: should the uSniffer image be used
  * @num_of_hcmds: how many HCMDs to send are present here
  * @hcmd: a variable length host command to be sent to apply the configuration.
  *	If there is more than one HCMD to send, they will appear one after the
  *	other and be sent in the order that they appear in.
- * This parses IWL_UCODE_TLV_FW_DBG_CONF
+ * This parses IWL_UCODE_TLV_FW_DBG_CONF. The user can add up-to
+ * %FW_DBG_CONF_MAX configuration per run.
  */
 struct iwl_fw_dbg_conf_tlv {
 	u8 id;
@@ -495,8 +673,6 @@ struct iwl_fw_dbg_conf_tlv {
 	u8 reserved;
 	u8 num_of_hcmds;
 	struct iwl_fw_dbg_conf_hcmd hcmd;
-
-	/* struct iwl_fw_dbg_trigger sits after all variable length hcmds */
 } __packed;
 
 #endif  /* __iwl_fw_file_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index ffd785cc67d6..cf75bafae51d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -68,6 +68,7 @@
 #include <net/mac80211.h>
 
 #include "iwl-fw-file.h"
+#include "iwl-fw-error-dump.h"
 
 /**
  * enum iwl_ucode_type
@@ -157,6 +158,8 @@ struct iwl_fw_cscheme_list {
  * @dbg_dest_tlv: points to the destination TLV for debug
  * @dbg_conf_tlv: array of pointers to configuration TLVs for debug
  * @dbg_conf_tlv_len: lengths of the @dbg_conf_tlv entries
+ * @dbg_trigger_tlv: array of pointers to triggers TLVs
+ * @dbg_trigger_tlv_len: lengths of the @dbg_trigger_tlv entries
  * @dbg_dest_reg_num: num of reg_ops in %dbg_dest_tlv
  */
 struct iwl_fw {
@@ -186,9 +189,10 @@ struct iwl_fw {
 	u32 sdio_adma_addr;
 
 	struct iwl_fw_dbg_dest_tlv *dbg_dest_tlv;
-	struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_MAX];
-	size_t dbg_conf_tlv_len[FW_DBG_MAX];
-
+	struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_CONF_MAX];
+	size_t dbg_conf_tlv_len[FW_DBG_CONF_MAX];
+	struct iwl_fw_dbg_trigger_tlv *dbg_trigger_tlv[FW_DBG_TRIGGER_MAX];
+	size_t dbg_trigger_tlv_len[FW_DBG_TRIGGER_MAX];
 	u8 dbg_dest_reg_num;
 };
 
@@ -206,46 +210,29 @@ static inline const char *get_fw_dbg_mode_string(int mode)
 	}
 }
 
-static inline const struct iwl_fw_dbg_trigger *
-iwl_fw_dbg_conf_get_trigger(const struct iwl_fw *fw, u8 id)
+static inline bool
+iwl_fw_dbg_conf_usniffer(const struct iwl_fw *fw, u8 id)
 {
 	const struct iwl_fw_dbg_conf_tlv *conf_tlv = fw->dbg_conf_tlv[id];
-	u8 *ptr;
-	int i;
 
 	if (!conf_tlv)
-		return NULL;
-
-	ptr = (void *)&conf_tlv->hcmd;
-	for (i = 0; i < conf_tlv->num_of_hcmds; i++) {
-		ptr += sizeof(conf_tlv->hcmd);
-		ptr += le16_to_cpu(conf_tlv->hcmd.len);
-	}
-
-	return (const struct iwl_fw_dbg_trigger *)ptr;
-}
-
-static inline bool
-iwl_fw_dbg_conf_enabled(const struct iwl_fw *fw, u8 id)
-{
-	const struct iwl_fw_dbg_trigger *trigger =
-		iwl_fw_dbg_conf_get_trigger(fw, id);
-
-	if (!trigger)
 		return false;
 
-	return trigger->enabled;
+	return conf_tlv->usniffer;
 }
 
-static inline bool
-iwl_fw_dbg_conf_usniffer(const struct iwl_fw *fw, u8 id)
-{
-	const struct iwl_fw_dbg_conf_tlv *conf_tlv = fw->dbg_conf_tlv[id];
+#define iwl_fw_dbg_trigger_enabled(fw, id) ({			\
+	void *__dbg_trigger = (fw)->dbg_trigger_tlv[(id)];	\
+	unlikely(__dbg_trigger);				\
+})
 
-	if (!conf_tlv)
-		return false;
+static inline struct iwl_fw_dbg_trigger_tlv*
+iwl_fw_dbg_get_trigger(const struct iwl_fw *fw, u8 id)
+{
+	if (WARN_ON(id >= ARRAY_SIZE(fw->dbg_trigger_tlv)))
+		return NULL;
 
-	return conf_tlv->usniffer;
+	return fw->dbg_trigger_tlv[id];
 }
 
 #endif  /* __iwl_fw_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/iwlwifi/iwl-io.c
index 03250a45272e..27c66e477833 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -186,21 +186,16 @@ IWL_EXPORT_SYMBOL(iwl_clear_bits_prph);
 
 void iwl_force_nmi(struct iwl_trans *trans)
 {
-	/*
-	 * In HW previous to the 8000 HW family, and in the 8000 HW family
-	 * itself when the revision step==0, the DEVICE_SET_NMI_REG is used
-	 * to force an NMI. Otherwise, a different register -
-	 * DEVICE_SET_NMI_8000B_REG - is used.
-	 */
-	if ((trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) ||
-	    (CSR_HW_REV_STEP(trans->hw_rev) == SILICON_A_STEP)) {
+	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
 		iwl_write_prph(trans, DEVICE_SET_NMI_REG,
 			       DEVICE_SET_NMI_VAL_DRV);
 		iwl_write_prph(trans, DEVICE_SET_NMI_REG,
 			       DEVICE_SET_NMI_VAL_HW);
 	} else {
-		iwl_write_prph(trans, DEVICE_SET_NMI_8000B_REG,
-			       DEVICE_SET_NMI_8000B_VAL);
+		iwl_write_prph(trans, DEVICE_SET_NMI_8000_REG,
+			       DEVICE_SET_NMI_8000_VAL);
+		iwl_write_prph(trans, DEVICE_SET_NMI_REG,
+			       DEVICE_SET_NMI_VAL_DRV);
 	}
 }
 IWL_EXPORT_SYMBOL(iwl_force_nmi);
diff --git a/drivers/net/wireless/iwlwifi/iwl-modparams.h b/drivers/net/wireless/iwlwifi/iwl-modparams.h
index e8eabd21ccfe..ac2b90df8413 100644
--- a/drivers/net/wireless/iwlwifi/iwl-modparams.h
+++ b/drivers/net/wireless/iwlwifi/iwl-modparams.h
@@ -103,6 +103,7 @@ enum iwl_disable_11n {
  * @debug_level: levels are IWL_DL_*
  * @ant_coupling: antenna coupling in dB, default = 0
  * @d0i3_disable: disable d0i3, default = 1,
+ * @lar_disable: disable LAR (regulatory), default = 0
  * @fw_monitor: allow to use firmware monitor
  */
 struct iwl_mod_params {
@@ -121,6 +122,7 @@ struct iwl_mod_params {
 	char *nvm_file;
 	bool uapsd_disable;
 	bool d0i3_disable;
+	bool lar_disable;
 	bool fw_monitor;
 };
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
index c74f1a4edf23..83903a5025c2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
@@ -99,12 +99,15 @@ enum family_8000_nvm_offsets {
 	/* NVM SW-Section offset (in words) definitions */
 	NVM_SW_SECTION_FAMILY_8000 = 0x1C0,
 	NVM_VERSION_FAMILY_8000 = 0,
-	RADIO_CFG_FAMILY_8000 = 2,
-	SKU_FAMILY_8000 = 4,
-	N_HW_ADDRS_FAMILY_8000 = 5,
+	RADIO_CFG_FAMILY_8000 = 0,
+	SKU_FAMILY_8000 = 2,
+	N_HW_ADDRS_FAMILY_8000 = 3,
 
 	/* NVM REGULATORY -Section offset (in words) definitions */
 	NVM_CHANNELS_FAMILY_8000 = 0,
+	NVM_LAR_OFFSET_FAMILY_8000_OLD = 0x4C7,
+	NVM_LAR_OFFSET_FAMILY_8000 = 0x507,
+	NVM_LAR_ENABLED_FAMILY_8000 = 0x7,
 
 	/* NVM calibration section offset (in words) definitions */
 	NVM_CALIB_SECTION_FAMILY_8000 = 0x2B8,
@@ -146,7 +149,9 @@ static const u8 iwl_nvm_channels_family_8000[] = {
 #define NUM_2GHZ_CHANNELS_FAMILY_8000	14
 #define FIRST_2GHZ_HT_MINUS		5
 #define LAST_2GHZ_HT_PLUS		9
-#define LAST_5GHZ_HT			161
+#define LAST_5GHZ_HT			165
+#define LAST_5GHZ_HT_FAMILY_8000	181
+#define N_HW_ADDR_MASK			0xF
 
 /* rate data (static) */
 static struct ieee80211_rate iwl_cfg80211_rates[] = {
@@ -201,9 +206,57 @@ enum iwl_nvm_channel_flags {
 #define CHECK_AND_PRINT_I(x)	\
 	((ch_flags & NVM_CHANNEL_##x) ? # x " " : "")
 
+static u32 iwl_get_channel_flags(u8 ch_num, int ch_idx, bool is_5ghz,
+				 u16 nvm_flags, const struct iwl_cfg *cfg)
+{
+	u32 flags = IEEE80211_CHAN_NO_HT40;
+	u32 last_5ghz_ht = LAST_5GHZ_HT;
+
+	if (cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		last_5ghz_ht = LAST_5GHZ_HT_FAMILY_8000;
+
+	if (!is_5ghz && (nvm_flags & NVM_CHANNEL_40MHZ)) {
+		if (ch_num <= LAST_2GHZ_HT_PLUS)
+			flags &= ~IEEE80211_CHAN_NO_HT40PLUS;
+		if (ch_num >= FIRST_2GHZ_HT_MINUS)
+			flags &= ~IEEE80211_CHAN_NO_HT40MINUS;
+	} else if (ch_num <= last_5ghz_ht && (nvm_flags & NVM_CHANNEL_40MHZ)) {
+		if ((ch_idx - NUM_2GHZ_CHANNELS) % 2 == 0)
+			flags &= ~IEEE80211_CHAN_NO_HT40PLUS;
+		else
+			flags &= ~IEEE80211_CHAN_NO_HT40MINUS;
+	}
+	if (!(nvm_flags & NVM_CHANNEL_80MHZ))
+		flags |= IEEE80211_CHAN_NO_80MHZ;
+	if (!(nvm_flags & NVM_CHANNEL_160MHZ))
+		flags |= IEEE80211_CHAN_NO_160MHZ;
+
+	if (!(nvm_flags & NVM_CHANNEL_IBSS))
+		flags |= IEEE80211_CHAN_NO_IR;
+
+	if (!(nvm_flags & NVM_CHANNEL_ACTIVE))
+		flags |= IEEE80211_CHAN_NO_IR;
+
+	if (nvm_flags & NVM_CHANNEL_RADAR)
+		flags |= IEEE80211_CHAN_RADAR;
+
+	if (nvm_flags & NVM_CHANNEL_INDOOR_ONLY)
+		flags |= IEEE80211_CHAN_INDOOR_ONLY;
+
+	/* Set the GO concurrent flag only in case that NO_IR is set.
+	 * Otherwise it is meaningless
+	 */
+	if ((nvm_flags & NVM_CHANNEL_GO_CONCURRENT) &&
+	    (flags & IEEE80211_CHAN_NO_IR))
+		flags |= IEEE80211_CHAN_GO_CONCURRENT;
+
+	return flags;
+}
+
 static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 				struct iwl_nvm_data *data,
-				const __le16 * const nvm_ch_flags)
+				const __le16 * const nvm_ch_flags,
+				bool lar_supported)
 {
 	int ch_idx;
 	int n_channels = 0;
@@ -228,9 +281,14 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 
 		if (ch_idx >= num_2ghz_channels &&
 		    !data->sku_cap_band_52GHz_enable)
-			ch_flags &= ~NVM_CHANNEL_VALID;
+			continue;
 
-		if (!(ch_flags & NVM_CHANNEL_VALID)) {
+		if (!lar_supported && !(ch_flags & NVM_CHANNEL_VALID)) {
+			/*
+			 * Channels might become valid later if lar is
+			 * supported, hence we still want to add them to
+			 * the list of supported channels to cfg80211.
+			 */
 			IWL_DEBUG_EEPROM(dev,
 					 "Ch. %d Flags %x [%sGHz] - No traffic\n",
 					 nvm_chan[ch_idx],
@@ -250,45 +308,6 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 			ieee80211_channel_to_frequency(
 				channel->hw_value, channel->band);
 
-		/* TODO: Need to be dependent to the NVM */
-		channel->flags = IEEE80211_CHAN_NO_HT40;
-		if (ch_idx < num_2ghz_channels &&
-		    (ch_flags & NVM_CHANNEL_40MHZ)) {
-			if (nvm_chan[ch_idx] <= LAST_2GHZ_HT_PLUS)
-				channel->flags &= ~IEEE80211_CHAN_NO_HT40PLUS;
-			if (nvm_chan[ch_idx] >= FIRST_2GHZ_HT_MINUS)
-				channel->flags &= ~IEEE80211_CHAN_NO_HT40MINUS;
-		} else if (nvm_chan[ch_idx] <= LAST_5GHZ_HT &&
-			   (ch_flags & NVM_CHANNEL_40MHZ)) {
-			if ((ch_idx - num_2ghz_channels) % 2 == 0)
-				channel->flags &= ~IEEE80211_CHAN_NO_HT40PLUS;
-			else
-				channel->flags &= ~IEEE80211_CHAN_NO_HT40MINUS;
-		}
-		if (!(ch_flags & NVM_CHANNEL_80MHZ))
-			channel->flags |= IEEE80211_CHAN_NO_80MHZ;
-		if (!(ch_flags & NVM_CHANNEL_160MHZ))
-			channel->flags |= IEEE80211_CHAN_NO_160MHZ;
-
-		if (!(ch_flags & NVM_CHANNEL_IBSS))
-			channel->flags |= IEEE80211_CHAN_NO_IR;
-
-		if (!(ch_flags & NVM_CHANNEL_ACTIVE))
-			channel->flags |= IEEE80211_CHAN_NO_IR;
-
-		if (ch_flags & NVM_CHANNEL_RADAR)
-			channel->flags |= IEEE80211_CHAN_RADAR;
-
-		if (ch_flags & NVM_CHANNEL_INDOOR_ONLY)
-			channel->flags |= IEEE80211_CHAN_INDOOR_ONLY;
-
-		/* Set the GO concurrent flag only in case that NO_IR is set.
-		 * Otherwise it is meaningless
-		 */
-		if ((ch_flags & NVM_CHANNEL_GO_CONCURRENT) &&
-		    (channel->flags & IEEE80211_CHAN_NO_IR))
-			channel->flags |= IEEE80211_CHAN_GO_CONCURRENT;
-
 		/* Initialize regulatory-based run-time data */
 
 		/*
@@ -297,6 +316,15 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 		 */
 		channel->max_power = IWL_DEFAULT_MAX_TX_POWER;
 		is_5ghz = channel->band == IEEE80211_BAND_5GHZ;
+
+		/* don't put limitations in case we're using LAR */
+		if (!lar_supported)
+			channel->flags = iwl_get_channel_flags(nvm_chan[ch_idx],
+							       ch_idx, is_5ghz,
+							       ch_flags, cfg);
+		else
+			channel->flags = 0;
+
 		IWL_DEBUG_EEPROM(dev,
 				 "Ch. %d [%sGHz] %s%s%s%s%s%s%s(0x%02x %ddBm): Ad-Hoc %ssupported\n",
 				 channel->hw_value,
@@ -370,8 +398,8 @@ static void iwl_init_vht_hw_capab(const struct iwl_cfg *cfg,
 
 static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
 			    struct iwl_nvm_data *data,
-			    const __le16 *ch_section, bool enable_vht,
-			    u8 tx_chains, u8 rx_chains)
+			    const __le16 *ch_section,
+			    u8 tx_chains, u8 rx_chains, bool lar_supported)
 {
 	int n_channels;
 	int n_used = 0;
@@ -380,11 +408,12 @@ static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		n_channels = iwl_init_channel_map(
 				dev, cfg, data,
-				&ch_section[NVM_CHANNELS]);
+				&ch_section[NVM_CHANNELS], lar_supported);
 	else
 		n_channels = iwl_init_channel_map(
 				dev, cfg, data,
-				&ch_section[NVM_CHANNELS_FAMILY_8000]);
+				&ch_section[NVM_CHANNELS_FAMILY_8000],
+				lar_supported);
 
 	sband = &data->bands[IEEE80211_BAND_2GHZ];
 	sband->band = IEEE80211_BAND_2GHZ;
@@ -403,7 +432,7 @@ static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
 					  IEEE80211_BAND_5GHZ);
 	iwl_init_ht_hw_capab(cfg, data, &sband->ht_cap, IEEE80211_BAND_5GHZ,
 			     tx_chains, rx_chains);
-	if (enable_vht)
+	if (data->sku_cap_11ac_enable)
 		iwl_init_vht_hw_capab(cfg, data, &sband->vht_cap,
 				      tx_chains, rx_chains);
 
@@ -412,17 +441,16 @@ static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
 			    n_used, n_channels);
 }
 
-static int iwl_get_sku(const struct iwl_cfg *cfg,
-		       const __le16 *nvm_sw)
+static int iwl_get_sku(const struct iwl_cfg *cfg, const __le16 *nvm_sw,
+		       const __le16 *phy_sku)
 {
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		return le16_to_cpup(nvm_sw + SKU);
-	else
-		return le32_to_cpup((__le32 *)(nvm_sw + SKU_FAMILY_8000));
+
+	return le32_to_cpup((__le32 *)(phy_sku + SKU_FAMILY_8000));
 }
 
-static int iwl_get_nvm_version(const struct iwl_cfg *cfg,
-			       const __le16 *nvm_sw)
+static int iwl_get_nvm_version(const struct iwl_cfg *cfg, const __le16 *nvm_sw)
 {
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		return le16_to_cpup(nvm_sw + NVM_VERSION);
@@ -431,24 +459,26 @@ static int iwl_get_nvm_version(const struct iwl_cfg *cfg,
 					       NVM_VERSION_FAMILY_8000));
 }
 
-static int iwl_get_radio_cfg(const struct iwl_cfg *cfg,
-			     const __le16 *nvm_sw)
+static int iwl_get_radio_cfg(const struct iwl_cfg *cfg, const __le16 *nvm_sw,
+			     const __le16 *phy_sku)
 {
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		return le16_to_cpup(nvm_sw + RADIO_CFG);
-	else
-		return le32_to_cpup((__le32 *)(nvm_sw + RADIO_CFG_FAMILY_8000));
+
+	return le32_to_cpup((__le32 *)(nvm_sw + RADIO_CFG_FAMILY_8000));
+
 }
 
-#define N_HW_ADDRS_MASK_FAMILY_8000	0xF
-static int iwl_get_n_hw_addrs(const struct iwl_cfg *cfg,
-			      const __le16 *nvm_sw)
+static int iwl_get_n_hw_addrs(const struct iwl_cfg *cfg, const __le16 *nvm_sw)
 {
+	int n_hw_addr;
+
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		return le16_to_cpup(nvm_sw + N_HW_ADDRS);
-	else
-		return le32_to_cpup((__le32 *)(nvm_sw + N_HW_ADDRS_FAMILY_8000))
-		       & N_HW_ADDRS_MASK_FAMILY_8000;
+
+	n_hw_addr = le32_to_cpup((__le32 *)(nvm_sw + N_HW_ADDRS_FAMILY_8000));
+
+	return n_hw_addr & N_HW_ADDR_MASK;
 }
 
 static void iwl_set_radio_cfg(const struct iwl_cfg *cfg,
@@ -491,7 +521,8 @@ static void iwl_set_hw_address_family_8000(struct device *dev,
 					   const struct iwl_cfg *cfg,
 					   struct iwl_nvm_data *data,
 					   const __le16 *mac_override,
-					   const __le16 *nvm_hw)
+					   const __le16 *nvm_hw,
+					   u32 mac_addr0, u32 mac_addr1)
 {
 	const u8 *hw_addr;
 
@@ -515,48 +546,17 @@ static void iwl_set_hw_address_family_8000(struct device *dev,
 	}
 
 	if (nvm_hw) {
-		/* read the MAC address from OTP */
-		if (!dev_is_pci(dev) || (data->nvm_version < 0xE08)) {
-			/* read the mac address from the WFPM location */
-			hw_addr = (const u8 *)(nvm_hw +
-					       HW_ADDR0_WFPM_FAMILY_8000);
-			data->hw_addr[0] = hw_addr[3];
-			data->hw_addr[1] = hw_addr[2];
-			data->hw_addr[2] = hw_addr[1];
-			data->hw_addr[3] = hw_addr[0];
-
-			hw_addr = (const u8 *)(nvm_hw +
-					       HW_ADDR1_WFPM_FAMILY_8000);
-			data->hw_addr[4] = hw_addr[1];
-			data->hw_addr[5] = hw_addr[0];
-		} else if ((data->nvm_version >= 0xE08) &&
-			   (data->nvm_version < 0xE0B)) {
-			/* read "reverse order"  from the PCIe location */
-			hw_addr = (const u8 *)(nvm_hw +
-					       HW_ADDR0_PCIE_FAMILY_8000);
-			data->hw_addr[5] = hw_addr[2];
-			data->hw_addr[4] = hw_addr[1];
-			data->hw_addr[3] = hw_addr[0];
-
-			hw_addr = (const u8 *)(nvm_hw +
-					       HW_ADDR1_PCIE_FAMILY_8000);
-			data->hw_addr[2] = hw_addr[3];
-			data->hw_addr[1] = hw_addr[2];
-			data->hw_addr[0] = hw_addr[1];
-		} else {
-			/* read from the PCIe location */
-			hw_addr = (const u8 *)(nvm_hw +
-					       HW_ADDR0_PCIE_FAMILY_8000);
-			data->hw_addr[5] = hw_addr[0];
-			data->hw_addr[4] = hw_addr[1];
-			data->hw_addr[3] = hw_addr[2];
-
-			hw_addr = (const u8 *)(nvm_hw +
-					       HW_ADDR1_PCIE_FAMILY_8000);
-			data->hw_addr[2] = hw_addr[1];
-			data->hw_addr[1] = hw_addr[2];
-			data->hw_addr[0] = hw_addr[3];
-		}
+		/* read the MAC address from HW resisters */
+		hw_addr = (const u8 *)&mac_addr0;
+		data->hw_addr[0] = hw_addr[3];
+		data->hw_addr[1] = hw_addr[2];
+		data->hw_addr[2] = hw_addr[1];
+		data->hw_addr[3] = hw_addr[0];
+
+		hw_addr = (const u8 *)&mac_addr1;
+		data->hw_addr[4] = hw_addr[1];
+		data->hw_addr[5] = hw_addr[0];
+
 		if (!is_valid_ether_addr(data->hw_addr))
 			IWL_ERR_DEV(dev,
 				    "mac address from hw section is not valid\n");
@@ -571,11 +571,14 @@ struct iwl_nvm_data *
 iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 		   const __le16 *nvm_hw, const __le16 *nvm_sw,
 		   const __le16 *nvm_calib, const __le16 *regulatory,
-		   const __le16 *mac_override, u8 tx_chains, u8 rx_chains)
+		   const __le16 *mac_override, const __le16 *phy_sku,
+		   u8 tx_chains, u8 rx_chains, bool lar_fw_supported,
+		   u32 mac_addr0, u32 mac_addr1)
 {
 	struct iwl_nvm_data *data;
 	u32 sku;
 	u32 radio_cfg;
+	u16 lar_config;
 
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		data = kzalloc(sizeof(*data) +
@@ -592,20 +595,21 @@ iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 
 	data->nvm_version = iwl_get_nvm_version(cfg, nvm_sw);
 
-	radio_cfg = iwl_get_radio_cfg(cfg, nvm_sw);
+	radio_cfg = iwl_get_radio_cfg(cfg, nvm_sw, phy_sku);
 	iwl_set_radio_cfg(cfg, data, radio_cfg);
 	if (data->valid_tx_ant)
 		tx_chains &= data->valid_tx_ant;
 	if (data->valid_rx_ant)
 		rx_chains &= data->valid_rx_ant;
 
-	sku = iwl_get_sku(cfg, nvm_sw);
+	sku = iwl_get_sku(cfg, nvm_sw, phy_sku);
 	data->sku_cap_band_24GHz_enable = sku & NVM_SKU_CAP_BAND_24GHZ;
 	data->sku_cap_band_52GHz_enable = sku & NVM_SKU_CAP_BAND_52GHZ;
 	data->sku_cap_11n_enable = sku & NVM_SKU_CAP_11N_ENABLE;
-	data->sku_cap_11ac_enable = sku & NVM_SKU_CAP_11AC_ENABLE;
 	if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
 		data->sku_cap_11n_enable = false;
+	data->sku_cap_11ac_enable = data->sku_cap_11n_enable &&
+				    (sku & NVM_SKU_CAP_11AC_ENABLE);
 
 	data->n_hw_addrs = iwl_get_n_hw_addrs(cfg, nvm_sw);
 
@@ -626,16 +630,23 @@ iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 		iwl_set_hw_address(cfg, data, nvm_hw);
 
 		iwl_init_sbands(dev, cfg, data, nvm_sw,
-				sku & NVM_SKU_CAP_11AC_ENABLE, tx_chains,
-				rx_chains);
+				tx_chains, rx_chains, lar_fw_supported);
 	} else {
+		u16 lar_offset = data->nvm_version < 0xE39 ?
+				 NVM_LAR_OFFSET_FAMILY_8000_OLD :
+				 NVM_LAR_OFFSET_FAMILY_8000;
+
+		lar_config = le16_to_cpup(regulatory + lar_offset);
+		data->lar_enabled = !!(lar_config &
+				       NVM_LAR_ENABLED_FAMILY_8000);
+
 		/* MAC address in family 8000 */
 		iwl_set_hw_address_family_8000(dev, cfg, data, mac_override,
-					       nvm_hw);
+					       nvm_hw, mac_addr0, mac_addr1);
 
 		iwl_init_sbands(dev, cfg, data, regulatory,
-				sku & NVM_SKU_CAP_11AC_ENABLE, tx_chains,
-				rx_chains);
+				tx_chains, rx_chains,
+				lar_fw_supported && data->lar_enabled);
 	}
 
 	data->calib_version = 255;
@@ -643,3 +654,164 @@ iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 	return data;
 }
 IWL_EXPORT_SYMBOL(iwl_parse_nvm_data);
+
+static u32 iwl_nvm_get_regdom_bw_flags(const u8 *nvm_chan,
+				       int ch_idx, u16 nvm_flags,
+				       const struct iwl_cfg *cfg)
+{
+	u32 flags = NL80211_RRF_NO_HT40;
+	u32 last_5ghz_ht = LAST_5GHZ_HT;
+
+	if (cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		last_5ghz_ht = LAST_5GHZ_HT_FAMILY_8000;
+
+	if (ch_idx < NUM_2GHZ_CHANNELS &&
+	    (nvm_flags & NVM_CHANNEL_40MHZ)) {
+		if (nvm_chan[ch_idx] <= LAST_2GHZ_HT_PLUS)
+			flags &= ~NL80211_RRF_NO_HT40PLUS;
+		if (nvm_chan[ch_idx] >= FIRST_2GHZ_HT_MINUS)
+			flags &= ~NL80211_RRF_NO_HT40MINUS;
+	} else if (nvm_chan[ch_idx] <= last_5ghz_ht &&
+		   (nvm_flags & NVM_CHANNEL_40MHZ)) {
+		if ((ch_idx - NUM_2GHZ_CHANNELS) % 2 == 0)
+			flags &= ~NL80211_RRF_NO_HT40PLUS;
+		else
+			flags &= ~NL80211_RRF_NO_HT40MINUS;
+	}
+
+	if (!(nvm_flags & NVM_CHANNEL_80MHZ))
+		flags |= NL80211_RRF_NO_80MHZ;
+	if (!(nvm_flags & NVM_CHANNEL_160MHZ))
+		flags |= NL80211_RRF_NO_160MHZ;
+
+	if (!(nvm_flags & NVM_CHANNEL_ACTIVE))
+		flags |= NL80211_RRF_NO_IR;
+
+	if (nvm_flags & NVM_CHANNEL_RADAR)
+		flags |= NL80211_RRF_DFS;
+
+	if (nvm_flags & NVM_CHANNEL_INDOOR_ONLY)
+		flags |= NL80211_RRF_NO_OUTDOOR;
+
+	/* Set the GO concurrent flag only in case that NO_IR is set.
+	 * Otherwise it is meaningless
+	 */
+	if ((nvm_flags & NVM_CHANNEL_GO_CONCURRENT) &&
+	    (flags & NL80211_RRF_NO_IR))
+		flags |= NL80211_RRF_GO_CONCURRENT;
+
+	return flags;
+}
+
+struct ieee80211_regdomain *
+iwl_parse_nvm_mcc_info(struct device *dev, const struct iwl_cfg *cfg,
+		       int num_of_ch, __le32 *channels, u16 fw_mcc)
+{
+	int ch_idx;
+	u16 ch_flags, prev_ch_flags = 0;
+	const u8 *nvm_chan = cfg->device_family == IWL_DEVICE_FAMILY_8000 ?
+			     iwl_nvm_channels_family_8000 : iwl_nvm_channels;
+	struct ieee80211_regdomain *regd;
+	int size_of_regd;
+	struct ieee80211_reg_rule *rule;
+	enum ieee80211_band band;
+	int center_freq, prev_center_freq = 0;
+	int valid_rules = 0;
+	bool new_rule;
+	int max_num_ch = cfg->device_family == IWL_DEVICE_FAMILY_8000 ?
+			 IWL_NUM_CHANNELS_FAMILY_8000 : IWL_NUM_CHANNELS;
+
+	if (WARN_ON_ONCE(num_of_ch > NL80211_MAX_SUPP_REG_RULES))
+		return ERR_PTR(-EINVAL);
+
+	if (WARN_ON(num_of_ch > max_num_ch))
+		num_of_ch = max_num_ch;
+
+	IWL_DEBUG_DEV(dev, IWL_DL_LAR, "building regdom for %d channels\n",
+		      num_of_ch);
+
+	/* build a regdomain rule for every valid channel */
+	size_of_regd =
+		sizeof(struct ieee80211_regdomain) +
+		num_of_ch * sizeof(struct ieee80211_reg_rule);
+
+	regd = kzalloc(size_of_regd, GFP_KERNEL);
+	if (!regd)
+		return ERR_PTR(-ENOMEM);
+
+	for (ch_idx = 0; ch_idx < num_of_ch; ch_idx++) {
+		ch_flags = (u16)__le32_to_cpup(channels + ch_idx);
+		band = (ch_idx < NUM_2GHZ_CHANNELS) ?
+		       IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+		center_freq = ieee80211_channel_to_frequency(nvm_chan[ch_idx],
+							     band);
+		new_rule = false;
+
+		if (!(ch_flags & NVM_CHANNEL_VALID)) {
+			IWL_DEBUG_DEV(dev, IWL_DL_LAR,
+				      "Ch. %d Flags %x [%sGHz] - No traffic\n",
+				      nvm_chan[ch_idx],
+				      ch_flags,
+				      (ch_idx >= NUM_2GHZ_CHANNELS) ?
+				      "5.2" : "2.4");
+			continue;
+		}
+
+		/* we can't continue the same rule */
+		if (ch_idx == 0 || prev_ch_flags != ch_flags ||
+		    center_freq - prev_center_freq > 20) {
+			valid_rules++;
+			new_rule = true;
+		}
+
+		rule = &regd->reg_rules[valid_rules - 1];
+
+		if (new_rule)
+			rule->freq_range.start_freq_khz =
+						MHZ_TO_KHZ(center_freq - 10);
+
+		rule->freq_range.end_freq_khz = MHZ_TO_KHZ(center_freq + 10);
+
+		/* this doesn't matter - not used by FW */
+		rule->power_rule.max_antenna_gain = DBI_TO_MBI(6);
+		rule->power_rule.max_eirp =
+			DBM_TO_MBM(IWL_DEFAULT_MAX_TX_POWER);
+
+		rule->flags = iwl_nvm_get_regdom_bw_flags(nvm_chan, ch_idx,
+							  ch_flags, cfg);
+
+		/* rely on auto-calculation to merge BW of contiguous chans */
+		rule->flags |= NL80211_RRF_AUTO_BW;
+		rule->freq_range.max_bandwidth_khz = 0;
+
+		prev_ch_flags = ch_flags;
+		prev_center_freq = center_freq;
+
+		IWL_DEBUG_DEV(dev, IWL_DL_LAR,
+			      "Ch. %d [%sGHz] %s%s%s%s%s%s%s%s%s(0x%02x): Ad-Hoc %ssupported\n",
+			      center_freq,
+			      band == IEEE80211_BAND_5GHZ ? "5.2" : "2.4",
+			      CHECK_AND_PRINT_I(VALID),
+			      CHECK_AND_PRINT_I(ACTIVE),
+			      CHECK_AND_PRINT_I(RADAR),
+			      CHECK_AND_PRINT_I(WIDE),
+			      CHECK_AND_PRINT_I(40MHZ),
+			      CHECK_AND_PRINT_I(80MHZ),
+			      CHECK_AND_PRINT_I(160MHZ),
+			      CHECK_AND_PRINT_I(INDOOR_ONLY),
+			      CHECK_AND_PRINT_I(GO_CONCURRENT),
+			      ch_flags,
+			      ((ch_flags & NVM_CHANNEL_ACTIVE) &&
+			       !(ch_flags & NVM_CHANNEL_RADAR))
+					 ? "" : "not ");
+	}
+
+	regd->n_reg_rules = valid_rules;
+
+	/* set alpha2 from FW. */
+	regd->alpha2[0] = fw_mcc >> 8;
+	regd->alpha2[1] = fw_mcc & 0xff;
+
+	return regd;
+}
+IWL_EXPORT_SYMBOL(iwl_parse_nvm_mcc_info);
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
index c9c45a39d212..822ba52e0e5a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
@@ -62,6 +62,7 @@
 #ifndef __iwl_nvm_parse_h__
 #define __iwl_nvm_parse_h__
 
+#include <net/cfg80211.h>
 #include "iwl-eeprom-parse.h"
 
 /**
@@ -76,6 +77,21 @@ struct iwl_nvm_data *
 iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 		   const __le16 *nvm_hw, const __le16 *nvm_sw,
 		   const __le16 *nvm_calib, const __le16 *regulatory,
-		   const __le16 *mac_override, u8 tx_chains, u8 rx_chains);
+		   const __le16 *mac_override, const __le16 *phy_sku,
+		   u8 tx_chains, u8 rx_chains, bool lar_fw_supported,
+		   u32 mac_addr0, u32 mac_addr1);
+
+/**
+ * iwl_parse_mcc_info - parse MCC (mobile country code) info coming from FW
+ *
+ * This function parses the regulatory channel data received as a
+ * MCC_UPDATE_CMD command. It returns a newly allocation regulatory domain,
+ * to be fed into the regulatory core. An ERR_PTR is returned on error.
+ * If not given to the regulatory core, the user is responsible for freeing
+ * the regdomain returned here with kfree.
+ */
+struct ieee80211_regdomain *
+iwl_parse_nvm_mcc_info(struct device *dev, const struct iwl_cfg *cfg,
+		       int num_of_ch, __le32 *channels, u16 fw_mcc);
 
 #endif /* __iwl_nvm_parse_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
index 17de6d46222a..ce1cdd7604e8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
@@ -94,7 +94,7 @@ struct iwl_cfg;
  * The operational mode has a very simple life cycle.
  *
  *	1) The driver layer (iwl-drv.c) chooses the op_mode based on the
- *	   capabilities advertized by the fw file (in TLV format).
+ *	   capabilities advertised by the fw file (in TLV format).
  *	2) The driver layer starts the op_mode (ops->start)
  *	3) The op_mode registers mac80211
  *	4) The op_mode is governed by mac80211
@@ -116,7 +116,7 @@ struct iwl_cfg;
  *	May sleep
  * @rx: Rx notification to the op_mode. rxb is the Rx buffer itself. Cmd is the
  *	HCMD this Rx responds to. Can't sleep.
- * @napi_add: NAPI initialisation. The transport is fully responsible for NAPI,
+ * @napi_add: NAPI initialization. The transport is fully responsible for NAPI,
  *	but the higher layers need to know about it (in particular mac80211 to
  *	to able to call the right NAPI RX functions); this function is needed
  *	to eventually call netif_napi_add() with higher layer involvement.
diff --git a/drivers/net/wireless/iwlwifi/iwl-phy-db.c b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
index d4fb5cad07ea..a105455b6a24 100644
--- a/drivers/net/wireless/iwlwifi/iwl-phy-db.c
+++ b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
@@ -72,7 +72,7 @@
 #include "iwl-trans.h"
 
 #define CHANNEL_NUM_SIZE	4	/* num of channels in calib_ch size */
-#define IWL_NUM_PAPD_CH_GROUPS	7
+#define IWL_NUM_PAPD_CH_GROUPS	9
 #define IWL_NUM_TXP_CH_GROUPS	9
 
 struct iwl_phy_db_entry {
@@ -125,7 +125,7 @@ struct iwl_phy_db_chg_txp {
 } __packed;
 
 /*
- * phy db - Receieve phy db chunk after calibrations
+ * phy db - Receive phy db chunk after calibrations
  */
 struct iwl_calib_res_notif_phy_db {
 	__le16 type;
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index 6221e4dfc64f..88a57e6e232f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -110,8 +110,8 @@
 #define DEVICE_SET_NMI_REG 0x00a01c30
 #define DEVICE_SET_NMI_VAL_HW BIT(0)
 #define DEVICE_SET_NMI_VAL_DRV BIT(7)
-#define DEVICE_SET_NMI_8000B_REG 0x00a01c24
-#define DEVICE_SET_NMI_8000B_VAL 0x1000000
+#define DEVICE_SET_NMI_8000_REG 0x00a01c24
+#define DEVICE_SET_NMI_8000_VAL 0x1000000
 
 /* Shared registers (0x0..0x3ff, via target indirect or periphery */
 #define SHR_BASE	0x00a10000
@@ -295,25 +295,6 @@
 #define OSC_CLK				(0xa04068)
 #define OSC_CLK_FORCE_CONTROL		(0x8)
 
-/* SECURE boot registers */
-#define LMPM_SECURE_BOOT_CONFIG_ADDR	(0x100)
-enum secure_boot_config_reg {
-	LMPM_SECURE_BOOT_CONFIG_INSPECTOR_BURNED_IN_OTP	= 0x00000001,
-	LMPM_SECURE_BOOT_CONFIG_INSPECTOR_NOT_REQ	= 0x00000002,
-};
-
-#define LMPM_SECURE_BOOT_CPU1_STATUS_ADDR_B0	(0xA01E30)
-#define LMPM_SECURE_BOOT_CPU1_STATUS_ADDR	(0x1E30)
-#define LMPM_SECURE_BOOT_CPU2_STATUS_ADDR	(0x1E34)
-enum secure_boot_status_reg {
-	LMPM_SECURE_BOOT_CPU_STATUS_VERF_STATUS		= 0x00000001,
-	LMPM_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED	= 0x00000002,
-	LMPM_SECURE_BOOT_CPU_STATUS_VERF_SUCCESS	= 0x00000004,
-	LMPM_SECURE_BOOT_CPU_STATUS_VERF_FAIL		= 0x00000008,
-	LMPM_SECURE_BOOT_CPU_STATUS_SIGN_VERF_FAIL	= 0x00000010,
-	LMPM_SECURE_BOOT_STATUS_SUCCESS			= 0x00000003,
-};
-
 #define FH_UCODE_LOAD_STATUS		(0x1AF0)
 #define CSR_UCODE_LOAD_STATUS_ADDR	(0x1E70)
 enum secure_load_status_reg {
@@ -334,8 +315,6 @@ enum secure_load_status_reg {
 #define LMPM_SECURE_CPU1_HDR_MEM_SPACE		(0x420000)
 #define LMPM_SECURE_CPU2_HDR_MEM_SPACE		(0x420400)
 
-#define LMPM_SECURE_TIME_OUT	(100) /* 10 micro */
-
 /* Rx FIFO */
 #define RXF_SIZE_ADDR			(0xa00c88)
 #define RXF_RD_D_SPACE			(0xa00c40)
@@ -370,7 +349,33 @@ enum secure_load_status_reg {
 #define MON_BUFF_CYCLE_CNT		(0xa03c48)
 
 #define DBGC_IN_SAMPLE			(0xa03c00)
-#define DBGC_OUT_CTRL			(0xa03c0c)
+
+/* enable the ID buf for read */
+#define WFPM_PS_CTL_CLR			0xA0300C
+#define WFMP_MAC_ADDR_0			0xA03080
+#define WFMP_MAC_ADDR_1			0xA03084
+#define LMPM_PMG_EN			0xA01CEC
+#define RADIO_REG_SYS_MANUAL_DFT_0	0xAD4078
+#define RFIC_REG_RD			0xAD0470
+#define WFPM_CTRL_REG			0xA03030
+enum {
+	ENABLE_WFPM = BIT(31),
+	WFPM_AUX_CTL_AUX_IF_MAC_OWNER_MSK	= 0x80000000,
+};
+
+#define AUX_MISC_REG			0xA200B0
+enum {
+	HW_STEP_LOCATION_BITS = 24,
+};
+
+#define AUX_MISC_MASTER1_EN		0xA20818
+enum aux_misc_master1_en {
+	AUX_MISC_MASTER1_EN_SBE_MSK	= 0x1,
+};
+
+#define AUX_MISC_MASTER1_SMPHR_STATUS	0xA20800
+#define RSA_ENABLE			0xA24B08
+#define PREG_AUX_BUS_WPROT_0		0xA04CC0
 
 /* FW chicken bits */
 #define LMPM_CHICK			0xA01FF8
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index a96bd8db6ceb..6dfed1259260 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -77,10 +77,10 @@
 /**
  * DOC: Transport layer - what is it ?
  *
- * The tranport layer is the layer that deals with the HW directly. It provides
+ * The transport layer is the layer that deals with the HW directly. It provides
  * an abstraction of the underlying HW to the upper layer. The transport layer
  * doesn't provide any policy, algorithm or anything of this kind, but only
- * mechanisms to make the HW do something.It is not completely stateless but
+ * mechanisms to make the HW do something. It is not completely stateless but
  * close to it.
  * We will have an implementation for each different supported bus.
  */
@@ -111,10 +111,10 @@
 /**
  * DOC: Host command section
  *
- * A host command is a commaned issued by the upper layer to the fw. There are
+ * A host command is a command issued by the upper layer to the fw. There are
  * several versions of fw that have several APIs. The transport layer is
  * completely agnostic to these differences.
- * The transport does provide helper functionnality (i.e. SYNC / ASYNC mode),
+ * The transport does provide helper functionality (i.e. SYNC / ASYNC mode),
  */
 #define SEQ_TO_QUEUE(s)	(((s) >> 8) & 0x1f)
 #define QUEUE_TO_SEQ(q)	(((q) & 0x1f) << 8)
@@ -195,7 +195,7 @@ static inline u32 iwl_rx_packet_payload_len(const struct iwl_rx_packet *pkt)
  * @CMD_WANT_SKB: Not valid with CMD_ASYNC. The caller needs the buffer of
  *	the response. The caller needs to call iwl_free_resp when done.
  * @CMD_HIGH_PRIO: The command is high priority - it goes to the front of the
- *	command queue, but after other high priority commands. valid only
+ *	command queue, but after other high priority commands. Valid only
  *	with CMD_ASYNC.
  * @CMD_SEND_IN_IDLE: The command should be sent even when the trans is idle.
  * @CMD_MAKE_TRANS_IDLE: The command response should mark the trans as idle.
@@ -458,6 +458,8 @@ struct iwl_trans_txq_scd_cfg {
  * @txq_disable: de-configure a Tx queue to send AMPDUs
  *	Must be atomic
  * @wait_tx_queue_empty: wait until tx queues are empty. May sleep.
+ * @freeze_txq_timer: prevents the timer of the queue from firing until the
+ *	queue is set to awake. Must be atomic.
  * @dbgfs_register: add the dbgfs files under this directory. Files will be
  *	automatically deleted.
  * @write8: write a u8 to a register at offset ofs from the BAR
@@ -517,6 +519,8 @@ struct iwl_trans_ops {
 
 	int (*dbgfs_register)(struct iwl_trans *trans, struct dentry* dir);
 	int (*wait_tx_queue_empty)(struct iwl_trans *trans, u32 txq_bm);
+	void (*freeze_txq_timer)(struct iwl_trans *trans, unsigned long txqs,
+				 bool freeze);
 
 	void (*write8)(struct iwl_trans *trans, u32 ofs, u8 val);
 	void (*write32)(struct iwl_trans *trans, u32 ofs, u32 val);
@@ -578,7 +582,7 @@ enum iwl_d0i3_mode {
  * @cfg - pointer to the configuration
  * @status: a bit-mask of transport status flags
  * @dev - pointer to struct device * that represents the device
- * @hw_id: a u32 with the ID of the device / subdevice.
+ * @hw_id: a u32 with the ID of the device / sub-device.
  *	Set during transport allocation.
  * @hw_id_str: a string with info about HW ID. Set during transport allocation.
  * @pm_support: set to true in start_hw if link pm is supported
@@ -595,6 +599,7 @@ enum iwl_d0i3_mode {
  * @dflt_pwr_limit: default power limit fetched from the platform (ACPI)
  * @dbg_dest_tlv: points to the destination TLV for debug
  * @dbg_conf_tlv: array of pointers to configuration TLVs for debug
+ * @dbg_trigger_tlv: array of pointers to triggers TLVs for debug
  * @dbg_dest_reg_num: num of reg_ops in %dbg_dest_tlv
  */
 struct iwl_trans {
@@ -628,7 +633,8 @@ struct iwl_trans {
 	u64 dflt_pwr_limit;
 
 	const struct iwl_fw_dbg_dest_tlv *dbg_dest_tlv;
-	const struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_MAX];
+	const struct iwl_fw_dbg_conf_tlv *dbg_conf_tlv[FW_DBG_CONF_MAX];
+	struct iwl_fw_dbg_trigger_tlv * const *dbg_trigger_tlv;
 	u8 dbg_dest_reg_num;
 
 	enum iwl_d0i3_mode d0i3_mode;
@@ -871,6 +877,17 @@ void iwl_trans_ac_txq_enable(struct iwl_trans *trans, int queue, int fifo,
 	iwl_trans_txq_enable_cfg(trans, queue, 0, &cfg, queue_wdg_timeout);
 }
 
+static inline void iwl_trans_freeze_txq_timer(struct iwl_trans *trans,
+					      unsigned long txqs,
+					      bool freeze)
+{
+	if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+
+	if (trans->ops->freeze_txq_timer)
+		trans->ops->freeze_txq_timer(trans, txqs, freeze);
+}
+
 static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans,
 						u32 txqs)
 {
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/iwlwifi/mvm/coex.c
index 1ec4d55155f7..13a0a03158de 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex.c
@@ -72,158 +72,6 @@
 #include "mvm.h"
 #include "iwl-debug.h"
 
-const u32 iwl_bt_ctl_kill_msk[BT_KILL_MSK_MAX] = {
-	[BT_KILL_MSK_DEFAULT] = 0xfffffc00,
-	[BT_KILL_MSK_NEVER] = 0xffffffff,
-	[BT_KILL_MSK_ALWAYS] = 0,
-};
-
-const u8 iwl_bt_cts_kill_msk[BT_MAX_AG][BT_COEX_MAX_LUT] = {
-	{
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-	},
-	{
-		BT_KILL_MSK_NEVER,
-		BT_KILL_MSK_NEVER,
-		BT_KILL_MSK_NEVER,
-	},
-	{
-		BT_KILL_MSK_NEVER,
-		BT_KILL_MSK_NEVER,
-		BT_KILL_MSK_NEVER,
-	},
-	{
-		BT_KILL_MSK_DEFAULT,
-		BT_KILL_MSK_NEVER,
-		BT_KILL_MSK_DEFAULT,
-	},
-};
-
-const u8 iwl_bt_ack_kill_msk[BT_MAX_AG][BT_COEX_MAX_LUT] = {
-	{
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-	},
-	{
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-	},
-	{
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_ALWAYS,
-	},
-	{
-		BT_KILL_MSK_DEFAULT,
-		BT_KILL_MSK_ALWAYS,
-		BT_KILL_MSK_DEFAULT,
-	},
-};
-
-static const __le32 iwl_bt_prio_boost[BT_COEX_BOOST_SIZE] = {
-	cpu_to_le32(0xf0f0f0f0), /* 50% */
-	cpu_to_le32(0xc0c0c0c0), /* 25% */
-	cpu_to_le32(0xfcfcfcfc), /* 75% */
-	cpu_to_le32(0xfefefefe), /* 87.5% */
-};
-
-static const __le32 iwl_single_shared_ant[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE] = {
-	{
-		cpu_to_le32(0x40000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x44000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x40000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x44000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xf0005000),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xf0005000),
-	},
-	{
-		cpu_to_le32(0x40000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x44000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x40000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x44000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xf0005000),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xf0005000),
-	},
-	{
-		cpu_to_le32(0x40000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x44000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x40000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x44000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xf0005000),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xf0005000),
-	},
-};
-
-static const __le32 iwl_combined_lookup[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE] = {
-	{
-		/* Tight */
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xaeaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xcc00ff28),
-		cpu_to_le32(0x0000aaaa),
-		cpu_to_le32(0xcc00aaaa),
-		cpu_to_le32(0x0000aaaa),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0x00004000),
-		cpu_to_le32(0xf0005000),
-		cpu_to_le32(0xf0005000),
-	},
-	{
-		/* Loose */
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xcc00ff28),
-		cpu_to_le32(0x0000aaaa),
-		cpu_to_le32(0xcc00aaaa),
-		cpu_to_le32(0x0000aaaa),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0x00000000),
-		cpu_to_le32(0xf0005000),
-		cpu_to_le32(0xf0005000),
-	},
-	{
-		/* Tx Tx disabled */
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xeeaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xcc00ff28),
-		cpu_to_le32(0x0000aaaa),
-		cpu_to_le32(0xcc00aaaa),
-		cpu_to_le32(0x0000aaaa),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0xf0005000),
-		cpu_to_le32(0xf0005000),
-	},
-};
-
 /* 20MHz / 40MHz below / 40Mhz above*/
 static const __le64 iwl_ci_mask[][3] = {
 	/* dummy entry for channel 0 */
@@ -596,14 +444,6 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 		goto send_cmd;
 	}
 
-	bt_cmd->max_kill = cpu_to_le32(5);
-	bt_cmd->bt4_antenna_isolation_thr =
-		cpu_to_le32(IWL_MVM_BT_COEX_ANTENNA_COUPLING_THRS);
-	bt_cmd->bt4_tx_tx_delta_freq_thr = cpu_to_le32(15);
-	bt_cmd->bt4_tx_rx_max_freq0 = cpu_to_le32(15);
-	bt_cmd->override_primary_lut = cpu_to_le32(BT_COEX_INVALID_LUT);
-	bt_cmd->override_secondary_lut = cpu_to_le32(BT_COEX_INVALID_LUT);
-
 	mode = iwlwifi_mod_params.bt_coex_active ? BT_COEX_NW : BT_COEX_DISABLE;
 	bt_cmd->mode = cpu_to_le32(mode);
 
@@ -611,7 +451,7 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 		bt_cmd->enabled_modules |=
 			cpu_to_le32(BT_COEX_SYNC2SCO_ENABLED);
 
-	if (IWL_MVM_BT_COEX_CORUNNING)
+	if (iwl_mvm_bt_is_plcr_supported(mvm))
 		bt_cmd->enabled_modules |= cpu_to_le32(BT_COEX_CORUN_ENABLED);
 
 	if (IWL_MVM_BT_COEX_MPLUT) {
@@ -622,18 +462,6 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 
 	bt_cmd->enabled_modules |= cpu_to_le32(BT_COEX_HIGH_BAND_RET);
 
-	if (mvm->cfg->bt_shared_single_ant)
-		memcpy(&bt_cmd->decision_lut, iwl_single_shared_ant,
-		       sizeof(iwl_single_shared_ant));
-	else
-		memcpy(&bt_cmd->decision_lut, iwl_combined_lookup,
-		       sizeof(iwl_combined_lookup));
-
-	memcpy(&bt_cmd->mplut_prio_boost, iwl_bt_prio_boost,
-	       sizeof(iwl_bt_prio_boost));
-	bt_cmd->multiprio_lut[0] = cpu_to_le32(IWL_MVM_BT_COEX_MPLUT_REG0);
-	bt_cmd->multiprio_lut[1] = cpu_to_le32(IWL_MVM_BT_COEX_MPLUT_REG1);
-
 send_cmd:
 	memset(&mvm->last_bt_notif, 0, sizeof(mvm->last_bt_notif));
 	memset(&mvm->last_bt_ci_cmd, 0, sizeof(mvm->last_bt_ci_cmd));
@@ -644,48 +472,6 @@ send_cmd:
 	return ret;
 }
 
-static int iwl_mvm_bt_udpate_sw_boost(struct iwl_mvm *mvm)
-{
-	struct iwl_bt_coex_profile_notif *notif = &mvm->last_bt_notif;
-	u32 primary_lut = le32_to_cpu(notif->primary_ch_lut);
-	u32 secondary_lut = le32_to_cpu(notif->secondary_ch_lut);
-	u32 ag = le32_to_cpu(notif->bt_activity_grading);
-	struct iwl_bt_coex_sw_boost_update_cmd cmd = {};
-	u8 ack_kill_msk[NUM_PHY_CTX] = {};
-	u8 cts_kill_msk[NUM_PHY_CTX] = {};
-	int i;
-
-	lockdep_assert_held(&mvm->mutex);
-
-	ack_kill_msk[0] = iwl_bt_ack_kill_msk[ag][primary_lut];
-	cts_kill_msk[0] = iwl_bt_cts_kill_msk[ag][primary_lut];
-
-	ack_kill_msk[1] = iwl_bt_ack_kill_msk[ag][secondary_lut];
-	cts_kill_msk[1] = iwl_bt_cts_kill_msk[ag][secondary_lut];
-
-	/* Don't send HCMD if there is no update */
-	if (!memcmp(ack_kill_msk, mvm->bt_ack_kill_msk, sizeof(ack_kill_msk)) ||
-	    !memcmp(cts_kill_msk, mvm->bt_cts_kill_msk, sizeof(cts_kill_msk)))
-		return 0;
-
-	memcpy(mvm->bt_ack_kill_msk, ack_kill_msk,
-	       sizeof(mvm->bt_ack_kill_msk));
-	memcpy(mvm->bt_cts_kill_msk, cts_kill_msk,
-	       sizeof(mvm->bt_cts_kill_msk));
-
-	BUILD_BUG_ON(ARRAY_SIZE(ack_kill_msk) < ARRAY_SIZE(cmd.boost_values));
-
-	for (i = 0; i < ARRAY_SIZE(cmd.boost_values); i++) {
-		cmd.boost_values[i].kill_ack_msk =
-			cpu_to_le32(iwl_bt_ctl_kill_msk[ack_kill_msk[i]]);
-		cmd.boost_values[i].kill_cts_msk =
-			cpu_to_le32(iwl_bt_ctl_kill_msk[cts_kill_msk[i]]);
-	}
-
-	return iwl_mvm_send_cmd_pdu(mvm, BT_COEX_UPDATE_SW_BOOST, 0,
-				    sizeof(cmd), &cmd);
-}
-
 static int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id,
 				       bool enable)
 {
@@ -793,7 +579,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;
 
@@ -950,9 +737,6 @@ static void iwl_mvm_bt_coex_notif_handle(struct iwl_mvm *mvm)
 			IWL_ERR(mvm, "Failed to send BT_CI cmd\n");
 		memcpy(&mvm->last_bt_ci_cmd, &cmd, sizeof(cmd));
 	}
-
-	if (iwl_mvm_bt_udpate_sw_boost(mvm))
-		IWL_ERR(mvm, "Failed to update the ctrl_kill_msk\n");
 }
 
 int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
@@ -1023,7 +807,7 @@ static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
 }
 
 void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			   enum ieee80211_rssi_event rssi_event)
+			   enum ieee80211_rssi_event_data rssi_event)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_bt_iterator_data data = {
@@ -1073,9 +857,6 @@ void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	ieee80211_iterate_active_interfaces_atomic(
 		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
 		iwl_mvm_bt_rssi_iterator, &data);
-
-	if (iwl_mvm_bt_udpate_sw_boost(mvm))
-		IWL_ERR(mvm, "Failed to update the ctrl_kill_msk\n");
 }
 
 #define LINK_QUAL_AGG_TIME_LIMIT_DEF	(4000)
@@ -1234,7 +1015,7 @@ int iwl_mvm_rx_ant_coupling_notif(struct iwl_mvm *mvm,
 	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_rx_ant_coupling_notif_old(mvm, rxb, dev_cmd);
 
-	if (!IWL_MVM_BT_COEX_CORUNNING)
+	if (!iwl_mvm_bt_is_plcr_supported(mvm))
 		return 0;
 
 	lockdep_assert_held(&mvm->mutex);
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
index d530ef3da107..d954591e0be5 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
@@ -288,6 +288,65 @@ static const __le64 iwl_ci_mask[][3] = {
 	},
 };
 
+enum iwl_bt_kill_msk {
+	BT_KILL_MSK_DEFAULT,
+	BT_KILL_MSK_NEVER,
+	BT_KILL_MSK_ALWAYS,
+	BT_KILL_MSK_MAX,
+};
+
+static const u32 iwl_bt_ctl_kill_msk[BT_KILL_MSK_MAX] = {
+	[BT_KILL_MSK_DEFAULT] = 0xfffffc00,
+	[BT_KILL_MSK_NEVER] = 0xffffffff,
+	[BT_KILL_MSK_ALWAYS] = 0,
+};
+
+static const u8 iwl_bt_cts_kill_msk[BT_MAX_AG][BT_COEX_MAX_LUT] = {
+	{
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+	},
+	{
+		BT_KILL_MSK_NEVER,
+		BT_KILL_MSK_NEVER,
+		BT_KILL_MSK_NEVER,
+	},
+	{
+		BT_KILL_MSK_NEVER,
+		BT_KILL_MSK_NEVER,
+		BT_KILL_MSK_NEVER,
+	},
+	{
+		BT_KILL_MSK_DEFAULT,
+		BT_KILL_MSK_NEVER,
+		BT_KILL_MSK_DEFAULT,
+	},
+};
+
+static const u8 iwl_bt_ack_kill_msk[BT_MAX_AG][BT_COEX_MAX_LUT] = {
+	{
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+	},
+	{
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+	},
+	{
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_ALWAYS,
+	},
+	{
+		BT_KILL_MSK_DEFAULT,
+		BT_KILL_MSK_ALWAYS,
+		BT_KILL_MSK_DEFAULT,
+	},
+};
+
 struct corunning_block_luts {
 	u8 range;
 	__le32 lut20[BT_COEX_CORUN_LUT_SIZE];
@@ -619,7 +678,7 @@ int iwl_send_bt_init_conf_old(struct iwl_mvm *mvm)
 	if (IWL_MVM_BT_COEX_SYNC2SCO)
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_SYNC2SCO);
 
-	if (IWL_MVM_BT_COEX_CORUNNING) {
+	if (iwl_mvm_bt_is_plcr_supported(mvm)) {
 		bt_cmd->valid_bit_msk |= cpu_to_le32(BT_VALID_CORUN_LUT_20 |
 						     BT_VALID_CORUN_LUT_40);
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_CORUNNING);
@@ -633,7 +692,7 @@ int iwl_send_bt_init_conf_old(struct iwl_mvm *mvm)
 	if (IWL_MVM_BT_COEX_TTC)
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_TTC);
 
-	if (IWL_MVM_BT_COEX_RRC)
+	if (iwl_mvm_bt_is_rrc_supported(mvm))
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_RRC);
 
 	if (mvm->cfg->bt_shared_single_ant)
@@ -832,7 +891,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,
@@ -1068,7 +1128,7 @@ static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
 }
 
 void iwl_mvm_bt_rssi_event_old(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			       enum ieee80211_rssi_event rssi_event)
+			       enum ieee80211_rssi_event_data rssi_event)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_bt_iterator_data data = {
@@ -1167,16 +1227,10 @@ bool iwl_mvm_bt_coex_is_mimo_allowed_old(struct iwl_mvm *mvm,
 	return lut_type != BT_COEX_LOOSE_LUT;
 }
 
-bool iwl_mvm_bt_coex_is_ant_avail_old(struct iwl_mvm *mvm, u8 ant)
-{
-	u32 ag = le32_to_cpu(mvm->last_bt_notif_old.bt_activity_grading);
-	return ag < BT_HIGH_TRAFFIC;
-}
-
 bool iwl_mvm_bt_coex_is_shared_ant_avail_old(struct iwl_mvm *mvm)
 {
 	u32 ag = le32_to_cpu(mvm->last_bt_notif_old.bt_activity_grading);
-	return ag == BT_OFF;
+	return ag < BT_HIGH_TRAFFIC;
 }
 
 bool iwl_mvm_bt_coex_is_tpc_allowed_old(struct iwl_mvm *mvm,
@@ -1213,7 +1267,7 @@ int iwl_mvm_rx_ant_coupling_notif_old(struct iwl_mvm *mvm,
 		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
 	};
 
-	if (!IWL_MVM_BT_COEX_CORUNNING)
+	if (!iwl_mvm_bt_is_plcr_supported(mvm))
 		return 0;
 
 	lockdep_assert_held(&mvm->mutex);
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index 14e8fd661889..a6c48c7b1e16 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -694,6 +694,9 @@ static int iwl_mvm_d3_reprogram(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	if (ret)
 		IWL_ERR(mvm, "Failed to send quota: %d\n", ret);
 
+	if (iwl_mvm_is_lar_supported(mvm) && iwl_mvm_init_fw_regd(mvm))
+		IWL_ERR(mvm, "Failed to initialize D3 LAR information\n");
+
 	return 0;
 }
 
@@ -1128,6 +1131,7 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 	iwl_trans_d3_suspend(mvm->trans, test);
  out:
 	if (ret < 0) {
+		iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
 		ieee80211_restart_hw(mvm->hw);
 		iwl_mvm_free_nd(mvm);
 	}
@@ -1596,7 +1600,7 @@ iwl_mvm_get_wakeup_status(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 	/* RF-kill already asserted again... */
 	if (!cmd.resp_pkt) {
-		ret = -ERFKILL;
+		fw_status = ERR_PTR(-ERFKILL);
 		goto out_free_resp;
 	}
 
@@ -1605,7 +1609,7 @@ iwl_mvm_get_wakeup_status(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 	len = iwl_rx_packet_payload_len(cmd.resp_pkt);
 	if (len < status_size) {
 		IWL_ERR(mvm, "Invalid WoWLAN status response!\n");
-		ret = -EIO;
+		fw_status = ERR_PTR(-EIO);
 		goto out_free_resp;
 	}
 
@@ -1613,7 +1617,7 @@ iwl_mvm_get_wakeup_status(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 	if (len != (status_size +
 		    ALIGN(le32_to_cpu(status->wake_packet_bufsize), 4))) {
 		IWL_ERR(mvm, "Invalid WoWLAN status response!\n");
-		ret = -EIO;
+		fw_status = ERR_PTR(-EIO);
 		goto out_free_resp;
 	}
 
@@ -1621,7 +1625,7 @@ iwl_mvm_get_wakeup_status(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 out_free_resp:
 	iwl_free_resp(&cmd);
-	return ret ? ERR_PTR(ret) : fw_status;
+	return fw_status;
 }
 
 /* releases the MVM mutex */
@@ -1722,6 +1726,10 @@ iwl_mvm_netdetect_query_results(struct iwl_mvm *mvm,
 	results->matched_profiles = le32_to_cpu(query->matched_profiles);
 	memcpy(results->matches, query->matches, sizeof(results->matches));
 
+#ifdef CPTCFG_IWLWIFI_DEBUGFS
+	mvm->last_netdetect_scans = le32_to_cpu(query->n_scans_done);
+#endif
+
 out_free_resp:
 	iwl_free_resp(&cmd);
 	return ret;
@@ -1874,27 +1882,36 @@ static int __iwl_mvm_resume(struct iwl_mvm *mvm, bool test)
 	/* query SRAM first in case we want event logging */
 	iwl_mvm_read_d3_sram(mvm);
 
+	/*
+	 * Query the current location and source from the D3 firmware so we
+	 * can play it back when we re-intiailize the D0 firmware
+	 */
+	iwl_mvm_update_changed_regdom(mvm);
+
 	if (mvm->net_detect) {
 		iwl_mvm_query_netdetect_reasons(mvm, vif);
+		/* has unlocked the mutex, so skip that */
+		goto out;
 	} else {
 		keep = iwl_mvm_query_wakeup_reasons(mvm, vif);
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 		if (keep)
 			mvm->keep_vif = vif;
 #endif
+		/* has unlocked the mutex, so skip that */
+		goto out_iterate;
 	}
-	/* has unlocked the mutex, so skip that */
-	goto out;
 
  out_unlock:
 	mutex_unlock(&mvm->mutex);
 
- out:
+out_iterate:
 	if (!test)
 		ieee80211_iterate_active_interfaces_rtnl(mvm->hw,
 			IEEE80211_IFACE_ITER_NORMAL,
 			iwl_mvm_d3_disconnect_iter, keep ? vif : NULL);
 
+out:
 	/* return 1 to reconfigure the device */
 	set_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status);
 	set_bit(IWL_MVM_STATUS_D3_RECONFIG, &mvm->status);
@@ -2004,6 +2021,7 @@ static int iwl_mvm_d3_test_release(struct inode *inode, struct file *file)
 	__iwl_mvm_resume(mvm, true);
 	rtnl_unlock();
 	iwl_abort_notification_waits(&mvm->notif_wait);
+	iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
 	ieee80211_restart_hw(mvm->hw);
 
 	/* wait for restart and disconnect all interfaces */
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
index 5fe14591e1c4..5f37eab5008d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
@@ -545,6 +545,57 @@ static ssize_t iwl_dbgfs_uapsd_misbehaving_write(struct ieee80211_vif *vif,
 	return ret ? count : -EINVAL;
 }
 
+static ssize_t iwl_dbgfs_rx_phyinfo_write(struct ieee80211_vif *vif, char *buf,
+					  size_t count, loff_t *ppos)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mvm *mvm = mvmvif->mvm;
+	struct ieee80211_chanctx_conf *chanctx_conf;
+	struct iwl_mvm_phy_ctxt *phy_ctxt;
+	u16 value;
+	int ret;
+
+	ret = kstrtou16(buf, 0, &value);
+	if (ret)
+		return ret;
+
+	mutex_lock(&mvm->mutex);
+	rcu_read_lock();
+
+	chanctx_conf = rcu_dereference(vif->chanctx_conf);
+	/* make sure the channel context is assigned */
+	if (!chanctx_conf) {
+		rcu_read_unlock();
+		mutex_unlock(&mvm->mutex);
+		return -EINVAL;
+	}
+
+	phy_ctxt = &mvm->phy_ctxts[*(u16 *)chanctx_conf->drv_priv];
+	rcu_read_unlock();
+
+	mvm->dbgfs_rx_phyinfo = value;
+
+	ret = iwl_mvm_phy_ctxt_changed(mvm, phy_ctxt, &chanctx_conf->min_def,
+				       chanctx_conf->rx_chains_static,
+				       chanctx_conf->rx_chains_dynamic);
+	mutex_unlock(&mvm->mutex);
+
+	return ret ?: count;
+}
+
+static ssize_t iwl_dbgfs_rx_phyinfo_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct ieee80211_vif *vif = file->private_data;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	char buf[8];
+
+	snprintf(buf, sizeof(buf), "0x%04x\n", mvmvif->mvm->dbgfs_rx_phyinfo);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, sizeof(buf));
+}
+
 #define MVM_DEBUGFS_WRITE_FILE_OPS(name, bufsz) \
 	_MVM_DEBUGFS_WRITE_FILE_OPS(name, bufsz, struct ieee80211_vif)
 #define MVM_DEBUGFS_READ_WRITE_FILE_OPS(name, bufsz) \
@@ -560,6 +611,7 @@ MVM_DEBUGFS_READ_WRITE_FILE_OPS(pm_params, 32);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(bf_params, 256);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(low_latency, 10);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(uapsd_misbehaving, 20);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(rx_phyinfo, 10);
 
 void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
@@ -575,7 +627,6 @@ void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 		return;
 
 	mvmvif->dbgfs_dir = debugfs_create_dir("iwlmvm", dbgfs_dir);
-	mvmvif->mvm = mvm;
 
 	if (!mvmvif->dbgfs_dir) {
 		IWL_ERR(mvm, "Failed to create debugfs directory under %s\n",
@@ -595,6 +646,8 @@ void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 				 S_IRUSR | S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE_VIF(uapsd_misbehaving, mvmvif->dbgfs_dir,
 				 S_IRUSR | S_IWUSR);
+	MVM_DEBUGFS_ADD_FILE_VIF(rx_phyinfo, mvmvif->dbgfs_dir,
+				 S_IRUSR | S_IWUSR);
 
 	if (vif->type == NL80211_IFTYPE_STATION && !vif->p2p &&
 	    mvmvif == mvm->bf_allowed_vif)
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
index 82c09d86af8c..9ac04c1ea706 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
@@ -562,11 +562,12 @@ static ssize_t iwl_dbgfs_bt_cmd_read(struct file *file, char __user *user_buf,
 			       "\tSecondary Channel Bitmap 0x%016llx\n",
 			       le64_to_cpu(cmd->bt_secondary_ci));
 
-		pos += scnprintf(buf+pos, bufsz-pos, "BT Configuration CMD\n");
-		pos += scnprintf(buf+pos, bufsz-pos, "\tACK Kill Mask 0x%08x\n",
-				 iwl_bt_ctl_kill_msk[mvm->bt_ack_kill_msk[0]]);
-		pos += scnprintf(buf+pos, bufsz-pos, "\tCTS Kill Mask 0x%08x\n",
-				 iwl_bt_ctl_kill_msk[mvm->bt_cts_kill_msk[0]]);
+		pos += scnprintf(buf+pos, bufsz-pos,
+				 "BT Configuration CMD - 0=default, 1=never, 2=always\n");
+		pos += scnprintf(buf+pos, bufsz-pos, "\tACK Kill msk idx %d\n",
+				 mvm->bt_ack_kill_msk[0]);
+		pos += scnprintf(buf+pos, bufsz-pos, "\tCTS Kill msk idx %d\n",
+				 mvm->bt_cts_kill_msk[0]);
 
 	} else {
 		struct iwl_bt_coex_ci_cmd *cmd = &mvm->last_bt_ci_cmd;
@@ -579,21 +580,6 @@ static ssize_t iwl_dbgfs_bt_cmd_read(struct file *file, char __user *user_buf,
 		pos += scnprintf(buf+pos, bufsz-pos,
 			       "\tSecondary Channel Bitmap 0x%016llx\n",
 			       le64_to_cpu(cmd->bt_secondary_ci));
-
-		pos += scnprintf(buf+pos, bufsz-pos, "BT Configuration CMD\n");
-		pos += scnprintf(buf+pos, bufsz-pos,
-				 "\tPrimary: ACK Kill Mask 0x%08x\n",
-				 iwl_bt_ctl_kill_msk[mvm->bt_ack_kill_msk[0]]);
-		pos += scnprintf(buf+pos, bufsz-pos,
-				 "\tPrimary: CTS Kill Mask 0x%08x\n",
-				 iwl_bt_ctl_kill_msk[mvm->bt_cts_kill_msk[0]]);
-		pos += scnprintf(buf+pos, bufsz-pos,
-				 "\tSecondary: ACK Kill Mask 0x%08x\n",
-				 iwl_bt_ctl_kill_msk[mvm->bt_ack_kill_msk[1]]);
-		pos += scnprintf(buf+pos, bufsz-pos,
-				 "\tSecondary: CTS Kill Mask 0x%08x\n",
-				 iwl_bt_ctl_kill_msk[mvm->bt_cts_kill_msk[1]]);
-
 	}
 
 	mutex_unlock(&mvm->mutex);
@@ -942,7 +928,7 @@ static ssize_t iwl_dbgfs_fw_dbg_conf_read(struct file *file,
 					  size_t count, loff_t *ppos)
 {
 	struct iwl_mvm *mvm = file->private_data;
-	enum iwl_fw_dbg_conf conf;
+	int conf;
 	char buf[8];
 	const size_t bufsz = sizeof(buf);
 	int pos = 0;
@@ -966,7 +952,7 @@ static ssize_t iwl_dbgfs_fw_dbg_conf_write(struct iwl_mvm *mvm,
 	if (ret)
 		return ret;
 
-	if (WARN_ON(conf_id >= FW_DBG_MAX))
+	if (WARN_ON(conf_id >= FW_DBG_CONF_MAX))
 		return -EINVAL;
 
 	mutex_lock(&mvm->mutex);
@@ -985,7 +971,7 @@ static ssize_t iwl_dbgfs_fw_dbg_collect_write(struct iwl_mvm *mvm,
 	if (ret)
 		return ret;
 
-	iwl_mvm_fw_dbg_collect(mvm);
+	iwl_mvm_fw_dbg_collect(mvm, FW_DBG_TRIGGER_USER, NULL, 0, 0);
 
 	iwl_mvm_unref(mvm, IWL_MVM_REF_PRPH_WRITE);
 
@@ -1487,26 +1473,6 @@ out:
 	return count;
 }
 
-static ssize_t iwl_dbgfs_enable_scan_iteration_notif_write(struct iwl_mvm *mvm,
-							   char *buf,
-							   size_t count,
-							   loff_t *ppos)
-{
-	int val;
-
-	mutex_lock(&mvm->mutex);
-
-	if (kstrtoint(buf, 10, &val)) {
-		mutex_unlock(&mvm->mutex);
-		return -EINVAL;
-	}
-
-	mvm->scan_iter_notif_enabled = val;
-	mutex_unlock(&mvm->mutex);
-
-	return count;
-}
-
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(prph_reg, 64);
 
 /* Device wide debugfs entries */
@@ -1529,7 +1495,6 @@ MVM_DEBUGFS_READ_WRITE_FILE_OPS(scan_ant_rxchain, 8);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(d0i3_refs, 8);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(fw_dbg_conf, 8);
 MVM_DEBUGFS_WRITE_FILE_OPS(fw_dbg_collect, 8);
-MVM_DEBUGFS_WRITE_FILE_OPS(enable_scan_iteration_notif, 8);
 
 #ifdef CONFIG_IWLWIFI_BCAST_FILTERING
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(bcast_filters, 256);
@@ -1573,8 +1538,11 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 	MVM_DEBUGFS_ADD_FILE(d0i3_refs, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(fw_dbg_conf, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(fw_dbg_collect, mvm->debugfs_dir, S_IWUSR);
-	MVM_DEBUGFS_ADD_FILE(enable_scan_iteration_notif, mvm->debugfs_dir,
-			     S_IWUSR);
+	if (!debugfs_create_bool("enable_scan_iteration_notif",
+				 S_IRUSR | S_IWUSR,
+				 mvm->debugfs_dir,
+				 &mvm->scan_iter_notif_enabled))
+		goto err;
 
 #ifdef CONFIG_IWLWIFI_BCAST_FILTERING
 	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_BCAST_FILTERING) {
@@ -1601,6 +1569,9 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 	if (!debugfs_create_bool("d3_wake_sysassert", S_IRUSR | S_IWUSR,
 				 mvm->debugfs_dir, &mvm->d3_wake_sysassert))
 		goto err;
+	if (!debugfs_create_u32("last_netdetect_scans", S_IRUSR,
+				mvm->debugfs_dir, &mvm->last_netdetect_scans))
+		goto err;
 	MVM_DEBUGFS_ADD_FILE(netdetect, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
 #endif
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h
index f3b11897991e..d398a6102805 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h
@@ -235,36 +235,12 @@ enum iwl_bt_coex_enabled_modules {
  * struct iwl_bt_coex_cmd - bt coex configuration command
  * @mode: enum %iwl_bt_coex_mode
  * @enabled_modules: enum %iwl_bt_coex_enabled_modules
- * @max_kill: max count of Tx retries due to kill from PTA
- * @override_primary_lut: enum %iwl_bt_coex_lut_type: BT_COEX_INVALID_LUT
- *	should be set by default
- * @override_secondary_lut: enum %iwl_bt_coex_lut_type: BT_COEX_INVALID_LUT
- *	should be set by default
- * @bt4_antenna_isolation_thr: antenna threshold value
- * @bt4_tx_tx_delta_freq_thr: TxTx delta frequency
- * @bt4_tx_rx_max_freq0: TxRx max frequency
- * @multiprio_lut: multi priority LUT configuration
- * @mplut_prio_boost: BT priority boost registers
- * @decision_lut: PTA decision LUT, per Prio-Ch
  *
  * The structure is used for the BT_COEX command.
  */
 struct iwl_bt_coex_cmd {
 	__le32 mode;
 	__le32 enabled_modules;
-
-	__le32 max_kill;
-	__le32 override_primary_lut;
-	__le32 override_secondary_lut;
-	__le32 bt4_antenna_isolation_thr;
-
-	__le32 bt4_tx_tx_delta_freq_thr;
-	__le32 bt4_tx_rx_max_freq0;
-
-	__le32 multiprio_lut[BT_COEX_MULTI_PRIO_LUT_SIZE];
-	__le32 mplut_prio_boost[BT_COEX_BOOST_SIZE];
-
-	__le32 decision_lut[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE];
 } __packed; /* BT_COEX_CMD_API_S_VER_6 */
 
 /**
@@ -280,29 +256,6 @@ struct iwl_bt_coex_corun_lut_update_cmd {
 } __packed; /* BT_COEX_UPDATE_CORUN_LUT_API_S_VER_1 */
 
 /**
- * struct iwl_bt_coex_sw_boost - SW boost values
- * @wifi_tx_prio_boost: SW boost of wifi tx priority
- * @wifi_rx_prio_boost: SW boost of wifi rx priority
- * @kill_ack_msk: kill ACK mask. 1 - Tx ACK, 0 - kill Tx of ACK.
- * @kill_cts_msk: kill CTS mask. 1 - Tx CTS, 0 - kill Tx of CTS.
- */
-struct iwl_bt_coex_sw_boost {
-	__le32 wifi_tx_prio_boost;
-	__le32 wifi_rx_prio_boost;
-	__le32 kill_ack_msk;
-	__le32 kill_cts_msk;
-};
-
-/**
- * struct iwl_bt_coex_sw_boost_update_cmd - command to update the SW boost
- * @boost_values: check struct  %iwl_bt_coex_sw_boost - one for each channel
- *	primary / secondary / low priority
- */
-struct iwl_bt_coex_sw_boost_update_cmd {
-	struct iwl_bt_coex_sw_boost boost_values[3];
-} __packed; /* BT_COEX_UPDATE_SW_BOOST_S_VER_1 */
-
-/**
  * struct iwl_bt_coex_reduced_txp_update_cmd
  * @reduced_txp: bit BT_REDUCED_TX_POWER_BIT to enable / disable, rest of the
  *	bits are the sta_id (value)
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
index 6d3bea5c59d1..d7658d16e965 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
@@ -132,7 +132,7 @@ struct iwl_proto_offload_cmd_common {
  * @solicited_node_ipv6_addr: broken -- solicited node address exists
  *	for each target address
  * @target_ipv6_addr: our target addresses
- * @ndp_mac_addr: neighbor soliciation response MAC address
+ * @ndp_mac_addr: neighbor solicitation response MAC address
  */
 struct iwl_proto_offload_cmd_v1 {
 	struct iwl_proto_offload_cmd_common common;
@@ -150,7 +150,7 @@ struct iwl_proto_offload_cmd_v1 {
  * @solicited_node_ipv6_addr: broken -- solicited node address exists
  *	for each target address
  * @target_ipv6_addr: our target addresses
- * @ndp_mac_addr: neighbor soliciation response MAC address
+ * @ndp_mac_addr: neighbor solicitation response MAC address
  */
 struct iwl_proto_offload_cmd_v2 {
 	struct iwl_proto_offload_cmd_common common;
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
index c405cda1025f..f3f3ee0a766b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
@@ -70,6 +70,7 @@
 #define MAC_INDEX_AUX		4
 #define MAC_INDEX_MIN_DRIVER	0
 #define NUM_MAC_INDEX_DRIVER	MAC_INDEX_AUX
+#define NUM_MAC_INDEX		(MAC_INDEX_AUX + 1)
 
 enum iwl_ac {
 	AC_BK,
@@ -254,7 +255,7 @@ struct iwl_mac_data_p2p_dev {
 /**
  * enum iwl_mac_filter_flags - MAC context filter flags
  * @MAC_FILTER_IN_PROMISC: accept all data frames
- * @MAC_FILTER_IN_CONTROL_AND_MGMT: pass all mangement and
+ * @MAC_FILTER_IN_CONTROL_AND_MGMT: pass all management and
  *	control frames to the host
  * @MAC_FILTER_ACCEPT_GRP: accept multicast frames
  * @MAC_FILTER_DIS_DECRYPT: don't decrypt unicast frames
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index cfc0e65b34a5..4f81dcf57a73 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -70,55 +70,10 @@
 
 /* Scan Commands, Responses, Notifications */
 
-/* Masks for iwl_scan_channel.type flags */
-#define SCAN_CHANNEL_TYPE_ACTIVE	BIT(0)
-#define SCAN_CHANNEL_NARROW_BAND	BIT(22)
-
 /* Max number of IEs for direct SSID scans in a command */
 #define PROBE_OPTION_MAX		20
 
 /**
- * struct iwl_scan_channel - entry in REPLY_SCAN_CMD channel table
- * @channel: band is selected by iwl_scan_cmd "flags" field
- * @tx_gain: gain for analog radio
- * @dsp_atten: gain for DSP
- * @active_dwell: dwell time for active scan in TU, typically 5-50
- * @passive_dwell: dwell time for passive scan in TU, typically 20-500
- * @type: type is broken down to these bits:
- *	bit 0: 0 = passive, 1 = active
- *	bits 1-20: SSID direct bit map. If any of these bits is set then
- *		the corresponding SSID IE is transmitted in probe request
- *		(bit i adds IE in position i to the probe request)
- *	bit 22: channel width, 0 = regular, 1 = TGj narrow channel
- *
- * @iteration_count:
- * @iteration_interval:
- * This struct is used once for each channel in the scan list.
- * Each channel can independently select:
- * 1)  SSID for directed active scans
- * 2)  Txpower setting (for rate specified within Tx command)
- * 3)  How long to stay on-channel (behavior may be modified by quiet_time,
- *     quiet_plcp_th, good_CRC_th)
- *
- * To avoid uCode errors, make sure the following are true (see comments
- * under struct iwl_scan_cmd about max_out_time and quiet_time):
- * 1)  If using passive_dwell (i.e. passive_dwell != 0):
- *     active_dwell <= passive_dwell (< max_out_time if max_out_time != 0)
- * 2)  quiet_time <= active_dwell
- * 3)  If restricting off-channel time (i.e. max_out_time !=0):
- *     passive_dwell < max_out_time
- *     active_dwell < max_out_time
- */
-struct iwl_scan_channel {
-	__le32 type;
-	__le16 channel;
-	__le16 iteration_count;
-	__le32 iteration_interval;
-	__le16 active_dwell;
-	__le16 passive_dwell;
-} __packed; /* SCAN_CHANNEL_CONTROL_API_S_VER_1 */
-
-/**
  * struct iwl_ssid_ie - directed scan network information element
  *
  * Up to 20 of these may appear in REPLY_SCAN_CMD,
@@ -132,152 +87,6 @@ struct iwl_ssid_ie {
 	u8 ssid[IEEE80211_MAX_SSID_LEN];
 } __packed; /* SCAN_DIRECT_SSID_IE_API_S_VER_1 */
 
-/**
- * iwl_scan_flags - masks for scan command flags
- *@SCAN_FLAGS_PERIODIC_SCAN:
- *@SCAN_FLAGS_P2P_PUBLIC_ACTION_FRAME_TX:
- *@SCAN_FLAGS_DELAYED_SCAN_LOWBAND:
- *@SCAN_FLAGS_DELAYED_SCAN_HIGHBAND:
- *@SCAN_FLAGS_FRAGMENTED_SCAN:
- *@SCAN_FLAGS_PASSIVE2ACTIVE: use active scan on channels that was active
- *	in the past hour, even if they are marked as passive.
- */
-enum iwl_scan_flags {
-	SCAN_FLAGS_PERIODIC_SCAN		= BIT(0),
-	SCAN_FLAGS_P2P_PUBLIC_ACTION_FRAME_TX	= BIT(1),
-	SCAN_FLAGS_DELAYED_SCAN_LOWBAND		= BIT(2),
-	SCAN_FLAGS_DELAYED_SCAN_HIGHBAND	= BIT(3),
-	SCAN_FLAGS_FRAGMENTED_SCAN		= BIT(4),
-	SCAN_FLAGS_PASSIVE2ACTIVE		= BIT(5),
-};
-
-/**
- * enum iwl_scan_type - Scan types for scan command
- * @SCAN_TYPE_FORCED:
- * @SCAN_TYPE_BACKGROUND:
- * @SCAN_TYPE_OS:
- * @SCAN_TYPE_ROAMING:
- * @SCAN_TYPE_ACTION:
- * @SCAN_TYPE_DISCOVERY:
- * @SCAN_TYPE_DISCOVERY_FORCED:
- */
-enum iwl_scan_type {
-	SCAN_TYPE_FORCED		= 0,
-	SCAN_TYPE_BACKGROUND		= 1,
-	SCAN_TYPE_OS			= 2,
-	SCAN_TYPE_ROAMING		= 3,
-	SCAN_TYPE_ACTION		= 4,
-	SCAN_TYPE_DISCOVERY		= 5,
-	SCAN_TYPE_DISCOVERY_FORCED	= 6,
-}; /* SCAN_ACTIVITY_TYPE_E_VER_1 */
-
-/**
- * struct iwl_scan_cmd - scan request command
- * ( SCAN_REQUEST_CMD = 0x80 )
- * @len: command length in bytes
- * @scan_flags: scan flags from SCAN_FLAGS_*
- * @channel_count: num of channels in channel list
- *	(1 - ucode_capa.n_scan_channels)
- * @quiet_time: in msecs, dwell this time for active scan on quiet channels
- * @quiet_plcp_th: quiet PLCP threshold (channel is quiet if less than
- *	this number of packets were received (typically 1)
- * @passive2active: is auto switching from passive to active during scan allowed
- * @rxchain_sel_flags: RXON_RX_CHAIN_*
- * @max_out_time: in TUs, max out of serving channel time
- * @suspend_time: how long to pause scan when returning to service channel:
- *	bits 0-19: beacon interal in TUs (suspend before executing)
- *	bits 20-23: reserved
- *	bits 24-31: number of beacons (suspend between channels)
- * @rxon_flags: RXON_FLG_*
- * @filter_flags: RXON_FILTER_*
- * @tx_cmd: for active scans (zero for passive), w/o payload,
- *	no RS so specify TX rate
- * @direct_scan: direct scan SSIDs
- * @type: one of SCAN_TYPE_*
- * @repeats: how many time to repeat the scan
- */
-struct iwl_scan_cmd {
-	__le16 len;
-	u8 scan_flags;
-	u8 channel_count;
-	__le16 quiet_time;
-	__le16 quiet_plcp_th;
-	__le16 passive2active;
-	__le16 rxchain_sel_flags;
-	__le32 max_out_time;
-	__le32 suspend_time;
-	/* RX_ON_FLAGS_API_S_VER_1 */
-	__le32 rxon_flags;
-	__le32 filter_flags;
-	struct iwl_tx_cmd tx_cmd;
-	struct iwl_ssid_ie direct_scan[PROBE_OPTION_MAX];
-	__le32 type;
-	__le32 repeats;
-
-	/*
-	 * Probe request frame, followed by channel list.
-	 *
-	 * Size of probe request frame is specified by byte count in tx_cmd.
-	 * Channel list follows immediately after probe request frame.
-	 * Number of channels in list is specified by channel_count.
-	 * Each channel in list is of type:
-	 *
-	 * struct iwl_scan_channel channels[0];
-	 *
-	 * NOTE:  Only one band of channels can be scanned per pass.  You
-	 * must not mix 2.4GHz channels and 5.2GHz channels, and you must wait
-	 * for one scan to complete (i.e. receive SCAN_COMPLETE_NOTIFICATION)
-	 * before requesting another scan.
-	 */
-	u8 data[0];
-} __packed; /* SCAN_REQUEST_FIXED_PART_API_S_VER_5 */
-
-/* Response to scan request contains only status with one of these values */
-#define SCAN_RESPONSE_OK	0x1
-#define SCAN_RESPONSE_ERROR	0x2
-
-/*
- * SCAN_ABORT_CMD = 0x81
- * When scan abort is requested, the command has no fields except the common
- * header. The response contains only a status with one of these values.
- */
-#define SCAN_ABORT_POSSIBLE	0x1
-#define SCAN_ABORT_IGNORED	0x2 /* no pending scans */
-
-/* TODO: complete documentation */
-#define  SCAN_OWNER_STATUS 0x1
-#define  MEASURE_OWNER_STATUS 0x2
-
-/**
- * struct iwl_scan_start_notif - notifies start of scan in the device
- * ( SCAN_START_NOTIFICATION = 0x82 )
- * @tsf_low: TSF timer (lower half) in usecs
- * @tsf_high: TSF timer (higher half) in usecs
- * @beacon_timer: structured as follows:
- *	bits 0:19 - beacon interval in usecs
- *	bits 20:23 - reserved (0)
- *	bits 24:31 - number of beacons
- * @channel: which channel is scanned
- * @band: 0 for 5.2 GHz, 1 for 2.4 GHz
- * @status: one of *_OWNER_STATUS
- */
-struct iwl_scan_start_notif {
-	__le32 tsf_low;
-	__le32 tsf_high;
-	__le32 beacon_timer;
-	u8 channel;
-	u8 band;
-	u8 reserved[2];
-	__le32 status;
-} __packed; /* SCAN_START_NTF_API_S_VER_1 */
-
-/* scan results probe_status first bit indicates success */
-#define SCAN_PROBE_STATUS_OK		0
-#define SCAN_PROBE_STATUS_TX_FAILED	BIT(0)
-/* error statuses combined with TX_FAILED */
-#define SCAN_PROBE_STATUS_FAIL_TTL	BIT(1)
-#define SCAN_PROBE_STATUS_FAIL_BT	BIT(2)
-
 /* How many statistics are gathered for each channel */
 #define SCAN_RESULTS_STATISTICS 1
 
@@ -294,7 +103,7 @@ struct iwl_scan_start_notif {
  * @SCAN_COMP_STATUS_ERR_COEX: medium was lost ot WiMax
  * @SCAN_COMP_STATUS_P2P_ACTION_OK: P2P public action frame TX was successful
  *	(not an error!)
- * @SCAN_COMP_STATUS_ITERATION_END: indicates end of one repeatition the driver
+ * @SCAN_COMP_STATUS_ITERATION_END: indicates end of one repetition the driver
  *	asked for
  * @SCAN_COMP_STATUS_ERR_ALLOC_TE: scan could not allocate time events
 */
@@ -378,11 +187,11 @@ enum scan_framework_client {
  * struct iwl_scan_offload_cmd - SCAN_REQUEST_FIXED_PART_API_S_VER_6
  * @scan_flags:		see enum iwl_scan_flags
  * @channel_count:	channels in channel list
- * @quiet_time:		dwell time, in milisiconds, on quiet channel
+ * @quiet_time:		dwell time, in milliseconds, on quiet channel
  * @quiet_plcp_th:	quiet channel num of packets threshold
  * @good_CRC_th:	passive to active promotion threshold
  * @rx_chain:		RXON rx chain.
- * @max_out_time:	max TUs to be out of assoceated channel
+ * @max_out_time:	max TUs to be out of associated channel
  * @suspend_time:	pause scan this TUs when returning to service channel
  * @flags:		RXON flags
  * @filter_flags:	RXONfilter
@@ -423,7 +232,7 @@ enum iwl_scan_offload_channel_flags {
  *	see enum iwl_scan_offload_channel_flags.
  * __le16 channel_number: channel number 1-13 etc.
  * __le16 iter_count: repetition count for the channel.
- * __le32 iter_interval: interval between two innteration on one channel.
+ * __le32 iter_interval: interval between two iterations on one channel.
  * u8 active_dwell.
  * u8 passive_dwell.
  */
@@ -466,8 +275,8 @@ enum iwl_scan_offload_band_selection {
 /**
  * iwl_scan_offload_profile - SCAN_OFFLOAD_PROFILE_S
  * @ssid_index:		index to ssid list in fixed part
- * @unicast_cipher:	encryption olgorithm to match - bitmap
- * @aut_alg:		authentication olgorithm to match - bitmap
+ * @unicast_cipher:	encryption algorithm to match - bitmap
+ * @aut_alg:		authentication algorithm to match - bitmap
  * @network_type:	enum iwl_scan_offload_network_type
  * @band_selection:	enum iwl_scan_offload_band_selection
  * @client_bitmap:	clients waiting for match - enum scan_framework_client
@@ -939,7 +748,7 @@ enum iwl_umac_scan_general_flags {
  * @flags:		bitmap - 0-19:	directed scan to i'th ssid.
  * @channel_num:	channel number 1-13 etc.
  * @iter_count:		repetition count for the channel.
- * @iter_interval:	interval between two scan interations on one channel.
+ * @iter_interval:	interval between two scan iterations on one channel.
  */
 struct iwl_scan_channel_cfg_umac {
 	__le32 flags;
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-stats.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-stats.h
index 928168b18346..709e28d8b1b0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-stats.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-stats.h
@@ -65,6 +65,7 @@
 
 #ifndef __fw_api_stats_h__
 #define __fw_api_stats_h__
+#include "fw-api-mac.h"
 
 struct mvm_statistics_dbg {
 	__le32 burst_check;
@@ -218,7 +219,7 @@ struct mvm_statistics_bt_activity {
 	__le32 lo_priority_rx_denied_cnt;
 } __packed;  /* STATISTICS_BT_ACTIVITY_API_S_VER_1 */
 
-struct mvm_statistics_general {
+struct mvm_statistics_general_v5 {
 	__le32 radio_temperature;
 	__le32 radio_voltage;
 	struct mvm_statistics_dbg dbg;
@@ -244,6 +245,39 @@ struct mvm_statistics_general {
 	struct mvm_statistics_bt_activity bt_activity;
 } __packed; /* STATISTICS_GENERAL_API_S_VER_5 */
 
+struct mvm_statistics_general_v8 {
+	__le32 radio_temperature;
+	__le32 radio_voltage;
+	struct mvm_statistics_dbg dbg;
+	__le32 sleep_time;
+	__le32 slots_out;
+	__le32 slots_idle;
+	__le32 ttl_timestamp;
+	struct mvm_statistics_div slow_div;
+	__le32 rx_enable_counter;
+	/*
+	 * num_of_sos_states:
+	 *  count the number of times we have to re-tune
+	 *  in order to get out of bad PHY status
+	 */
+	__le32 num_of_sos_states;
+	__le32 beacon_filtered;
+	__le32 missed_beacons;
+	__s8 beacon_filter_average_energy;
+	__s8 beacon_filter_reason;
+	__s8 beacon_filter_current_energy;
+	__s8 beacon_filter_reserved;
+	__le32 beacon_filter_delta_time;
+	struct mvm_statistics_bt_activity bt_activity;
+	__le64 rx_time;
+	__le64 on_time_rf;
+	__le64 on_time_scan;
+	__le64 tx_time;
+	__le32 beacon_counter[NUM_MAC_INDEX];
+	u8 beacon_average_energy[NUM_MAC_INDEX];
+	u8 reserved[4 - (NUM_MAC_INDEX % 4)];
+} __packed; /* STATISTICS_GENERAL_API_S_VER_8 */
+
 struct mvm_statistics_rx {
 	struct mvm_statistics_rx_phy ofdm;
 	struct mvm_statistics_rx_phy cck;
@@ -256,22 +290,28 @@ struct mvm_statistics_rx {
  *
  * By default, uCode issues this notification after receiving a beacon
  * while associated.  To disable this behavior, set DISABLE_NOTIF flag in the
- * REPLY_STATISTICS_CMD 0x9c, above.
- *
- * Statistics counters continue to increment beacon after beacon, but are
- * cleared when changing channels or when driver issues REPLY_STATISTICS_CMD
- * 0x9c with CLEAR_STATS bit set (see above).
- *
- * uCode also issues this notification during scans.  uCode clears statistics
- * appropriately so that each notification contains statistics for only the
- * one channel that has just been scanned.
+ * STATISTICS_CMD (0x9c), below.
  */
 
-struct iwl_notif_statistics {
+struct iwl_notif_statistics_v8 {
 	__le32 flag;
 	struct mvm_statistics_rx rx;
 	struct mvm_statistics_tx tx;
-	struct mvm_statistics_general general;
+	struct mvm_statistics_general_v5 general;
 } __packed; /* STATISTICS_NTFY_API_S_VER_8 */
 
+struct iwl_notif_statistics_v10 {
+	__le32 flag;
+	struct mvm_statistics_rx rx;
+	struct mvm_statistics_tx tx;
+	struct mvm_statistics_general_v8 general;
+} __packed; /* STATISTICS_NTFY_API_S_VER_10 */
+
+#define IWL_STATISTICS_FLG_CLEAR		0x1
+#define IWL_STATISTICS_FLG_DISABLE_NOTIF	0x2
+
+struct iwl_statistics_cmd {
+	__le32 flags;
+} __packed; /* STATISTICS_CMD_API_S_VER_1 */
+
 #endif /* __fw_api_stats_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
index b56154fe8ec5..aab68cbae754 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
@@ -192,6 +192,7 @@ enum {
 	BEACON_NOTIFICATION = 0x90,
 	BEACON_TEMPLATE_CMD = 0x91,
 	TX_ANT_CONFIGURATION_CMD = 0x98,
+	STATISTICS_CMD = 0x9c,
 	STATISTICS_NOTIFICATION = 0x9d,
 	EOSP_NOTIFICATION = 0x9e,
 	REDUCE_TX_POWER_CMD = 0x9f,
@@ -211,6 +212,10 @@ enum {
 	REPLY_RX_MPDU_CMD = 0xc1,
 	BA_NOTIF = 0xc5,
 
+	/* Location Aware Regulatory */
+	MCC_UPDATE_CMD = 0xc8,
+	MCC_CHUB_UPDATE_CMD = 0xc9,
+
 	MARKER_CMD = 0xcb,
 
 	/* BT Coex */
@@ -361,7 +366,8 @@ enum {
 	NVM_SECTION_TYPE_CALIBRATION = 4,
 	NVM_SECTION_TYPE_PRODUCTION = 5,
 	NVM_SECTION_TYPE_MAC_OVERRIDE = 11,
-	NVM_MAX_NUM_SECTIONS = 12,
+	NVM_SECTION_TYPE_PHY_SKU = 12,
+	NVM_MAX_NUM_SECTIONS = 13,
 };
 
 /**
@@ -431,7 +437,7 @@ enum {
 
 #define IWL_ALIVE_FLG_RFKILL	BIT(0)
 
-struct mvm_alive_resp {
+struct mvm_alive_resp_ver1 {
 	__le16 status;
 	__le16 flags;
 	u8 ucode_minor;
@@ -482,6 +488,30 @@ struct mvm_alive_resp_ver2 {
 	__le32 dbg_print_buff_addr;
 } __packed; /* ALIVE_RES_API_S_VER_2 */
 
+struct mvm_alive_resp {
+	__le16 status;
+	__le16 flags;
+	__le32 ucode_minor;
+	__le32 ucode_major;
+	u8 ver_subtype;
+	u8 ver_type;
+	u8 mac;
+	u8 opt;
+	__le32 timestamp;
+	__le32 error_event_table_ptr;	/* SRAM address for error log */
+	__le32 log_event_table_ptr;	/* SRAM address for LMAC event log */
+	__le32 cpu_register_ptr;
+	__le32 dbgm_config_ptr;
+	__le32 alive_counter_ptr;
+	__le32 scd_base_ptr;		/* SRAM address for SCD */
+	__le32 st_fwrd_addr;		/* pointer to Store and forward */
+	__le32 st_fwrd_size;
+	__le32 umac_minor;		/* UMAC version: minor */
+	__le32 umac_major;		/* UMAC version: major */
+	__le32 error_info_addr;		/* SRAM address for UMAC error log */
+	__le32 dbg_print_buff_addr;
+} __packed; /* ALIVE_RES_API_S_VER_3 */
+
 /* Error response/notification */
 enum {
 	FW_ERR_UNKNOWN_CMD = 0x0,
@@ -1417,7 +1447,19 @@ enum iwl_sf_scenario {
 #define SF_W_MARK_LEGACY 4096
 #define SF_W_MARK_SCAN 4096
 
-/* SF Scenarios timers for FULL_ON state (aligned to 32 uSec) */
+/* SF Scenarios timers for default configuration (aligned to 32 uSec) */
+#define SF_SINGLE_UNICAST_IDLE_TIMER_DEF 160	/* 150 uSec  */
+#define SF_SINGLE_UNICAST_AGING_TIMER_DEF 400	/* 0.4 mSec */
+#define SF_AGG_UNICAST_IDLE_TIMER_DEF 160		/* 150 uSec */
+#define SF_AGG_UNICAST_AGING_TIMER_DEF 400		/* 0.4 mSec */
+#define SF_MCAST_IDLE_TIMER_DEF 160		/* 150 mSec */
+#define SF_MCAST_AGING_TIMER_DEF 400		/* 0.4 mSec */
+#define SF_BA_IDLE_TIMER_DEF 160			/* 150 uSec */
+#define SF_BA_AGING_TIMER_DEF 400			/* 0.4 mSec */
+#define SF_TX_RE_IDLE_TIMER_DEF 160			/* 150 uSec */
+#define SF_TX_RE_AGING_TIMER_DEF 400		/* 0.4 mSec */
+
+/* SF Scenarios timers for BSS MAC configuration (aligned to 32 uSec) */
 #define SF_SINGLE_UNICAST_IDLE_TIMER 320	/* 300 uSec  */
 #define SF_SINGLE_UNICAST_AGING_TIMER 2016	/* 2 mSec */
 #define SF_AGG_UNICAST_IDLE_TIMER 320		/* 300 uSec */
@@ -1448,6 +1490,92 @@ struct iwl_sf_cfg_cmd {
 	__le32 full_on_timeouts[SF_NUM_SCENARIO][SF_NUM_TIMEOUT_TYPES];
 } __packed; /* SF_CFG_API_S_VER_2 */
 
+/***********************************
+ * Location Aware Regulatory (LAR) API - MCC updates
+ ***********************************/
+
+/**
+ * struct iwl_mcc_update_cmd - Request the device to update geographic
+ * regulatory profile according to the given MCC (Mobile Country Code).
+ * The MCC is two letter-code, ascii upper case[A-Z] or '00' for world domain.
+ * 'ZZ' MCC will be used to switch to NVM default profile; in this case, the
+ * MCC in the cmd response will be the relevant MCC in the NVM.
+ * @mcc: given mobile country code
+ * @source_id: the source from where we got the MCC, see iwl_mcc_source
+ * @reserved: reserved for alignment
+ */
+struct iwl_mcc_update_cmd {
+	__le16 mcc;
+	u8 source_id;
+	u8 reserved;
+} __packed; /* LAR_UPDATE_MCC_CMD_API_S */
+
+/**
+ * iwl_mcc_update_resp - response to MCC_UPDATE_CMD.
+ * Contains the new channel control profile map, if changed, and the new MCC
+ * (mobile country code).
+ * The new MCC may be different than what was requested in MCC_UPDATE_CMD.
+ * @status: see &enum iwl_mcc_update_status
+ * @mcc: the new applied MCC
+ * @cap: capabilities for all channels which matches the MCC
+ * @source_id: the MCC source, see iwl_mcc_source
+ * @n_channels: number of channels in @channels_data (may be 14, 39, 50 or 51
+ *		channels, depending on platform)
+ * @channels: channel control data map, DWORD for each channel. Only the first
+ *	16bits are used.
+ */
+struct iwl_mcc_update_resp {
+	__le32 status;
+	__le16 mcc;
+	u8 cap;
+	u8 source_id;
+	__le32 n_channels;
+	__le32 channels[0];
+} __packed; /* LAR_UPDATE_MCC_CMD_RESP_S */
+
+/**
+ * struct iwl_mcc_chub_notif - chub notifies of mcc change
+ * (MCC_CHUB_UPDATE_CMD = 0xc9)
+ * The Chub (Communication Hub, CommsHUB) is a HW component that connects to
+ * the cellular and connectivity cores that gets updates of the mcc, and
+ * notifies the ucode directly of any mcc change.
+ * The ucode requests the driver to request the device to update geographic
+ * regulatory  profile according to the given MCC (Mobile Country Code).
+ * The MCC is two letter-code, ascii upper case[A-Z] or '00' for world domain.
+ * 'ZZ' MCC will be used to switch to NVM default profile; in this case, the
+ * MCC in the cmd response will be the relevant MCC in the NVM.
+ * @mcc: given mobile country code
+ * @source_id: identity of the change originator, see iwl_mcc_source
+ * @reserved1: reserved for alignment
+ */
+struct iwl_mcc_chub_notif {
+	u16 mcc;
+	u8 source_id;
+	u8 reserved1;
+} __packed; /* LAR_MCC_NOTIFY_S */
+
+enum iwl_mcc_update_status {
+	MCC_RESP_NEW_CHAN_PROFILE,
+	MCC_RESP_SAME_CHAN_PROFILE,
+	MCC_RESP_INVALID,
+	MCC_RESP_NVM_DISABLED,
+	MCC_RESP_ILLEGAL,
+	MCC_RESP_LOW_PRIORITY,
+};
+
+enum iwl_mcc_source {
+	MCC_SOURCE_OLD_FW = 0,
+	MCC_SOURCE_ME = 1,
+	MCC_SOURCE_BIOS = 2,
+	MCC_SOURCE_3G_LTE_HOST = 3,
+	MCC_SOURCE_3G_LTE_DEVICE = 4,
+	MCC_SOURCE_WIFI = 5,
+	MCC_SOURCE_RESERVED = 6,
+	MCC_SOURCE_DEFAULT = 7,
+	MCC_SOURCE_UNINITIALIZED = 8,
+	MCC_SOURCE_GET_CURRENT = 0x10
+};
+
 /* DTS measurements */
 
 enum iwl_dts_measurement_flags {
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/iwlwifi/mvm/fw.c
index ca38e9817374..bc5eac4960e1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/iwlwifi/mvm/fw.c
@@ -112,25 +112,27 @@ static bool iwl_alive_fn(struct iwl_notif_wait_data *notif_wait,
 	struct iwl_mvm *mvm =
 		container_of(notif_wait, struct iwl_mvm, notif_wait);
 	struct iwl_mvm_alive_data *alive_data = data;
-	struct mvm_alive_resp *palive;
+	struct mvm_alive_resp_ver1 *palive1;
 	struct mvm_alive_resp_ver2 *palive2;
+	struct mvm_alive_resp *palive;
 
-	if (iwl_rx_packet_payload_len(pkt) == sizeof(*palive)) {
-		palive = (void *)pkt->data;
+	if (iwl_rx_packet_payload_len(pkt) == sizeof(*palive1)) {
+		palive1 = (void *)pkt->data;
 
 		mvm->support_umac_log = false;
 		mvm->error_event_table =
-			le32_to_cpu(palive->error_event_table_ptr);
-		mvm->log_event_table = le32_to_cpu(palive->log_event_table_ptr);
-		alive_data->scd_base_addr = le32_to_cpu(palive->scd_base_ptr);
+			le32_to_cpu(palive1->error_event_table_ptr);
+		mvm->log_event_table =
+			le32_to_cpu(palive1->log_event_table_ptr);
+		alive_data->scd_base_addr = le32_to_cpu(palive1->scd_base_ptr);
 
-		alive_data->valid = le16_to_cpu(palive->status) ==
+		alive_data->valid = le16_to_cpu(palive1->status) ==
 				    IWL_ALIVE_STATUS_OK;
 		IWL_DEBUG_FW(mvm,
 			     "Alive VER1 ucode status 0x%04x revision 0x%01X 0x%01X flags 0x%01X\n",
-			     le16_to_cpu(palive->status), palive->ver_type,
-			     palive->ver_subtype, palive->flags);
-	} else {
+			     le16_to_cpu(palive1->status), palive1->ver_type,
+			     palive1->ver_subtype, palive1->flags);
+	} else if (iwl_rx_packet_payload_len(pkt) == sizeof(*palive2)) {
 		palive2 = (void *)pkt->data;
 
 		mvm->error_event_table =
@@ -156,6 +158,33 @@ static bool iwl_alive_fn(struct iwl_notif_wait_data *notif_wait,
 		IWL_DEBUG_FW(mvm,
 			     "UMAC version: Major - 0x%x, Minor - 0x%x\n",
 			     palive2->umac_major, palive2->umac_minor);
+	} else if (iwl_rx_packet_payload_len(pkt) == sizeof(*palive)) {
+		palive = (void *)pkt->data;
+
+		mvm->error_event_table =
+			le32_to_cpu(palive->error_event_table_ptr);
+		mvm->log_event_table =
+			le32_to_cpu(palive->log_event_table_ptr);
+		alive_data->scd_base_addr = le32_to_cpu(palive->scd_base_ptr);
+		mvm->umac_error_event_table =
+			le32_to_cpu(palive->error_info_addr);
+		mvm->sf_space.addr = le32_to_cpu(palive->st_fwrd_addr);
+		mvm->sf_space.size = le32_to_cpu(palive->st_fwrd_size);
+
+		alive_data->valid = le16_to_cpu(palive->status) ==
+				    IWL_ALIVE_STATUS_OK;
+		if (mvm->umac_error_event_table)
+			mvm->support_umac_log = true;
+
+		IWL_DEBUG_FW(mvm,
+			     "Alive VER3 ucode status 0x%04x revision 0x%01X 0x%01X flags 0x%01X\n",
+			     le16_to_cpu(palive->status), palive->ver_type,
+			     palive->ver_subtype, palive->flags);
+
+		IWL_DEBUG_FW(mvm,
+			     "UMAC version: Major - 0x%x, Minor - 0x%x\n",
+			     le32_to_cpu(palive->umac_major),
+			     le32_to_cpu(palive->umac_minor));
 	}
 
 	return true;
@@ -188,8 +217,7 @@ static int iwl_mvm_load_ucode_wait_alive(struct iwl_mvm *mvm,
 	struct iwl_sf_region st_fwrd_space;
 
 	if (ucode_type == IWL_UCODE_REGULAR &&
-	    iwl_fw_dbg_conf_usniffer(mvm->fw, FW_DBG_CUSTOM) &&
-	    iwl_fw_dbg_conf_enabled(mvm->fw, FW_DBG_CUSTOM))
+	    iwl_fw_dbg_conf_usniffer(mvm->fw, FW_DBG_START_FROM_ALIVE))
 		fw = iwl_get_ucode_image(mvm, IWL_UCODE_REGULAR_USNIFFER);
 	else
 		fw = iwl_get_ucode_image(mvm, ucode_type);
@@ -451,20 +479,97 @@ exit:
 	iwl_free_resp(&cmd);
 }
 
-void iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm)
+int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
+				struct iwl_mvm_dump_desc *desc,
+				unsigned int delay)
 {
+	if (test_and_set_bit(IWL_MVM_STATUS_DUMPING_FW_LOG, &mvm->status))
+		return -EBUSY;
+
+	if (WARN_ON(mvm->fw_dump_desc))
+		iwl_mvm_free_fw_dump_desc(mvm);
+
+	IWL_WARN(mvm, "Collecting data: trigger %d fired.\n",
+		 le32_to_cpu(desc->trig_desc.type));
+
+	mvm->fw_dump_desc = desc;
+
 	/* stop recording */
 	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
 		iwl_set_bits_prph(mvm->trans, MON_BUFF_SAMPLE_CTL, 0x100);
 	} else {
 		iwl_write_prph(mvm->trans, DBGC_IN_SAMPLE, 0);
-		iwl_write_prph(mvm->trans, DBGC_OUT_CTRL, 0);
+		/* wait before we collect the data till the DBGC stop */
+		udelay(100);
+	}
+
+	queue_delayed_work(system_wq, &mvm->fw_dump_wk, delay);
+
+	return 0;
+}
+
+int iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm, enum iwl_fw_dbg_trigger trig,
+			   const char *str, size_t len, unsigned int delay)
+{
+	struct iwl_mvm_dump_desc *desc;
+
+	desc = kzalloc(sizeof(*desc) + len, GFP_ATOMIC);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->len = len;
+	desc->trig_desc.type = cpu_to_le32(trig);
+	memcpy(desc->trig_desc.data, str, len);
+
+	return iwl_mvm_fw_dbg_collect_desc(mvm, desc, delay);
+}
+
+int iwl_mvm_fw_dbg_collect_trig(struct iwl_mvm *mvm,
+				struct iwl_fw_dbg_trigger_tlv *trigger,
+				const char *fmt, ...)
+{
+	unsigned int delay = msecs_to_jiffies(le32_to_cpu(trigger->stop_delay));
+	u16 occurrences = le16_to_cpu(trigger->occurrences);
+	int ret, len = 0;
+	char buf[64];
+
+	if (!occurrences)
+		return 0;
+
+	if (fmt) {
+		va_list ap;
+
+		buf[sizeof(buf) - 1] = '\0';
+
+		va_start(ap, fmt);
+		vsnprintf(buf, sizeof(buf), fmt, ap);
+		va_end(ap);
+
+		/* check for truncation */
+		if (WARN_ON_ONCE(buf[sizeof(buf) - 1]))
+			buf[sizeof(buf) - 1] = '\0';
+
+		len = strlen(buf) + 1;
 	}
 
-	schedule_work(&mvm->fw_error_dump_wk);
+	ret = iwl_mvm_fw_dbg_collect(mvm, le32_to_cpu(trigger->id), buf,
+				     len, delay);
+	if (ret)
+		return ret;
+
+	trigger->occurrences = cpu_to_le16(occurrences - 1);
+	return 0;
 }
 
-int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, enum iwl_fw_dbg_conf conf_id)
+static inline void iwl_mvm_restart_early_start(struct iwl_mvm *mvm)
+{
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000)
+		iwl_clear_bits_prph(mvm->trans, MON_BUFF_SAMPLE_CTL, 0x100);
+	else
+		iwl_write_prph(mvm->trans, DBGC_IN_SAMPLE, 1);
+}
+
+int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, u8 conf_id)
 {
 	u8 *ptr;
 	int ret;
@@ -474,6 +579,14 @@ int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, enum iwl_fw_dbg_conf conf_id)
 		      "Invalid configuration %d\n", conf_id))
 		return -EINVAL;
 
+	/* EARLY START - firmware's configuration is hard coded */
+	if ((!mvm->fw->dbg_conf_tlv[conf_id] ||
+	     !mvm->fw->dbg_conf_tlv[conf_id]->num_of_hcmds) &&
+	    conf_id == FW_DBG_START_FROM_ALIVE) {
+		iwl_mvm_restart_early_start(mvm);
+		return 0;
+	}
+
 	if (!mvm->fw->dbg_conf_tlv[conf_id])
 		return -EINVAL;
 
@@ -583,7 +696,10 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 		IWL_ERR(mvm, "Failed to initialize Smart Fifo\n");
 
 	mvm->fw_dbg_conf = FW_DBG_INVALID;
-	iwl_mvm_start_fw_dbg_conf(mvm, FW_DBG_CUSTOM);
+	/* if we have a destination, assume EARLY START */
+	if (mvm->fw->dbg_dest_tlv)
+		mvm->fw_dbg_conf = FW_DBG_START_FROM_ALIVE;
+	iwl_mvm_start_fw_dbg_conf(mvm, FW_DBG_START_FROM_ALIVE);
 
 	ret = iwl_send_tx_ant_cfg(mvm, iwl_mvm_get_valid_tx_ant(mvm));
 	if (ret)
@@ -640,6 +756,16 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	if (ret)
 		goto error;
 
+	/*
+	 * RTNL is not taken during Ct-kill, but we don't need to scan/Tx
+	 * anyway, so don't init MCC.
+	 */
+	if (!test_bit(IWL_MVM_STATUS_HW_CTKILL, &mvm->status)) {
+		ret = iwl_mvm_init_mcc(mvm);
+		if (ret)
+			goto error;
+	}
+
 	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
 		ret = iwl_mvm_config_scan(mvm);
 		if (ret)
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index 7bdc6220743f..8088c7137f7c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -244,6 +244,7 @@ static void iwl_mvm_mac_sta_hw_queues_iter(void *_data,
 unsigned long iwl_mvm_get_used_hw_queues(struct iwl_mvm *mvm,
 					 struct ieee80211_vif *exclude_vif)
 {
+	u8 sta_id;
 	struct iwl_mvm_hw_queues_iface_iterator_data data = {
 		.exclude_vif = exclude_vif,
 		.used_hw_queues =
@@ -264,6 +265,13 @@ unsigned long iwl_mvm_get_used_hw_queues(struct iwl_mvm *mvm,
 					  iwl_mvm_mac_sta_hw_queues_iter,
 					  &data);
 
+	/*
+	 * Some TDLS stations may be removed but are in the process of being
+	 * drained. Don't touch their queues.
+	 */
+	for_each_set_bit(sta_id, mvm->sta_drained, IWL_MVM_STATION_COUNT)
+		data.used_hw_queues |= mvm->tfd_drained[sta_id];
+
 	return data.used_hw_queues;
 }
 
@@ -462,9 +470,8 @@ exit_fail:
 
 int iwl_mvm_mac_ctxt_init(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
-	unsigned int wdg_timeout = iwlmvm_mod_params.tfd_q_hang_detect ?
-					mvm->cfg->base_params->wd_timeout :
-					IWL_WATCHDOG_DISABLED;
+	unsigned int wdg_timeout =
+		iwl_mvm_get_wd_timeout(mvm, vif, false, false);
 	u32 ac;
 	int ret;
 
@@ -1367,10 +1374,18 @@ static void iwl_mvm_beacon_loss_iterator(void *_data, u8 *mac,
 {
 	struct iwl_missed_beacons_notif *missed_beacons = _data;
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mvm *mvm = mvmvif->mvm;
+	struct iwl_fw_dbg_trigger_missed_bcon *bcon_trig;
+	struct iwl_fw_dbg_trigger_tlv *trigger;
+	u32 stop_trig_missed_bcon, stop_trig_missed_bcon_since_rx;
+	u32 rx_missed_bcon, rx_missed_bcon_since_rx;
 
 	if (mvmvif->id != (u16)le32_to_cpu(missed_beacons->mac_id))
 		return;
 
+	rx_missed_bcon = le32_to_cpu(missed_beacons->consec_missed_beacons);
+	rx_missed_bcon_since_rx =
+		le32_to_cpu(missed_beacons->consec_missed_beacons_since_last_rx);
 	/*
 	 * TODO: the threshold should be adjusted based on latency conditions,
 	 * and/or in case of a CS flow on one of the other AP vifs.
@@ -1378,6 +1393,26 @@ static void iwl_mvm_beacon_loss_iterator(void *_data, u8 *mac,
 	if (le32_to_cpu(missed_beacons->consec_missed_beacons_since_last_rx) >
 	     IWL_MVM_MISSED_BEACONS_THRESHOLD)
 		ieee80211_beacon_loss(vif);
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw,
+					FW_DBG_TRIGGER_MISSED_BEACONS))
+		return;
+
+	trigger = iwl_fw_dbg_get_trigger(mvm->fw,
+					 FW_DBG_TRIGGER_MISSED_BEACONS);
+	bcon_trig = (void *)trigger->data;
+	stop_trig_missed_bcon = le32_to_cpu(bcon_trig->stop_consec_missed_bcon);
+	stop_trig_missed_bcon_since_rx =
+		le32_to_cpu(bcon_trig->stop_consec_missed_bcon_since_rx);
+
+	/* TODO: implement start trigger */
+
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trigger))
+		return;
+
+	if (rx_missed_bcon_since_rx >= stop_trig_missed_bcon_since_rx ||
+	    rx_missed_bcon >= stop_trig_missed_bcon)
+		iwl_mvm_fw_dbg_collect_trig(mvm, trigger, NULL);
 }
 
 int iwl_mvm_rx_missed_beacons_notif(struct iwl_mvm *mvm,
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 1ff7ec08532d..84555170b6f7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -86,6 +86,7 @@
 #include "iwl-fw-error-dump.h"
 #include "iwl-prph.h"
 #include "iwl-csr.h"
+#include "iwl-nvm-parse.h"
 
 static const struct ieee80211_iface_limit iwl_mvm_limits[] = {
 	{
@@ -301,6 +302,116 @@ static void iwl_mvm_reset_phy_ctxts(struct iwl_mvm *mvm)
 	}
 }
 
+struct ieee80211_regdomain *iwl_mvm_get_regdomain(struct wiphy *wiphy,
+						  const char *alpha2,
+						  enum iwl_mcc_source src_id,
+						  bool *changed)
+{
+	struct ieee80211_regdomain *regd = NULL;
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	struct iwl_mcc_update_resp *resp;
+
+	IWL_DEBUG_LAR(mvm, "Getting regdomain data for %s from FW\n", alpha2);
+
+	lockdep_assert_held(&mvm->mutex);
+
+	resp = iwl_mvm_update_mcc(mvm, alpha2, src_id);
+	if (IS_ERR_OR_NULL(resp)) {
+		IWL_DEBUG_LAR(mvm, "Could not get update from FW %d\n",
+			      PTR_RET(resp));
+		goto out;
+	}
+
+	if (changed)
+		*changed = (resp->status == MCC_RESP_NEW_CHAN_PROFILE);
+
+	regd = iwl_parse_nvm_mcc_info(mvm->trans->dev, mvm->cfg,
+				      __le32_to_cpu(resp->n_channels),
+				      resp->channels,
+				      __le16_to_cpu(resp->mcc));
+	/* Store the return source id */
+	src_id = resp->source_id;
+	kfree(resp);
+	if (IS_ERR_OR_NULL(regd)) {
+		IWL_DEBUG_LAR(mvm, "Could not get parse update from FW %d\n",
+			      PTR_RET(regd));
+		goto out;
+	}
+
+	IWL_DEBUG_LAR(mvm, "setting alpha2 from FW to %s (0x%x, 0x%x) src=%d\n",
+		      regd->alpha2, regd->alpha2[0], regd->alpha2[1], src_id);
+	mvm->lar_regdom_set = true;
+	mvm->mcc_src = src_id;
+
+out:
+	return regd;
+}
+
+void iwl_mvm_update_changed_regdom(struct iwl_mvm *mvm)
+{
+	bool changed;
+	struct ieee80211_regdomain *regd;
+
+	if (!iwl_mvm_is_lar_supported(mvm))
+		return;
+
+	regd = iwl_mvm_get_current_regdomain(mvm, &changed);
+	if (!IS_ERR_OR_NULL(regd)) {
+		/* only update the regulatory core if changed */
+		if (changed)
+			regulatory_set_wiphy_regd(mvm->hw->wiphy, regd);
+
+		kfree(regd);
+	}
+}
+
+struct ieee80211_regdomain *iwl_mvm_get_current_regdomain(struct iwl_mvm *mvm,
+							  bool *changed)
+{
+	return iwl_mvm_get_regdomain(mvm->hw->wiphy, "ZZ",
+				     iwl_mvm_is_wifi_mcc_supported(mvm) ?
+				     MCC_SOURCE_GET_CURRENT :
+				     MCC_SOURCE_OLD_FW, changed);
+}
+
+int iwl_mvm_init_fw_regd(struct iwl_mvm *mvm)
+{
+	enum iwl_mcc_source used_src;
+	struct ieee80211_regdomain *regd;
+	int ret;
+	bool changed;
+	const struct ieee80211_regdomain *r =
+			rtnl_dereference(mvm->hw->wiphy->regd);
+
+	if (!r)
+		return -ENOENT;
+
+	/* save the last source in case we overwrite it below */
+	used_src = mvm->mcc_src;
+	if (iwl_mvm_is_wifi_mcc_supported(mvm)) {
+		/* Notify the firmware we support wifi location updates */
+		regd = iwl_mvm_get_current_regdomain(mvm, NULL);
+		if (!IS_ERR_OR_NULL(regd))
+			kfree(regd);
+	}
+
+	/* Now set our last stored MCC and source */
+	regd = iwl_mvm_get_regdomain(mvm->hw->wiphy, r->alpha2, used_src,
+				     &changed);
+	if (IS_ERR_OR_NULL(regd))
+		return -EIO;
+
+	/* update cfg80211 if the regdomain was changed */
+	if (changed)
+		ret = regulatory_set_wiphy_regd_sync_rtnl(mvm->hw->wiphy, regd);
+	else
+		ret = 0;
+
+	kfree(regd);
+	return ret;
+}
+
 int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 {
 	struct ieee80211_hw *hw = mvm->hw;
@@ -339,13 +450,10 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	    !iwlwifi_mod_params.sw_crypto)
 		hw->flags |= IEEE80211_HW_MFP_CAPABLE;
 
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN ||
-	    mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
-		hw->flags |= IEEE80211_SINGLE_HW_SCAN_ON_ALL_BANDS;
-		hw->wiphy->features |=
-			NL80211_FEATURE_SCHED_SCAN_RANDOM_MAC_ADDR |
-			NL80211_FEATURE_SCAN_RANDOM_MAC_ADDR;
-	}
+	hw->flags |= IEEE80211_SINGLE_HW_SCAN_ON_ALL_BANDS;
+	hw->wiphy->features |=
+		NL80211_FEATURE_SCHED_SCAN_RANDOM_MAC_ADDR |
+		NL80211_FEATURE_SCAN_RANDOM_MAC_ADDR;
 
 	hw->sta_data_size = sizeof(struct iwl_mvm_sta);
 	hw->vif_data_size = sizeof(struct iwl_mvm_vif);
@@ -359,8 +467,12 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 		BIT(NL80211_IFTYPE_ADHOC);
 
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
-	hw->wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG |
-				       REGULATORY_DISABLE_BEACON_HINTS;
+	hw->wiphy->regulatory_flags |= REGULATORY_ENABLE_RELAX_NO_IR;
+	if (iwl_mvm_is_lar_supported(mvm))
+		hw->wiphy->regulatory_flags |= REGULATORY_WIPHY_SELF_MANAGED;
+	else
+		hw->wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG |
+					       REGULATORY_DISABLE_BEACON_HINTS;
 
 	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_GO_UAPSD)
 		hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
@@ -405,7 +517,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;
 	}
@@ -889,12 +1004,26 @@ static void iwl_mvm_dump_fifos(struct iwl_mvm *mvm,
 	iwl_trans_release_nic_access(mvm->trans, &flags);
 }
 
+void iwl_mvm_free_fw_dump_desc(struct iwl_mvm *mvm)
+{
+	if (mvm->fw_dump_desc == &iwl_mvm_dump_desc_assert ||
+	    !mvm->fw_dump_desc)
+		return;
+
+	kfree(mvm->fw_dump_desc);
+	mvm->fw_dump_desc = NULL;
+}
+
+#define IWL8260_ICCM_OFFSET		0x44000 /* Only for B-step */
+#define IWL8260_ICCM_LEN		0xC000 /* Only for B-step */
+
 void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 {
 	struct iwl_fw_error_dump_file *dump_file;
 	struct iwl_fw_error_dump_data *dump_data;
 	struct iwl_fw_error_dump_info *dump_info;
 	struct iwl_fw_error_dump_mem *dump_mem;
+	struct iwl_fw_error_dump_trigger_desc *dump_trig;
 	struct iwl_mvm_dump_ptrs *fw_error_dump;
 	u32 sram_len, sram_ofs;
 	u32 file_len, fifo_data_len = 0;
@@ -903,16 +1032,6 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 
 	lockdep_assert_held(&mvm->mutex);
 
-	/* W/A for 8000 HW family A-step */
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
-	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_A_STEP) {
-		if (smem_len)
-			smem_len = 0x38000;
-
-		if (sram2_len)
-			sram2_len = 0x10000;
-	}
-
 	fw_error_dump = kzalloc(sizeof(*fw_error_dump), GFP_KERNEL);
 	if (!fw_error_dump)
 		return;
@@ -964,6 +1083,18 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 		   fifo_data_len +
 		   sizeof(*dump_info);
 
+	/*
+	 * In 8000 HW family B-step include the ICCM (which resides separately)
+	 */
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
+	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP)
+		file_len += sizeof(*dump_data) + sizeof(*dump_mem) +
+			    IWL8260_ICCM_LEN;
+
+	if (mvm->fw_dump_desc)
+		file_len += sizeof(*dump_data) + sizeof(*dump_trig) +
+			    mvm->fw_dump_desc->len;
+
 	/* Make room for the SMEM, if it exists */
 	if (smem_len)
 		file_len += sizeof(*dump_data) + sizeof(*dump_mem) + smem_len;
@@ -975,6 +1106,7 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 	dump_file = vzalloc(file_len);
 	if (!dump_file) {
 		kfree(fw_error_dump);
+		iwl_mvm_free_fw_dump_desc(mvm);
 		return;
 	}
 
@@ -1003,6 +1135,19 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 	if (test_bit(STATUS_FW_ERROR, &mvm->trans->status))
 		iwl_mvm_dump_fifos(mvm, &dump_data);
 
+	if (mvm->fw_dump_desc) {
+		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_ERROR_INFO);
+		dump_data->len = cpu_to_le32(sizeof(*dump_trig) +
+					     mvm->fw_dump_desc->len);
+		dump_trig = (void *)dump_data->data;
+		memcpy(dump_trig, &mvm->fw_dump_desc->trig_desc,
+		       sizeof(*dump_trig) + mvm->fw_dump_desc->len);
+
+		/* now we can free this copy */
+		iwl_mvm_free_fw_dump_desc(mvm);
+		dump_data = iwl_fw_error_next_data(dump_data);
+	}
+
 	dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
 	dump_data->len = cpu_to_le32(sram_len + sizeof(*dump_mem));
 	dump_mem = (void *)dump_data->data;
@@ -1033,6 +1178,19 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 					 dump_mem->data, sram2_len);
 	}
 
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
+	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP) {
+		dump_data = iwl_fw_error_next_data(dump_data);
+		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
+		dump_data->len = cpu_to_le32(IWL8260_ICCM_LEN +
+					     sizeof(*dump_mem));
+		dump_mem = (void *)dump_data->data;
+		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
+		dump_mem->offset = cpu_to_le32(IWL8260_ICCM_OFFSET);
+		iwl_trans_read_mem_bytes(mvm->trans, IWL8260_ICCM_OFFSET,
+					 dump_mem->data, IWL8260_ICCM_LEN);
+	}
+
 	fw_error_dump->trans_ptr = iwl_trans_dump_data(mvm->trans);
 	fw_error_dump->op_mode_len = file_len;
 	if (fw_error_dump->trans_ptr)
@@ -1041,16 +1199,26 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 
 	dev_coredumpm(mvm->trans->dev, THIS_MODULE, fw_error_dump, 0,
 		      GFP_KERNEL, iwl_mvm_read_coredump, iwl_mvm_free_coredump);
+
+	clear_bit(IWL_MVM_STATUS_DUMPING_FW_LOG, &mvm->status);
 }
 
+struct iwl_mvm_dump_desc iwl_mvm_dump_desc_assert = {
+	.trig_desc = {
+		.type = cpu_to_le32(FW_DBG_TRIGGER_FW_ASSERT),
+	},
+};
+
 static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 {
 	/* clear the D3 reconfig, we only need it to avoid dumping a
 	 * firmware coredump on reconfiguration, we shouldn't do that
 	 * on D3->D0 transition
 	 */
-	if (!test_and_clear_bit(IWL_MVM_STATUS_D3_RECONFIG, &mvm->status))
+	if (!test_and_clear_bit(IWL_MVM_STATUS_D3_RECONFIG, &mvm->status)) {
+		mvm->fw_dump_desc = &iwl_mvm_dump_desc_assert;
 		iwl_mvm_fw_error_dump(mvm);
+	}
 
 	/* cleanup all stale references (scan, roc), but keep the
 	 * ucode_down ref until reconfig is complete
@@ -1091,6 +1259,10 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 
 	mvm->vif_count = 0;
 	mvm->rx_ba_sessions = 0;
+	mvm->fw_dbg_conf = FW_DBG_INVALID;
+
+	/* keep statistics ticking */
+	iwl_mvm_accu_radio_stats(mvm);
 }
 
 int __iwl_mvm_mac_start(struct iwl_mvm *mvm)
@@ -1150,7 +1322,7 @@ static void iwl_mvm_restart_complete(struct iwl_mvm *mvm)
 
 	clear_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status);
 	iwl_mvm_d0i3_enable_tx(mvm, NULL);
-	ret = iwl_mvm_update_quotas(mvm, NULL);
+	ret = iwl_mvm_update_quotas(mvm, false, NULL);
 	if (ret)
 		IWL_ERR(mvm, "Failed to update quotas after restart (%d)\n",
 			ret);
@@ -1213,6 +1385,11 @@ void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 {
 	lockdep_assert_held(&mvm->mutex);
 
+	/* firmware counters are obviously reset now, but we shouldn't
+	 * partially track so also clear the fw_reset_accu counters.
+	 */
+	memset(&mvm->accu_radio_stats, 0, sizeof(mvm->accu_radio_stats));
+
 	/*
 	 * Disallow low power states when the FW is down by taking
 	 * the UCODE_DOWN ref. in case of ongoing hw restart the
@@ -1243,6 +1420,20 @@ void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 	 */
 	clear_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status);
 
+	/* We shouldn't have any UIDs still set.  Loop over all the UIDs to
+	 * make sure there's nothing left there and warn if any is found.
+	 */
+	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
+		int i;
+
+		for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++) {
+			if (WARN_ONCE(mvm->scan_uid[i],
+				      "UMAC scan UID %d was not cleaned\n",
+				      mvm->scan_uid[i]))
+				mvm->scan_uid[i] = 0;
+		}
+	}
+
 	mvm->ucode_loaded = false;
 }
 
@@ -1252,7 +1443,8 @@ static void iwl_mvm_mac_stop(struct ieee80211_hw *hw)
 
 	flush_work(&mvm->d0i3_exit_work);
 	flush_work(&mvm->async_handlers_wk);
-	flush_work(&mvm->fw_error_dump_wk);
+	cancel_delayed_work_sync(&mvm->fw_dump_wk);
+	iwl_mvm_free_fw_dump_desc(mvm);
 
 	mutex_lock(&mvm->mutex);
 	__iwl_mvm_mac_stop(mvm);
@@ -1300,6 +1492,8 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	int ret;
 
+	mvmvif->mvm = mvm;
+
 	/*
 	 * make sure D0i3 exit is completed, otherwise a target access
 	 * during tx queue configuration could be done when still in
@@ -1317,6 +1511,11 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
+	/* make sure that beacon statistics don't go backwards with FW reset */
+	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
+		mvmvif->beacon_stats.accu_num_beacons +=
+			mvmvif->beacon_stats.num_beacons;
+
 	/* Allocate resources for the MAC context, and add it to the fw  */
 	ret = iwl_mvm_mac_ctxt_init(mvm, vif);
 	if (ret)
@@ -1431,9 +1630,33 @@ static void iwl_mvm_prepare_mac_removal(struct iwl_mvm *mvm,
 	u32 tfd_msk = iwl_mvm_mac_get_queues_mask(vif);
 
 	if (tfd_msk) {
+		/*
+		 * mac80211 first removes all the stations of the vif and
+		 * then removes the vif. When it removes a station it also
+		 * flushes the AMPDU session. So by now, all the AMPDU sessions
+		 * of all the stations of this vif are closed, and the queues
+		 * of these AMPDU sessions are properly closed.
+		 * We still need to take care of the shared queues of the vif.
+		 * Flush them here.
+		 */
 		mutex_lock(&mvm->mutex);
 		iwl_mvm_flush_tx_path(mvm, tfd_msk, true);
 		mutex_unlock(&mvm->mutex);
+
+		/*
+		 * There are transports that buffer a few frames in the host.
+		 * For these, the flush above isn't enough since while we were
+		 * flushing, the transport might have sent more frames to the
+		 * device. To solve this, wait here until the transport is
+		 * empty. Technically, this could have replaced the flush
+		 * above, but flush is much faster than draining. So flush
+		 * first, and drain to make sure we have no frames in the
+		 * transport anymore.
+		 * If a station still had frames on the shared queues, it is
+		 * already marked as draining, so to complete the draining, we
+		 * just need to wait until the transport is empty.
+		 */
+		iwl_trans_wait_tx_queue_empty(mvm->trans, tfd_msk);
 	}
 
 	if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
@@ -1810,8 +2033,13 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 
 	if (changes & BSS_CHANGED_ASSOC) {
 		if (bss_conf->assoc) {
+			/* clear statistics to get clean beacon counter */
+			iwl_mvm_request_statistics(mvm, true);
+			memset(&mvmvif->beacon_stats, 0,
+			       sizeof(mvmvif->beacon_stats));
+
 			/* add quota for this interface */
-			ret = iwl_mvm_update_quotas(mvm, NULL);
+			ret = iwl_mvm_update_quotas(mvm, true, NULL);
 			if (ret) {
 				IWL_ERR(mvm, "failed to update quotas\n");
 				return;
@@ -1863,7 +2091,7 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 				mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
 			mvmvif->ap_sta_id = IWL_MVM_STATION_COUNT;
 			/* remove quota for this interface */
-			ret = iwl_mvm_update_quotas(mvm, NULL);
+			ret = iwl_mvm_update_quotas(mvm, false, NULL);
 			if (ret)
 				IWL_ERR(mvm, "failed to update quotas\n");
 
@@ -1982,7 +2210,7 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 	/* power updated needs to be done before quotas */
 	iwl_mvm_power_update_mac(mvm);
 
-	ret = iwl_mvm_update_quotas(mvm, NULL);
+	ret = iwl_mvm_update_quotas(mvm, false, NULL);
 	if (ret)
 		goto out_quota_failed;
 
@@ -1998,8 +2226,7 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 	if (iwl_mvm_phy_ctx_count(mvm) > 1)
 		iwl_mvm_teardown_tdls_peers(mvm);
 
-	mutex_unlock(&mvm->mutex);
-	return 0;
+	goto out_unlock;
 
 out_quota_failed:
 	iwl_mvm_power_update_mac(mvm);
@@ -2048,7 +2275,7 @@ static void iwl_mvm_stop_ap_ibss(struct ieee80211_hw *hw,
 	if (vif->p2p && mvm->p2p_device_vif)
 		iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false, NULL);
 
-	iwl_mvm_update_quotas(mvm, NULL);
+	iwl_mvm_update_quotas(mvm, false, NULL);
 	iwl_mvm_send_rm_bcast_sta(mvm, vif);
 	iwl_mvm_binding_remove_vif(mvm, vif);
 
@@ -2187,6 +2414,12 @@ static int iwl_mvm_mac_hw_scan(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
+	if (iwl_mvm_is_lar_supported(mvm) && !mvm->lar_regdom_set) {
+		IWL_ERR(mvm, "scan while LAR regdomain is not set\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
 	if (mvm->scan_status != IWL_MVM_SCAN_NONE) {
 		ret = -EBUSY;
 		goto out;
@@ -2196,10 +2429,8 @@ static int iwl_mvm_mac_hw_scan(struct ieee80211_hw *hw,
 
 	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)
 		ret = iwl_mvm_scan_umac(mvm, vif, hw_req);
-	else if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN)
-		ret = iwl_mvm_unified_scan_lmac(mvm, vif, hw_req);
 	else
-		ret = iwl_mvm_scan_request(mvm, vif, req);
+		ret = iwl_mvm_unified_scan_lmac(mvm, vif, hw_req);
 
 	if (ret)
 		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
@@ -2215,7 +2446,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);
 }
@@ -2257,25 +2500,35 @@ static void iwl_mvm_mac_sta_notify(struct ieee80211_hw *hw,
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+	unsigned long txqs = 0, tids = 0;
 	int tid;
 
+	spin_lock_bh(&mvmsta->lock);
+	for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++) {
+		struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
+
+		if (tid_data->state != IWL_AGG_ON &&
+		    tid_data->state != IWL_EMPTYING_HW_QUEUE_DELBA)
+			continue;
+
+		__set_bit(tid_data->txq_id, &txqs);
+
+		if (iwl_mvm_tid_queued(tid_data) == 0)
+			continue;
+
+		__set_bit(tid, &tids);
+	}
+
 	switch (cmd) {
 	case STA_NOTIFY_SLEEP:
 		if (atomic_read(&mvm->pending_frames[mvmsta->sta_id]) > 0)
 			ieee80211_sta_block_awake(hw, sta, true);
-		spin_lock_bh(&mvmsta->lock);
-		for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++) {
-			struct iwl_mvm_tid_data *tid_data;
 
-			tid_data = &mvmsta->tid_data[tid];
-			if (tid_data->state != IWL_AGG_ON &&
-			    tid_data->state != IWL_EMPTYING_HW_QUEUE_DELBA)
-				continue;
-			if (iwl_mvm_tid_queued(tid_data) == 0)
-				continue;
+		for_each_set_bit(tid, &tids, IWL_MAX_TID_COUNT)
 			ieee80211_sta_set_buffered(sta, tid, true);
-		}
-		spin_unlock_bh(&mvmsta->lock);
+
+		if (txqs)
+			iwl_trans_freeze_txq_timer(mvm->trans, txqs, true);
 		/*
 		 * The fw updates the STA to be asleep. Tx packets on the Tx
 		 * queues to this station will not be transmitted. The fw will
@@ -2285,11 +2538,15 @@ static void iwl_mvm_mac_sta_notify(struct ieee80211_hw *hw,
 	case STA_NOTIFY_AWAKE:
 		if (WARN_ON(mvmsta->sta_id == IWL_MVM_STATION_COUNT))
 			break;
+
+		if (txqs)
+			iwl_trans_freeze_txq_timer(mvm->trans, txqs, false);
 		iwl_mvm_sta_modify_ps_wake(mvm, sta);
 		break;
 	default:
 		break;
 	}
+	spin_unlock_bh(&mvmsta->lock);
 }
 
 static void iwl_mvm_sta_pre_rcu_remove(struct ieee80211_hw *hw,
@@ -2527,13 +2784,13 @@ static int iwl_mvm_mac_sched_scan_start(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
-	/* Newest FW fixes sched scan while connected on another interface */
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN) {
-		if (!vif->bss_conf.idle) {
-			ret = -EBUSY;
-			goto out;
-		}
-	} else if (!iwl_mvm_is_idle(mvm)) {
+	if (iwl_mvm_is_lar_supported(mvm) && !mvm->lar_regdom_set) {
+		IWL_ERR(mvm, "sched-scan while LAR regdomain is not set\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (!vif->bss_conf.idle) {
 		ret = -EBUSY;
 		goto out;
 	}
@@ -2559,12 +2816,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,
@@ -2806,6 +3080,8 @@ static int iwl_mvm_roc(struct ieee80211_hw *hw,
 	IWL_DEBUG_MAC80211(mvm, "enter (%d, %d, %d)\n", channel->hw_value,
 			   duration, type);
 
+	flush_work(&mvm->roc_done_wk);
+
 	mutex_lock(&mvm->mutex);
 
 	switch (vif->type) {
@@ -3077,14 +3353,14 @@ static int __iwl_mvm_assign_vif_chanctx(struct iwl_mvm *mvm,
 	 */
 	if (vif->type == NL80211_IFTYPE_MONITOR) {
 		mvmvif->monitor_active = true;
-		ret = iwl_mvm_update_quotas(mvm, NULL);
+		ret = iwl_mvm_update_quotas(mvm, false, NULL);
 		if (ret)
 			goto out_remove_binding;
 	}
 
 	/* Handle binding during CSA */
 	if (vif->type == NL80211_IFTYPE_AP) {
-		iwl_mvm_update_quotas(mvm, NULL);
+		iwl_mvm_update_quotas(mvm, false, NULL);
 		iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
 	}
 
@@ -3108,7 +3384,7 @@ static int __iwl_mvm_assign_vif_chanctx(struct iwl_mvm *mvm,
 
 		iwl_mvm_unref(mvm, IWL_MVM_REF_PROTECT_CSA);
 
-		iwl_mvm_update_quotas(mvm, NULL);
+		iwl_mvm_update_quotas(mvm, false, NULL);
 	}
 
 	goto out;
@@ -3181,7 +3457,7 @@ static void __iwl_mvm_unassign_vif_chanctx(struct iwl_mvm *mvm,
 		break;
 	}
 
-	iwl_mvm_update_quotas(mvm, disabled_vif);
+	iwl_mvm_update_quotas(mvm, false, disabled_vif);
 	iwl_mvm_binding_remove_vif(mvm, vif);
 
 out:
@@ -3373,7 +3649,7 @@ static int __iwl_mvm_mac_testmode_cmd(struct iwl_mvm *mvm,
 		mvm->noa_duration = noa_duration;
 		mvm->noa_vif = vif;
 
-		return iwl_mvm_update_quotas(mvm, NULL);
+		return iwl_mvm_update_quotas(mvm, false, NULL);
 	case IWL_MVM_TM_CMD_SET_BEACON_FILTER:
 		/* must be associated client vif - ignore authorized */
 		if (!vif || vif->type != NL80211_IFTYPE_STATION ||
@@ -3430,9 +3706,13 @@ static int iwl_mvm_pre_channel_switch(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
+	mvmvif->csa_failed = false;
+
 	IWL_DEBUG_MAC80211(mvm, "pre CSA to freq %d\n",
 			   chsw->chandef.center_freq1);
 
+	iwl_fw_dbg_trigger_simple_stop(mvm, vif, FW_DBG_TRIGGER_CHANNEL_SWITCH);
+
 	switch (vif->type) {
 	case NL80211_IFTYPE_AP:
 		csa_vif =
@@ -3502,6 +3782,12 @@ static int iwl_mvm_post_channel_switch(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
+	if (mvmvif->csa_failed) {
+		mvmvif->csa_failed = false;
+		ret = -EIO;
+		goto out_unlock;
+	}
+
 	if (vif->type == NL80211_IFTYPE_STATION) {
 		struct iwl_mvm_sta *mvmsta;
 
@@ -3581,6 +3867,154 @@ static void iwl_mvm_mac_flush(struct ieee80211_hw *hw,
 	}
 }
 
+static int iwl_mvm_mac_get_survey(struct ieee80211_hw *hw, int idx,
+				  struct survey_info *survey)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	int ret;
+
+	memset(survey, 0, sizeof(*survey));
+
+	/* only support global statistics right now */
+	if (idx != 0)
+		return -ENOENT;
+
+	if (!(mvm->fw->ucode_capa.capa[0] &
+			IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS))
+		return -ENOENT;
+
+	mutex_lock(&mvm->mutex);
+
+	if (mvm->ucode_loaded) {
+		ret = iwl_mvm_request_statistics(mvm, false);
+		if (ret)
+			goto out;
+	}
+
+	survey->filled = SURVEY_INFO_TIME |
+			 SURVEY_INFO_TIME_RX |
+			 SURVEY_INFO_TIME_TX |
+			 SURVEY_INFO_TIME_SCAN;
+	survey->time = mvm->accu_radio_stats.on_time_rf +
+		       mvm->radio_stats.on_time_rf;
+	do_div(survey->time, USEC_PER_MSEC);
+
+	survey->time_rx = mvm->accu_radio_stats.rx_time +
+			  mvm->radio_stats.rx_time;
+	do_div(survey->time_rx, USEC_PER_MSEC);
+
+	survey->time_tx = mvm->accu_radio_stats.tx_time +
+			  mvm->radio_stats.tx_time;
+	do_div(survey->time_tx, USEC_PER_MSEC);
+
+	survey->time_scan = mvm->accu_radio_stats.on_time_scan +
+			    mvm->radio_stats.on_time_scan;
+	do_div(survey->time_scan, USEC_PER_MSEC);
+
+	ret = 0;
+ out:
+	mutex_unlock(&mvm->mutex);
+	return ret;
+}
+
+static void iwl_mvm_mac_sta_statistics(struct ieee80211_hw *hw,
+				       struct ieee80211_vif *vif,
+				       struct ieee80211_sta *sta,
+				       struct station_info *sinfo)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+
+	if (!(mvm->fw->ucode_capa.capa[0] &
+				IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS))
+		return;
+
+	/* if beacon filtering isn't on mac80211 does it anyway */
+	if (!(vif->driver_flags & IEEE80211_VIF_BEACON_FILTER))
+		return;
+
+	if (!vif->bss_conf.assoc)
+		return;
+
+	mutex_lock(&mvm->mutex);
+
+	if (mvmvif->ap_sta_id != mvmsta->sta_id)
+		goto unlock;
+
+	if (iwl_mvm_request_statistics(mvm, false))
+		goto unlock;
+
+	sinfo->rx_beacon = mvmvif->beacon_stats.num_beacons +
+			   mvmvif->beacon_stats.accu_num_beacons;
+	sinfo->filled |= BIT(NL80211_STA_INFO_BEACON_RX);
+	if (mvmvif->beacon_stats.avg_signal) {
+		/* firmware only reports a value after RXing a few beacons */
+		sinfo->rx_beacon_signal_avg = mvmvif->beacon_stats.avg_signal;
+		sinfo->filled |= BIT(NL80211_STA_INFO_BEACON_SIGNAL_AVG);
+	}
+ unlock:
+	mutex_unlock(&mvm->mutex);
+}
+
+static void iwl_mvm_mac_event_callback(struct ieee80211_hw *hw,
+				       struct ieee80211_vif *vif,
+				       const struct ieee80211_event *event)
+{
+#define CHECK_MLME_TRIGGER(_mvm, _trig, _buf, _cnt, _fmt...)	\
+	do {							\
+		if ((_cnt) && --(_cnt))				\
+			break;					\
+		iwl_mvm_fw_dbg_collect_trig(_mvm, _trig, _fmt);\
+	} while (0)
+
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_mlme *trig_mlme;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_MLME))
+		return;
+
+	if (event->u.mlme.status == MLME_SUCCESS)
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_MLME);
+	trig_mlme = (void *)trig->data;
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trig))
+		return;
+
+	if (event->u.mlme.data == ASSOC_EVENT) {
+		if (event->u.mlme.status == MLME_DENIED)
+			CHECK_MLME_TRIGGER(mvm, trig, buf,
+					   trig_mlme->stop_assoc_denied,
+					   "DENIED ASSOC: reason %d",
+					    event->u.mlme.reason);
+		else if (event->u.mlme.status == MLME_TIMEOUT)
+			CHECK_MLME_TRIGGER(mvm, trig, buf,
+					   trig_mlme->stop_assoc_timeout,
+					   "ASSOC TIMEOUT");
+	} else if (event->u.mlme.data == AUTH_EVENT) {
+		if (event->u.mlme.status == MLME_DENIED)
+			CHECK_MLME_TRIGGER(mvm, trig, buf,
+					   trig_mlme->stop_auth_denied,
+					   "DENIED AUTH: reason %d",
+					   event->u.mlme.reason);
+		else if (event->u.mlme.status == MLME_TIMEOUT)
+			CHECK_MLME_TRIGGER(mvm, trig, buf,
+					   trig_mlme->stop_auth_timeout,
+					   "AUTH TIMEOUT");
+	} else if (event->u.mlme.data == DEAUTH_RX_EVENT) {
+		CHECK_MLME_TRIGGER(mvm, trig, buf,
+				   trig_mlme->stop_rx_deauth,
+				   "DEAUTH RX %d", event->u.mlme.reason);
+	} else if (event->u.mlme.data == DEAUTH_TX_EVENT) {
+		CHECK_MLME_TRIGGER(mvm, trig, buf,
+				   trig_mlme->stop_tx_deauth,
+				   "DEAUTH TX %d", event->u.mlme.reason);
+	}
+#undef CHECK_MLME_TRIGGER
+}
+
 const struct ieee80211_ops iwl_mvm_hw_ops = {
 	.tx = iwl_mvm_mac_tx,
 	.ampdu_action = iwl_mvm_mac_ampdu_action,
@@ -3634,6 +4068,8 @@ const struct ieee80211_ops iwl_mvm_hw_ops = {
 	.tdls_cancel_channel_switch = iwl_mvm_tdls_cancel_channel_switch,
 	.tdls_recv_channel_switch = iwl_mvm_tdls_recv_channel_switch,
 
+	.event_callback = iwl_mvm_mac_event_callback,
+
 	CFG80211_TESTMODE_CMD(iwl_mvm_mac_testmode_cmd)
 
 #ifdef CONFIG_PM_SLEEP
@@ -3647,4 +4083,6 @@ const struct ieee80211_ops iwl_mvm_hw_ops = {
 #endif
 	.set_default_unicast_key = iwl_mvm_set_default_unicast_key,
 #endif
+	.get_survey = iwl_mvm_mac_get_survey,
+	.sta_statistics = iwl_mvm_mac_sta_statistics,
 };
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index 6c69d0584f6c..d5522a161242 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -75,6 +75,7 @@
 #include "iwl-trans.h"
 #include "iwl-notif-wait.h"
 #include "iwl-eeprom-parse.h"
+#include "iwl-fw-file.h"
 #include "sta.h"
 #include "fw-api.h"
 #include "constants.h"
@@ -145,6 +146,19 @@ struct iwl_mvm_dump_ptrs {
 	u32 op_mode_len;
 };
 
+/**
+ * struct iwl_mvm_dump_desc - describes the dump
+ * @len: length of trig_desc->data
+ * @trig_desc: the description of the dump
+ */
+struct iwl_mvm_dump_desc {
+	size_t len;
+	/* must be last */
+	struct iwl_fw_error_dump_trigger_desc trig_desc;
+};
+
+extern struct iwl_mvm_dump_desc iwl_mvm_dump_desc_assert;
+
 struct iwl_mvm_phy_ctxt {
 	u16 id;
 	u16 color;
@@ -335,10 +349,15 @@ struct iwl_mvm_vif_bf_data {
  * @bcast_sta: station used for broadcast packets. Used by the following
  *  vifs: P2P_DEVICE, GO and AP.
  * @beacon_skb: the skb used to hold the AP/GO beacon template
- * @smps_requests: the SMPS requests of differents parts of the driver,
+ * @smps_requests: the SMPS requests of different parts of the driver,
  *	combined on update to yield the overall request to mac80211.
+ * @beacon_stats: beacon statistics, containing the # of received beacons,
+ *	# of received beacons accumulated over FW restart, and the current
+ *	average signal of beacons retrieved from the firmware
+ * @csa_failed: CSA failed to schedule time event, report an error later
  */
 struct iwl_mvm_vif {
+	struct iwl_mvm *mvm;
 	u16 id;
 	u16 color;
 	u8 ap_sta_id;
@@ -354,6 +373,11 @@ struct iwl_mvm_vif {
 	bool ps_disabled;
 	struct iwl_mvm_vif_bf_data bf_data;
 
+	struct {
+		u32 num_beacons, accu_num_beacons;
+		u8 avg_signal;
+	} beacon_stats;
+
 	u32 ap_beacon_time;
 
 	enum iwl_tsf_id tsf_id;
@@ -396,7 +420,6 @@ struct iwl_mvm_vif {
 #endif
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
-	struct iwl_mvm *mvm;
 	struct dentry *dbgfs_dir;
 	struct dentry *dbgfs_slink;
 	struct iwl_dbgfs_pm dbgfs_pm;
@@ -411,6 +434,7 @@ struct iwl_mvm_vif {
 
 	/* Indicates that CSA countdown may be started */
 	bool csa_countdown;
+	bool csa_failed;
 };
 
 static inline struct iwl_mvm_vif *
@@ -593,6 +617,13 @@ struct iwl_mvm {
 
 	struct mvm_statistics_rx rx_stats;
 
+	struct {
+		u64 rx_time;
+		u64 tx_time;
+		u64 on_time_rf;
+		u64 on_time_scan;
+	} radio_stats, accu_radio_stats;
+
 	u8 queue_to_mac80211[IWL_MAX_HW_QUEUES];
 	atomic_t mac80211_queue_stop_count[IEEE80211_MAX_QUEUES];
 
@@ -657,7 +688,7 @@ struct iwl_mvm {
 	bool disable_power_off;
 	bool disable_power_off_d3;
 
-	bool scan_iter_notif_enabled;
+	u32 scan_iter_notif_enabled; /* must be u32 for debugfs_create_bool */
 
 	struct debugfs_blob_wrapper nvm_hw_blob;
 	struct debugfs_blob_wrapper nvm_sw_blob;
@@ -666,6 +697,7 @@ struct iwl_mvm {
 
 	struct iwl_mvm_frame_stats drv_rx_stats;
 	spinlock_t drv_stats_lock;
+	u16 dbgfs_rx_phyinfo;
 #endif
 
 	struct iwl_mvm_phy_ctxt phy_ctxts[NUM_PHY_CTX];
@@ -687,8 +719,9 @@ struct iwl_mvm {
 
 	/* -1 for always, 0 for never, >0 for that many times */
 	s8 restart_fw;
-	struct work_struct fw_error_dump_wk;
-	enum iwl_fw_dbg_conf fw_dbg_conf;
+	u8 fw_dbg_conf;
+	struct delayed_work fw_dump_wk;
+	struct iwl_mvm_dump_desc *fw_dump_desc;
 
 #ifdef CONFIG_IWLWIFI_LEDS
 	struct led_classdev led;
@@ -715,6 +748,7 @@ struct iwl_mvm {
 	void *d3_resume_sram;
 	u32 d3_test_pme_ptr;
 	struct ieee80211_vif *keep_vif;
+	u32 last_netdetect_scans; /* no. of scans in the last net-detect wake */
 #endif
 #endif
 
@@ -779,6 +813,9 @@ struct iwl_mvm {
 	/* system time of last beacon (for AP/GO interface) */
 	u32 ap_last_beacon_gp2;
 
+	bool lar_regdom_set;
+	enum iwl_mcc_source mcc_src;
+
 	u8 low_latency_agg_frame_limit;
 
 	/* TDLS channel switch data */
@@ -824,6 +861,7 @@ enum iwl_mvm_status {
 	IWL_MVM_STATUS_IN_D0I3,
 	IWL_MVM_STATUS_ROC_AUX_RUNNING,
 	IWL_MVM_STATUS_D3_RECONFIG,
+	IWL_MVM_STATUS_DUMPING_FW_LOG,
 };
 
 static inline bool iwl_mvm_is_radio_killed(struct iwl_mvm *mvm)
@@ -878,11 +916,48 @@ static inline bool iwl_mvm_is_d0i3_supported(struct iwl_mvm *mvm)
 	       (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_D0I3_SUPPORT);
 }
 
+static inline bool iwl_mvm_is_lar_supported(struct iwl_mvm *mvm)
+{
+	bool nvm_lar = mvm->nvm_data->lar_enabled;
+	bool tlv_lar = mvm->fw->ucode_capa.capa[0] &
+		IWL_UCODE_TLV_CAPA_LAR_SUPPORT;
+
+	if (iwlwifi_mod_params.lar_disable)
+		return false;
+
+	/*
+	 * Enable LAR only if it is supported by the FW (TLV) &&
+	 * enabled in the NVM
+	 */
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		return nvm_lar && tlv_lar;
+	else
+		return tlv_lar;
+}
+
+static inline bool iwl_mvm_is_wifi_mcc_supported(struct iwl_mvm *mvm)
+{
+	return mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_WIFI_MCC_UPDATE ||
+	       mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC;
+}
+
 static inline bool iwl_mvm_is_scd_cfg_supported(struct iwl_mvm *mvm)
 {
 	return mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_SCD_CFG;
 }
 
+static inline bool iwl_mvm_bt_is_plcr_supported(struct iwl_mvm *mvm)
+{
+	return (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_BT_COEX_PLCR) &&
+		IWL_MVM_BT_COEX_CORUNNING;
+}
+
+static inline bool iwl_mvm_bt_is_rrc_supported(struct iwl_mvm *mvm)
+{
+	return (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_BT_COEX_RRC) &&
+		IWL_MVM_BT_COEX_RRC;
+}
+
 extern const u8 iwl_mvm_ac_to_tx_fifo[];
 
 struct iwl_rate_info {
@@ -951,12 +1026,13 @@ static inline void iwl_mvm_wait_for_async_handlers(struct iwl_mvm *mvm)
 }
 
 /* Statistics */
-int iwl_mvm_rx_reply_statistics(struct iwl_mvm *mvm,
-				struct iwl_rx_cmd_buffer *rxb,
-				struct iwl_device_cmd *cmd);
+void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
+				  struct iwl_rx_packet *pkt);
 int iwl_mvm_rx_statistics(struct iwl_mvm *mvm,
 			  struct iwl_rx_cmd_buffer *rxb,
 			  struct iwl_device_cmd *cmd);
+int iwl_mvm_request_statistics(struct iwl_mvm *mvm, bool clear);
+void iwl_mvm_accu_radio_stats(struct iwl_mvm *mvm);
 
 /* NVM */
 int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic);
@@ -1067,20 +1143,14 @@ int iwl_mvm_binding_add_vif(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_binding_remove_vif(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 
 /* Quota management */
-int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
+int iwl_mvm_update_quotas(struct iwl_mvm *mvm, bool force_upload,
 			  struct ieee80211_vif *disabled_vif);
 
 /* Scanning */
 int iwl_mvm_scan_size(struct iwl_mvm *mvm);
-int iwl_mvm_scan_request(struct iwl_mvm *mvm,
-			 struct ieee80211_vif *vif,
-			 struct cfg80211_scan_request *req);
-int iwl_mvm_rx_scan_response(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
-			     struct iwl_device_cmd *cmd);
-int iwl_mvm_rx_scan_complete(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
-			     struct iwl_device_cmd *cmd);
 int iwl_mvm_cancel_scan(struct iwl_mvm *mvm);
 int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan);
+void iwl_mvm_report_scan_aborted(struct iwl_mvm *mvm);
 
 /* Scheduled scan */
 int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
@@ -1089,14 +1159,8 @@ int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
 int iwl_mvm_rx_scan_offload_iter_complete_notif(struct iwl_mvm *mvm,
 						struct iwl_rx_cmd_buffer *rxb,
 						struct iwl_device_cmd *cmd);
-int iwl_mvm_config_sched_scan(struct iwl_mvm *mvm,
-			      struct ieee80211_vif *vif,
-			      struct cfg80211_sched_scan_request *req,
-			      struct ieee80211_scan_ies *ies);
 int iwl_mvm_config_sched_scan_profiles(struct iwl_mvm *mvm,
 				       struct cfg80211_sched_scan_request *req);
-int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
-			     struct cfg80211_sched_scan_request *req);
 int iwl_mvm_scan_offload_start(struct iwl_mvm *mvm,
 			       struct ieee80211_vif *vif,
 			       struct cfg80211_sched_scan_request *req,
@@ -1225,7 +1289,7 @@ int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
 			     struct iwl_rx_cmd_buffer *rxb,
 			     struct iwl_device_cmd *cmd);
 void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			   enum ieee80211_rssi_event rssi_event);
+			   enum ieee80211_rssi_event_data);
 void iwl_mvm_bt_coex_vif_change(struct iwl_mvm *mvm);
 u16 iwl_mvm_coex_agg_time_limit(struct iwl_mvm *mvm,
 				struct ieee80211_sta *sta);
@@ -1238,7 +1302,6 @@ bool iwl_mvm_bt_coex_is_tpc_allowed(struct iwl_mvm *mvm,
 u8 iwl_mvm_bt_coex_tx_prio(struct iwl_mvm *mvm, struct ieee80211_hdr *hdr,
 			   struct ieee80211_tx_info *info, u8 ac);
 
-bool iwl_mvm_bt_coex_is_ant_avail_old(struct iwl_mvm *mvm, u8 ant);
 bool iwl_mvm_bt_coex_is_shared_ant_avail_old(struct iwl_mvm *mvm);
 void iwl_mvm_bt_coex_vif_change_old(struct iwl_mvm *mvm);
 int iwl_send_bt_init_conf_old(struct iwl_mvm *mvm);
@@ -1246,7 +1309,7 @@ int iwl_mvm_rx_bt_coex_notif_old(struct iwl_mvm *mvm,
 				 struct iwl_rx_cmd_buffer *rxb,
 				 struct iwl_device_cmd *cmd);
 void iwl_mvm_bt_rssi_event_old(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			       enum ieee80211_rssi_event rssi_event);
+			       enum ieee80211_rssi_event_data);
 u16 iwl_mvm_coex_agg_time_limit_old(struct iwl_mvm *mvm,
 				    struct ieee80211_sta *sta);
 bool iwl_mvm_bt_coex_is_mimo_allowed_old(struct iwl_mvm *mvm,
@@ -1257,17 +1320,6 @@ int iwl_mvm_rx_ant_coupling_notif_old(struct iwl_mvm *mvm,
 				      struct iwl_rx_cmd_buffer *rxb,
 				      struct iwl_device_cmd *cmd);
 
-enum iwl_bt_kill_msk {
-	BT_KILL_MSK_DEFAULT,
-	BT_KILL_MSK_NEVER,
-	BT_KILL_MSK_ALWAYS,
-	BT_KILL_MSK_MAX,
-};
-
-extern const u8 iwl_bt_ack_kill_msk[BT_MAX_AG][BT_COEX_MAX_LUT];
-extern const u8 iwl_bt_cts_kill_msk[BT_MAX_AG][BT_COEX_MAX_LUT];
-extern const u32 iwl_bt_ctl_kill_msk[BT_KILL_MSK_MAX];
-
 /* beacon filtering */
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 void
@@ -1352,9 +1404,6 @@ static inline void iwl_mvm_enable_agg_txq(struct iwl_mvm *mvm, int queue,
 	iwl_mvm_enable_txq(mvm, queue, ssn, &cfg, wdg_timeout);
 }
 
-/* Assoc status */
-bool iwl_mvm_is_idle(struct iwl_mvm *mvm);
-
 /* Thermal management and CT-kill */
 void iwl_mvm_tt_tx_backoff(struct iwl_mvm *mvm, u32 backoff);
 void iwl_mvm_tt_temp_changed(struct iwl_mvm *mvm, u32 temp);
@@ -1367,6 +1416,23 @@ void iwl_mvm_tt_exit(struct iwl_mvm *mvm);
 void iwl_mvm_set_hw_ctkill_state(struct iwl_mvm *mvm, bool state);
 int iwl_mvm_get_temp(struct iwl_mvm *mvm);
 
+/* Location Aware Regulatory */
+struct iwl_mcc_update_resp *
+iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
+		   enum iwl_mcc_source src_id);
+int iwl_mvm_init_mcc(struct iwl_mvm *mvm);
+int iwl_mvm_rx_chub_update_mcc(struct iwl_mvm *mvm,
+			       struct iwl_rx_cmd_buffer *rxb,
+			       struct iwl_device_cmd *cmd);
+struct ieee80211_regdomain *iwl_mvm_get_regdomain(struct wiphy *wiphy,
+						  const char *alpha2,
+						  enum iwl_mcc_source src_id,
+						  bool *changed);
+struct ieee80211_regdomain *iwl_mvm_get_current_regdomain(struct iwl_mvm *mvm,
+							  bool *changed);
+int iwl_mvm_init_fw_regd(struct iwl_mvm *mvm);
+void iwl_mvm_update_changed_regdom(struct iwl_mvm *mvm);
+
 /* smart fifo */
 int iwl_mvm_sf_update(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 		      bool added_vif);
@@ -1405,7 +1471,65 @@ struct ieee80211_vif *iwl_mvm_get_bss_vif(struct iwl_mvm *mvm);
 void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error);
 void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm);
 
-int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, enum iwl_fw_dbg_conf id);
-void iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm);
+int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, u8 id);
+int iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm, enum iwl_fw_dbg_trigger trig,
+			   const char *str, size_t len, unsigned int delay);
+int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
+				struct iwl_mvm_dump_desc *desc,
+				unsigned int delay);
+void iwl_mvm_free_fw_dump_desc(struct iwl_mvm *mvm);
+int iwl_mvm_fw_dbg_collect_trig(struct iwl_mvm *mvm,
+				struct iwl_fw_dbg_trigger_tlv *trigger,
+				const char *fmt, ...) __printf(3, 4);
+unsigned int iwl_mvm_get_wd_timeout(struct iwl_mvm *mvm,
+				    struct ieee80211_vif *vif,
+				    bool tdls, bool cmd_q);
+void iwl_mvm_connection_loss(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			     const char *errmsg);
+static inline bool
+iwl_fw_dbg_trigger_vif_match(struct iwl_fw_dbg_trigger_tlv *trig,
+			     struct ieee80211_vif *vif)
+{
+	u32 trig_vif = le32_to_cpu(trig->vif_type);
+
+	return trig_vif == IWL_FW_DBG_CONF_VIF_ANY || vif->type == trig_vif;
+}
+
+static inline bool
+iwl_fw_dbg_trigger_stop_conf_match(struct iwl_mvm *mvm,
+				   struct iwl_fw_dbg_trigger_tlv *trig)
+{
+	return ((trig->mode & IWL_FW_DBG_TRIGGER_STOP) &&
+		(mvm->fw_dbg_conf == FW_DBG_INVALID ||
+		(BIT(mvm->fw_dbg_conf) & le32_to_cpu(trig->stop_conf_ids))));
+}
+
+static inline bool
+iwl_fw_dbg_trigger_check_stop(struct iwl_mvm *mvm,
+			      struct ieee80211_vif *vif,
+			      struct iwl_fw_dbg_trigger_tlv *trig)
+{
+	if (vif && !iwl_fw_dbg_trigger_vif_match(trig, vif))
+		return false;
+
+	return iwl_fw_dbg_trigger_stop_conf_match(mvm, trig);
+}
+
+static inline void
+iwl_fw_dbg_trigger_simple_stop(struct iwl_mvm *mvm,
+			       struct ieee80211_vif *vif,
+			       enum iwl_fw_dbg_trigger trig)
+{
+	struct iwl_fw_dbg_trigger_tlv *trigger;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, trig))
+		return;
+
+	trigger = iwl_fw_dbg_get_trigger(mvm->fw, trig);
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trigger))
+		return;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trigger, NULL);
+}
 
 #endif /* __IWL_MVM_H__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/nvm.c b/drivers/net/wireless/iwlwifi/mvm/nvm.c
index 5383429d96c1..87b2a30a2308 100644
--- a/drivers/net/wireless/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/iwlwifi/mvm/nvm.c
@@ -63,18 +63,21 @@
  *
  *****************************************************************************/
 #include <linux/firmware.h>
+#include <linux/rtnetlink.h>
+#include <linux/pci.h>
+#include <linux/acpi.h>
 #include "iwl-trans.h"
 #include "iwl-csr.h"
 #include "mvm.h"
 #include "iwl-eeprom-parse.h"
 #include "iwl-eeprom-read.h"
 #include "iwl-nvm-parse.h"
+#include "iwl-prph.h"
 
 /* Default NVM size to read */
 #define IWL_NVM_DEFAULT_CHUNK_SIZE (2*1024)
 #define IWL_MAX_NVM_SECTION_SIZE	0x1b58
-#define IWL_MAX_NVM_8000A_SECTION_SIZE	0xffc
-#define IWL_MAX_NVM_8000B_SECTION_SIZE	0x1ffc
+#define IWL_MAX_NVM_8000_SECTION_SIZE	0x1ffc
 
 #define NVM_WRITE_OPCODE 1
 #define NVM_READ_OPCODE 0
@@ -262,7 +265,9 @@ static struct iwl_nvm_data *
 iwl_parse_nvm_sections(struct iwl_mvm *mvm)
 {
 	struct iwl_nvm_section *sections = mvm->nvm_sections;
-	const __le16 *hw, *sw, *calib, *regulatory, *mac_override;
+	const __le16 *hw, *sw, *calib, *regulatory, *mac_override, *phy_sku;
+	bool lar_enabled;
+	u32 mac_addr0, mac_addr1;
 
 	/* Checking for required sections */
 	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
@@ -286,22 +291,38 @@ iwl_parse_nvm_sections(struct iwl_mvm *mvm)
 				"Can't parse mac_address, empty sections\n");
 			return NULL;
 		}
+
+		/* PHY_SKU section is mandatory in B0 */
+		if (!mvm->nvm_sections[NVM_SECTION_TYPE_PHY_SKU].data) {
+			IWL_ERR(mvm,
+				"Can't parse phy_sku in B0, empty sections\n");
+			return NULL;
+		}
 	}
 
 	if (WARN_ON(!mvm->cfg))
 		return NULL;
 
+	/* read the mac address from WFMP registers */
+	mac_addr0 = iwl_trans_read_prph(mvm->trans, WFMP_MAC_ADDR_0);
+	mac_addr1 = iwl_trans_read_prph(mvm->trans, WFMP_MAC_ADDR_1);
+
 	hw = (const __le16 *)sections[mvm->cfg->nvm_hw_section_num].data;
 	sw = (const __le16 *)sections[NVM_SECTION_TYPE_SW].data;
 	calib = (const __le16 *)sections[NVM_SECTION_TYPE_CALIBRATION].data;
 	regulatory = (const __le16 *)sections[NVM_SECTION_TYPE_REGULATORY].data;
 	mac_override =
 		(const __le16 *)sections[NVM_SECTION_TYPE_MAC_OVERRIDE].data;
+	phy_sku = (const __le16 *)sections[NVM_SECTION_TYPE_PHY_SKU].data;
+
+	lar_enabled = !iwlwifi_mod_params.lar_disable &&
+		      (mvm->fw->ucode_capa.capa[0] &
+		       IWL_UCODE_TLV_CAPA_LAR_SUPPORT);
 
 	return iwl_parse_nvm_data(mvm->trans->dev, mvm->cfg, hw, sw, calib,
-				  regulatory, mac_override,
-				  mvm->fw->valid_tx_ant,
-				  mvm->fw->valid_rx_ant);
+				  regulatory, mac_override, phy_sku,
+				  mvm->fw->valid_tx_ant, mvm->fw->valid_rx_ant,
+				  lar_enabled, mac_addr0, mac_addr1);
 }
 
 #define MAX_NVM_FILE_LEN	16384
@@ -354,10 +375,8 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 	/* Maximal size depends on HW family and step */
 	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		max_section_size = IWL_MAX_NVM_SECTION_SIZE;
-	else if (CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_A_STEP)
-		max_section_size = IWL_MAX_NVM_8000A_SECTION_SIZE;
-	else /* Family 8000 B-step or C-step */
-		max_section_size = IWL_MAX_NVM_8000B_SECTION_SIZE;
+	else
+		max_section_size = IWL_MAX_NVM_8000_SECTION_SIZE;
 
 	/*
 	 * Obtain NVM image via request_firmware. Since we already used
@@ -399,6 +418,15 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 		IWL_INFO(mvm, "NVM Version %08X\n", le32_to_cpu(dword_buff[2]));
 		IWL_INFO(mvm, "NVM Manufacturing date %08X\n",
 			 le32_to_cpu(dword_buff[3]));
+
+		/* nvm file validation, dword_buff[2] holds the file version */
+		if ((CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_C_STEP &&
+		     le32_to_cpu(dword_buff[2]) < 0xE4A) ||
+		    (CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP &&
+		     le32_to_cpu(dword_buff[2]) >= 0xE4A)) {
+			ret = -EFAULT;
+			goto out;
+		}
 	} else {
 		file_sec = (void *)fw_entry->data;
 	}
@@ -497,6 +525,8 @@ int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 	int ret, section;
 	u32 size_read = 0;
 	u8 *nvm_buffer, *temp;
+	const char *nvm_file_B = mvm->cfg->default_nvm_file_B_step;
+	const char *nvm_file_C = mvm->cfg->default_nvm_file_C_step;
 
 	if (WARN_ON_ONCE(mvm->cfg->nvm_hw_section_num >= NVM_MAX_NUM_SECTIONS))
 		return -EINVAL;
@@ -555,10 +585,27 @@ int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 
 	/* load external NVM if configured */
 	if (mvm->nvm_file_name) {
-		/* move to External NVM flow */
+		/* read External NVM file - take the default */
 		ret = iwl_mvm_read_external_nvm(mvm);
-		if (ret)
-			return ret;
+		if (ret) {
+			/* choose the nvm_file name according to the
+			 * HW step
+			 */
+			if (CSR_HW_REV_STEP(mvm->trans->hw_rev) ==
+			    SILICON_B_STEP)
+				mvm->nvm_file_name = nvm_file_B;
+			else
+				mvm->nvm_file_name = nvm_file_C;
+
+			if (ret == -EFAULT && mvm->nvm_file_name) {
+				/* in case nvm file was failed try again */
+				ret = iwl_mvm_read_external_nvm(mvm);
+				if (ret)
+					return ret;
+			} else {
+				return ret;
+			}
+		}
 	}
 
 	/* parse the relevant nvm sections */
@@ -570,3 +617,257 @@ int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 
 	return 0;
 }
+
+struct iwl_mcc_update_resp *
+iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
+		   enum iwl_mcc_source src_id)
+{
+	struct iwl_mcc_update_cmd mcc_update_cmd = {
+		.mcc = cpu_to_le16(alpha2[0] << 8 | alpha2[1]),
+		.source_id = (u8)src_id,
+	};
+	struct iwl_mcc_update_resp *mcc_resp, *resp_cp = NULL;
+	struct iwl_rx_packet *pkt;
+	struct iwl_host_cmd cmd = {
+		.id = MCC_UPDATE_CMD,
+		.flags = CMD_WANT_SKB,
+		.data = { &mcc_update_cmd },
+	};
+
+	int ret;
+	u32 status;
+	int resp_len, n_channels;
+	u16 mcc;
+
+	if (WARN_ON_ONCE(!iwl_mvm_is_lar_supported(mvm)))
+		return ERR_PTR(-EOPNOTSUPP);
+
+	cmd.len[0] = sizeof(struct iwl_mcc_update_cmd);
+
+	IWL_DEBUG_LAR(mvm, "send MCC update to FW with '%c%c' src = %d\n",
+		      alpha2[0], alpha2[1], src_id);
+
+	ret = iwl_mvm_send_cmd(mvm, &cmd);
+	if (ret)
+		return ERR_PTR(ret);
+
+	pkt = cmd.resp_pkt;
+	if (pkt->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERR(mvm, "Bad return from MCC_UPDATE_COMMAND (0x%08X)\n",
+			pkt->hdr.flags);
+		ret = -EIO;
+		goto exit;
+	}
+
+	/* Extract MCC response */
+	mcc_resp = (void *)pkt->data;
+	status = le32_to_cpu(mcc_resp->status);
+
+	mcc = le16_to_cpu(mcc_resp->mcc);
+
+	/* W/A for a FW/NVM issue - returns 0x00 for the world domain */
+	if (mcc == 0) {
+		mcc = 0x3030;  /* "00" - world */
+		mcc_resp->mcc = cpu_to_le16(mcc);
+	}
+
+	n_channels =  __le32_to_cpu(mcc_resp->n_channels);
+	IWL_DEBUG_LAR(mvm,
+		      "MCC response status: 0x%x. new MCC: 0x%x ('%c%c') change: %d n_chans: %d\n",
+		      status, mcc, mcc >> 8, mcc & 0xff,
+		      !!(status == MCC_RESP_NEW_CHAN_PROFILE), n_channels);
+
+	resp_len = sizeof(*mcc_resp) + n_channels * sizeof(__le32);
+	resp_cp = kmemdup(mcc_resp, resp_len, GFP_KERNEL);
+	if (!resp_cp) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	ret = 0;
+exit:
+	iwl_free_resp(&cmd);
+	if (ret)
+		return ERR_PTR(ret);
+	return resp_cp;
+}
+
+#ifdef CONFIG_ACPI
+#define WRD_METHOD		"WRDD"
+#define WRDD_WIFI		(0x07)
+#define WRDD_WIGIG		(0x10)
+
+static u32 iwl_mvm_wrdd_get_mcc(struct iwl_mvm *mvm, union acpi_object *wrdd)
+{
+	union acpi_object *mcc_pkg, *domain_type, *mcc_value;
+	u32 i;
+
+	if (wrdd->type != ACPI_TYPE_PACKAGE ||
+	    wrdd->package.count < 2 ||
+	    wrdd->package.elements[0].type != ACPI_TYPE_INTEGER ||
+	    wrdd->package.elements[0].integer.value != 0) {
+		IWL_DEBUG_LAR(mvm, "Unsupported wrdd structure\n");
+		return 0;
+	}
+
+	for (i = 1 ; i < wrdd->package.count ; ++i) {
+		mcc_pkg = &wrdd->package.elements[i];
+
+		if (mcc_pkg->type != ACPI_TYPE_PACKAGE ||
+		    mcc_pkg->package.count < 2 ||
+		    mcc_pkg->package.elements[0].type != ACPI_TYPE_INTEGER ||
+		    mcc_pkg->package.elements[1].type != ACPI_TYPE_INTEGER) {
+			mcc_pkg = NULL;
+			continue;
+		}
+
+		domain_type = &mcc_pkg->package.elements[0];
+		if (domain_type->integer.value == WRDD_WIFI)
+			break;
+
+		mcc_pkg = NULL;
+	}
+
+	if (mcc_pkg) {
+		mcc_value = &mcc_pkg->package.elements[1];
+		return mcc_value->integer.value;
+	}
+
+	return 0;
+}
+
+static int iwl_mvm_get_bios_mcc(struct iwl_mvm *mvm, char *mcc)
+{
+	acpi_handle root_handle;
+	acpi_handle handle;
+	struct acpi_buffer wrdd = {ACPI_ALLOCATE_BUFFER, NULL};
+	acpi_status status;
+	u32 mcc_val;
+	struct pci_dev *pdev = to_pci_dev(mvm->dev);
+
+	root_handle = ACPI_HANDLE(&pdev->dev);
+	if (!root_handle) {
+		IWL_DEBUG_LAR(mvm,
+			      "Could not retrieve root port ACPI handle\n");
+		return -ENOENT;
+	}
+
+	/* Get the method's handle */
+	status = acpi_get_handle(root_handle, (acpi_string)WRD_METHOD, &handle);
+	if (ACPI_FAILURE(status)) {
+		IWL_DEBUG_LAR(mvm, "WRD method not found\n");
+		return -ENOENT;
+	}
+
+	/* Call WRDD with no arguments */
+	status = acpi_evaluate_object(handle, NULL, NULL, &wrdd);
+	if (ACPI_FAILURE(status)) {
+		IWL_DEBUG_LAR(mvm, "WRDC invocation failed (0x%x)\n", status);
+		return -ENOENT;
+	}
+
+	mcc_val = iwl_mvm_wrdd_get_mcc(mvm, wrdd.pointer);
+	kfree(wrdd.pointer);
+	if (!mcc_val)
+		return -ENOENT;
+
+	mcc[0] = (mcc_val >> 8) & 0xff;
+	mcc[1] = mcc_val & 0xff;
+	mcc[2] = '\0';
+	return 0;
+}
+#else /* CONFIG_ACPI */
+static int iwl_mvm_get_bios_mcc(struct iwl_mvm *mvm, char *mcc)
+{
+	return -ENOENT;
+}
+#endif
+
+int iwl_mvm_init_mcc(struct iwl_mvm *mvm)
+{
+	bool tlv_lar;
+	bool nvm_lar;
+	int retval;
+	struct ieee80211_regdomain *regd;
+	char mcc[3];
+
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
+		tlv_lar = mvm->fw->ucode_capa.capa[0] &
+			IWL_UCODE_TLV_CAPA_LAR_SUPPORT;
+		nvm_lar = mvm->nvm_data->lar_enabled;
+		if (tlv_lar != nvm_lar)
+			IWL_INFO(mvm,
+				 "Conflict between TLV & NVM regarding enabling LAR (TLV = %s NVM =%s)\n",
+				 tlv_lar ? "enabled" : "disabled",
+				 nvm_lar ? "enabled" : "disabled");
+	}
+
+	if (!iwl_mvm_is_lar_supported(mvm))
+		return 0;
+
+	/*
+	 * try to replay the last set MCC to FW. If it doesn't exist,
+	 * queue an update to cfg80211 to retrieve the default alpha2 from FW.
+	 */
+	retval = iwl_mvm_init_fw_regd(mvm);
+	if (retval != -ENOENT)
+		return retval;
+
+	/*
+	 * Driver regulatory hint for initial update, this also informs the
+	 * firmware we support wifi location updates.
+	 * Disallow scans that might crash the FW while the LAR regdomain
+	 * is not set.
+	 */
+	mvm->lar_regdom_set = false;
+
+	regd = iwl_mvm_get_current_regdomain(mvm, NULL);
+	if (IS_ERR_OR_NULL(regd))
+		return -EIO;
+
+	if (iwl_mvm_is_wifi_mcc_supported(mvm) &&
+	    !iwl_mvm_get_bios_mcc(mvm, mcc)) {
+		kfree(regd);
+		regd = iwl_mvm_get_regdomain(mvm->hw->wiphy, mcc,
+					     MCC_SOURCE_BIOS, NULL);
+		if (IS_ERR_OR_NULL(regd))
+			return -EIO;
+	}
+
+	retval = regulatory_set_wiphy_regd_sync_rtnl(mvm->hw->wiphy, regd);
+	kfree(regd);
+	return retval;
+}
+
+int iwl_mvm_rx_chub_update_mcc(struct iwl_mvm *mvm,
+			       struct iwl_rx_cmd_buffer *rxb,
+			       struct iwl_device_cmd *cmd)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_mcc_chub_notif *notif = (void *)pkt->data;
+	enum iwl_mcc_source src;
+	char mcc[3];
+	struct ieee80211_regdomain *regd;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	if (WARN_ON_ONCE(!iwl_mvm_is_lar_supported(mvm)))
+		return 0;
+
+	mcc[0] = notif->mcc >> 8;
+	mcc[1] = notif->mcc & 0xff;
+	mcc[2] = '\0';
+	src = notif->source_id;
+
+	IWL_DEBUG_LAR(mvm,
+		      "RX: received chub update mcc cmd (mcc '%s' src %d)\n",
+		      mcc, src);
+	regd = iwl_mvm_get_regdomain(mvm->hw->wiphy, mcc, src, NULL);
+	if (IS_ERR_OR_NULL(regd))
+		return 0;
+
+	regulatory_set_wiphy_regd(mvm->hw->wiphy, regd);
+	kfree(regd);
+
+	return 0;
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/iwlwifi/mvm/ops.c
index 2dffc3600ed3..a08b03d58d4b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/iwlwifi/mvm/ops.c
@@ -82,7 +82,6 @@
 #include "rs.h"
 #include "fw-api-scan.h"
 #include "time-event.h"
-#include "iwl-fw-error-dump.h"
 
 #define DRV_DESCRIPTION	"The new Intel(R) wireless AGN driver for Linux"
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
@@ -234,11 +233,10 @@ static const struct iwl_rx_handlers iwl_mvm_rx_handlers[] = {
 		   iwl_mvm_rx_ant_coupling_notif, true),
 
 	RX_HANDLER(TIME_EVENT_NOTIFICATION, iwl_mvm_rx_time_event_notif, false),
+	RX_HANDLER(MCC_CHUB_UPDATE_CMD, iwl_mvm_rx_chub_update_mcc, true),
 
 	RX_HANDLER(EOSP_NOTIFICATION, iwl_mvm_rx_eosp_notif, false),
 
-	RX_HANDLER(SCAN_REQUEST_CMD, iwl_mvm_rx_scan_response, false),
-	RX_HANDLER(SCAN_COMPLETE_NOTIFICATION, iwl_mvm_rx_scan_complete, true),
 	RX_HANDLER(SCAN_ITERATION_COMPLETE,
 		   iwl_mvm_rx_scan_offload_iter_complete_notif, false),
 	RX_HANDLER(SCAN_OFFLOAD_COMPLETE,
@@ -311,6 +309,7 @@ static const char *const iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(REPLY_RX_MPDU_CMD),
 	CMD(BEACON_NOTIFICATION),
 	CMD(BEACON_TEMPLATE_CMD),
+	CMD(STATISTICS_CMD),
 	CMD(STATISTICS_NOTIFICATION),
 	CMD(EOSP_NOTIFICATION),
 	CMD(REDUCE_TX_POWER_CMD),
@@ -359,6 +358,7 @@ static const char *const iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(TDLS_CHANNEL_SWITCH_CMD),
 	CMD(TDLS_CHANNEL_SWITCH_NOTIFICATION),
 	CMD(TDLS_CONFIG_CMD),
+	CMD(MCC_UPDATE_CMD),
 };
 #undef CMD
 
@@ -456,7 +456,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	INIT_WORK(&mvm->roc_done_wk, iwl_mvm_roc_done_wk);
 	INIT_WORK(&mvm->sta_drained_wk, iwl_mvm_sta_drained_wk);
 	INIT_WORK(&mvm->d0i3_exit_work, iwl_mvm_d0i3_exit_work);
-	INIT_WORK(&mvm->fw_error_dump_wk, iwl_mvm_fw_error_dump_wk);
+	INIT_DELAYED_WORK(&mvm->fw_dump_wk, iwl_mvm_fw_error_dump_wk);
 	INIT_DELAYED_WORK(&mvm->tdls_cs.dwork, iwl_mvm_tdls_ch_switch_work);
 
 	spin_lock_init(&mvm->d0i3_tx_lock);
@@ -488,8 +488,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 
 	/* Set a short watchdog for the command queue */
 	trans_cfg.cmd_q_wdg_timeout =
-		iwlmvm_mod_params.tfd_q_hang_detect ? IWL_DEF_WD_TIMEOUT :
-						      IWL_WATCHDOG_DISABLED;
+		iwl_mvm_get_wd_timeout(mvm, NULL, false, true);
 
 	snprintf(mvm->hw->wiphy->fw_version,
 		 sizeof(mvm->hw->wiphy->fw_version),
@@ -504,6 +503,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	trans->dbg_dest_reg_num = mvm->fw->dbg_dest_reg_num;
 	memcpy(trans->dbg_conf_tlv, mvm->fw->dbg_conf_tlv,
 	       sizeof(trans->dbg_conf_tlv));
+	trans->dbg_trigger_tlv = mvm->fw->dbg_trigger_tlv;
 
 	/* set up notification wait support */
 	iwl_notification_wait_init(&mvm->notif_wait);
@@ -523,12 +523,11 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	/* set the nvm_file_name according to priority */
 	if (iwlwifi_mod_params.nvm_file) {
 		mvm->nvm_file_name = iwlwifi_mod_params.nvm_file;
-	} else {
-		if ((trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) &&
-		    (CSR_HW_REV_STEP(trans->hw_rev) == SILICON_A_STEP))
-			mvm->nvm_file_name = mvm->cfg->default_nvm_file_8000A;
+	} else if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
+		if (CSR_HW_REV_STEP(trans->hw_rev) == SILICON_B_STEP)
+			mvm->nvm_file_name = mvm->cfg->default_nvm_file_B_step;
 		else
-			mvm->nvm_file_name = mvm->cfg->default_nvm_file;
+			mvm->nvm_file_name = mvm->cfg->default_nvm_file_C_step;
 	}
 
 	if (WARN(cfg->no_power_up_nic_in_init && !mvm->nvm_file_name,
@@ -685,6 +684,37 @@ static void iwl_mvm_async_handlers_wk(struct work_struct *wk)
 	mutex_unlock(&mvm->mutex);
 }
 
+static inline void iwl_mvm_rx_check_trigger(struct iwl_mvm *mvm,
+					    struct iwl_rx_packet *pkt)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_cmd *cmds_trig;
+	int i;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_FW_NOTIF))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_FW_NOTIF);
+	cmds_trig = (void *)trig->data;
+
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, NULL, trig))
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(cmds_trig->cmds); i++) {
+		/* don't collect on CMD 0 */
+		if (!cmds_trig->cmds[i].cmd_id)
+			break;
+
+		if (cmds_trig->cmds[i].cmd_id != pkt->hdr.cmd)
+			continue;
+
+		iwl_mvm_fw_dbg_collect_trig(mvm, trig,
+					    "CMD 0x%02x received",
+					    pkt->hdr.cmd);
+		break;
+	}
+}
+
 static int iwl_mvm_rx_dispatch(struct iwl_op_mode *op_mode,
 			       struct iwl_rx_cmd_buffer *rxb,
 			       struct iwl_device_cmd *cmd)
@@ -693,6 +723,8 @@ static int iwl_mvm_rx_dispatch(struct iwl_op_mode *op_mode,
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
 	u8 i;
 
+	iwl_mvm_rx_check_trigger(mvm, pkt);
+
 	/*
 	 * Do the notification wait before RX handlers so
 	 * even if the RX handler consumes the RXB we have
@@ -827,7 +859,7 @@ static void iwl_mvm_reprobe_wk(struct work_struct *wk)
 static void iwl_mvm_fw_error_dump_wk(struct work_struct *work)
 {
 	struct iwl_mvm *mvm =
-		container_of(work, struct iwl_mvm, fw_error_dump_wk);
+		container_of(work, struct iwl_mvm, fw_dump_wk.work);
 
 	if (iwl_mvm_ref_sync(mvm, IWL_MVM_REF_FW_DBG_COLLECT))
 		return;
@@ -837,8 +869,8 @@ static void iwl_mvm_fw_error_dump_wk(struct work_struct *work)
 
 	/* start recording again if the firmware is not crashed */
 	WARN_ON_ONCE((!test_bit(STATUS_FW_ERROR, &mvm->trans->status)) &&
-		      mvm->fw->dbg_dest_tlv &&
-		      iwl_mvm_start_fw_dbg_conf(mvm, mvm->fw_dbg_conf));
+		     mvm->fw->dbg_dest_tlv &&
+		     iwl_mvm_start_fw_dbg_conf(mvm, mvm->fw_dbg_conf));
 
 	mutex_unlock(&mvm->mutex);
 
@@ -859,18 +891,7 @@ void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error)
 	 * the next start() call from mac80211. If restart isn't called
 	 * (no fw restart) scan status will stay busy.
 	 */
-	switch (mvm->scan_status) {
-	case IWL_MVM_SCAN_NONE:
-		break;
-	case IWL_MVM_SCAN_OS:
-		ieee80211_scan_completed(mvm->hw, true);
-		break;
-	case IWL_MVM_SCAN_SCHED:
-		/* Sched scan will be restarted by mac80211 in restart_hw. */
-		if (!mvm->restart_fw)
-			ieee80211_sched_scan_stopped(mvm->hw);
-		break;
-	}
+	iwl_mvm_report_scan_aborted(mvm);
 
 	/*
 	 * If we're restarting already, don't cycle restarts.
@@ -879,7 +900,7 @@ void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error)
 	 * can't recover this since we're already half suspended.
 	 */
 	if (!mvm->restart_fw && fw_error) {
-		schedule_work(&mvm->fw_error_dump_wk);
+		iwl_mvm_fw_dbg_collect_desc(mvm, &iwl_mvm_dump_desc_assert, 0);
 	} else if (test_and_set_bit(IWL_MVM_STATUS_IN_HW_RESTART,
 				    &mvm->status)) {
 		struct iwl_mvm_reprobe *reprobe;
@@ -1140,7 +1161,7 @@ static void iwl_mvm_d0i3_disconnect_iter(void *data, u8 *mac,
 
 	if (vif->type == NL80211_IFTYPE_STATION && vif->bss_conf.assoc &&
 	    mvm->d0i3_ap_sta_id == mvmvif->ap_sta_id)
-		ieee80211_connection_loss(vif);
+		iwl_mvm_connection_loss(mvm, vif, "D0i3");
 }
 
 void iwl_mvm_d0i3_enable_tx(struct iwl_mvm *mvm, __le16 *qos_seq)
@@ -1236,6 +1257,10 @@ static void iwl_mvm_d0i3_exit_work(struct work_struct *wk)
 	iwl_free_resp(&get_status_cmd);
 out:
 	iwl_mvm_d0i3_enable_tx(mvm, qos_seq);
+
+	/* the FW might have updated the regdomain */
+	iwl_mvm_update_changed_regdom(mvm);
+
 	iwl_mvm_unref(mvm, IWL_MVM_REF_EXIT_WORK);
 	mutex_unlock(&mvm->mutex);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
index 5b43616eeb06..e68a475e3071 100644
--- a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
@@ -67,7 +67,7 @@
 #include "fw-api.h"
 #include "mvm.h"
 
-/* Maps the driver specific channel width definition to the the fw values */
+/* Maps the driver specific channel width definition to the fw values */
 u8 iwl_mvm_get_channel_width(struct cfg80211_chan_def *chandef)
 {
 	switch (chandef->width) {
@@ -175,6 +175,10 @@ static void iwl_mvm_phy_ctxt_cmd_data(struct iwl_mvm *mvm,
 	cmd->rxchain_info |= cpu_to_le32(idle_cnt << PHY_RX_CHAIN_CNT_POS);
 	cmd->rxchain_info |= cpu_to_le32(active_cnt <<
 					 PHY_RX_CHAIN_MIMO_CNT_POS);
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	if (unlikely(mvm->dbgfs_rx_phyinfo))
+		cmd->rxchain_info = cpu_to_le32(mvm->dbgfs_rx_phyinfo);
+#endif
 
 	cmd->txchain_info = cpu_to_le32(iwl_mvm_get_valid_tx_ant(mvm));
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/power.c b/drivers/net/wireless/iwlwifi/mvm/power.c
index 2620dd0c45f9..d2c6ba9d326b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/power.c
+++ b/drivers/net/wireless/iwlwifi/mvm/power.c
@@ -66,6 +66,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/etherdevice.h>
 
 #include <net/mac80211.h>
 
@@ -357,7 +358,7 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
 
 	if (!vif->bss_conf.ps || iwl_mvm_vif_low_latency(mvmvif) ||
-	    !mvmvif->pm_enabled || iwl_mvm_tdls_sta_count(mvm, vif))
+	    !mvmvif->pm_enabled)
 		return;
 
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK);
@@ -491,7 +492,7 @@ void iwl_mvm_power_vif_assoc(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 	if (memcmp(vif->bss_conf.bssid, mvmvif->uapsd_misbehaving_bssid,
 		   ETH_ALEN))
-		memset(mvmvif->uapsd_misbehaving_bssid, 0, ETH_ALEN);
+		eth_zero_addr(mvmvif->uapsd_misbehaving_bssid);
 }
 
 static void iwl_mvm_power_uapsd_misbehav_ap_iterator(void *_data, u8 *mac,
@@ -638,6 +639,10 @@ static void iwl_mvm_power_set_pm(struct iwl_mvm *mvm,
 	if (vifs->ap_vif)
 		ap_mvmvif = iwl_mvm_vif_from_mac80211(vifs->ap_vif);
 
+	/* don't allow PM if any TDLS stations exist */
+	if (iwl_mvm_tdls_sta_count(mvm, NULL))
+		return;
+
 	/* enable PM on bss if bss stand alone */
 	if (vifs->bss_active && !vifs->p2p_active && !vifs->ap_active) {
 		bss_mvmvif->pm_enabled = true;
diff --git a/drivers/net/wireless/iwlwifi/mvm/quota.c b/drivers/net/wireless/iwlwifi/mvm/quota.c
index dbb2594390e9..509a66d05245 100644
--- a/drivers/net/wireless/iwlwifi/mvm/quota.c
+++ b/drivers/net/wireless/iwlwifi/mvm/quota.c
@@ -172,6 +172,7 @@ static void iwl_mvm_adjust_quota_for_noa(struct iwl_mvm *mvm,
 }
 
 int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
+			  bool force_update,
 			  struct ieee80211_vif *disabled_vif)
 {
 	struct iwl_time_quota_cmd cmd = {};
@@ -309,7 +310,7 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
 			  "zero quota on binding %d\n", i);
 	}
 
-	if (!send) {
+	if (!send && !force_update) {
 		/* don't send a practically unchanged command, the firmware has
 		 * to re-initialize a lot of state and that can have an adverse
 		 * impact on it
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 194bd1f939ca..f9928f2c125f 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,14 +150,19 @@ 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)
 {
+	struct iwl_mvm_sta *mvmsta;
+	struct iwl_mvm_vif *mvmvif;
+
 	if (!sta->ht_cap.ht_supported)
 		return false;
 
@@ -167,11 +175,17 @@ static bool rs_mimo_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	if (!iwl_mvm_bt_coex_is_mimo_allowed(mvm, sta))
 		return false;
 
+	mvmsta = iwl_mvm_sta_from_mac80211(sta);
+	mvmvif = iwl_mvm_vif_from_mac80211(mvmsta->vif);
+	if (iwl_mvm_vif_low_latency(mvmvif) && mvmsta->vif->p2p)
+		return false;
+
 	return true;
 }
 
 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 +194,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;
@@ -800,6 +815,8 @@ static int rs_rate_from_ucode_rate(const u32 ucode_rate,
 		rate->ldpc = true;
 	if (ucode_rate & RATE_MCS_VHT_STBC_MSK)
 		rate->stbc = true;
+	if (ucode_rate & RATE_MCS_BF_MSK)
+		rate->bfer = true;
 
 	rate->bw = ucode_rate & RATE_MCS_CHAN_WIDTH_MSK;
 
@@ -809,7 +826,9 @@ static int rs_rate_from_ucode_rate(const u32 ucode_rate,
 
 		if (nss == 1) {
 			rate->type = LQ_HT_SISO;
-			WARN_ON_ONCE(!rate->stbc && num_of_ant != 1);
+			WARN_ONCE(!rate->stbc && !rate->bfer && num_of_ant != 1,
+				  "stbc %d bfer %d",
+				  rate->stbc, rate->bfer);
 		} else if (nss == 2) {
 			rate->type = LQ_HT_MIMO2;
 			WARN_ON_ONCE(num_of_ant != 2);
@@ -822,7 +841,9 @@ static int rs_rate_from_ucode_rate(const u32 ucode_rate,
 
 		if (nss == 1) {
 			rate->type = LQ_VHT_SISO;
-			WARN_ON_ONCE(!rate->stbc && num_of_ant != 1);
+			WARN_ONCE(!rate->stbc && !rate->bfer && num_of_ant != 1,
+				  "stbc %d bfer %d",
+				  rate->stbc, rate->bfer);
 		} else if (nss == 2) {
 			rate->type = LQ_VHT_MIMO2;
 			WARN_ON_ONCE(num_of_ant != 2);
@@ -1001,13 +1022,41 @@ static void rs_get_lower_rate_down_column(struct iwl_lq_sta *lq_sta,
 		rs_get_lower_rate_in_column(lq_sta, rate);
 }
 
-/* Simple function to compare two rate scale table types */
-static inline bool rs_rate_match(struct rs_rate *a,
-				 struct rs_rate *b)
+/* Check if both rates are identical
+ * allow_ant_mismatch enables matching a SISO rate on ANT_A or ANT_B
+ * with a rate indicating STBC/BFER and ANT_AB.
+ */
+static inline bool rs_rate_equal(struct rs_rate *a,
+				 struct rs_rate *b,
+				 bool allow_ant_mismatch)
+
+{
+	bool ant_match = (a->ant == b->ant) && (a->stbc == b->stbc) &&
+		(a->bfer == b->bfer);
+
+	if (allow_ant_mismatch) {
+		if (a->stbc || a->bfer) {
+			WARN_ONCE(a->ant != ANT_AB, "stbc %d bfer %d ant %d",
+				  a->stbc, a->bfer, a->ant);
+			ant_match |= (b->ant == ANT_A || b->ant == ANT_B);
+		} else if (b->stbc || b->bfer) {
+			WARN_ONCE(b->ant != ANT_AB, "stbc %d bfer %d ant %d",
+				  b->stbc, b->bfer, b->ant);
+			ant_match |= (a->ant == ANT_A || a->ant == ANT_B);
+		}
+	}
+
+	return (a->type == b->type) && (a->bw == b->bw) && (a->sgi == b->sgi) &&
+		(a->ldpc == b->ldpc) && (a->index == b->index) && ant_match;
+}
+
+/* Check if both rates share the same column */
+static inline bool rs_rate_column_match(struct rs_rate *a,
+					struct rs_rate *b)
 {
 	bool ant_match;
 
-	if (a->stbc)
+	if (a->stbc || a->bfer)
 		ant_match = (b->ant == ANT_A || b->ant == ANT_B);
 	else
 		ant_match = (a->ant == b->ant);
@@ -1016,16 +1065,35 @@ static inline bool rs_rate_match(struct rs_rate *a,
 		&& ant_match;
 }
 
-static u32 rs_ch_width_from_mac_flags(enum mac80211_rate_control_flags flags)
+static inline enum rs_column rs_get_column_from_rate(struct rs_rate *rate)
 {
-	if (flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
-		return RATE_MCS_CHAN_WIDTH_40;
-	else if (flags & IEEE80211_TX_RC_80_MHZ_WIDTH)
-		return RATE_MCS_CHAN_WIDTH_80;
-	else if (flags & IEEE80211_TX_RC_160_MHZ_WIDTH)
-		return RATE_MCS_CHAN_WIDTH_160;
+	if (is_legacy(rate)) {
+		if (rate->ant == ANT_A)
+			return RS_COLUMN_LEGACY_ANT_A;
 
-	return RATE_MCS_CHAN_WIDTH_20;
+		if (rate->ant == ANT_B)
+			return RS_COLUMN_LEGACY_ANT_B;
+
+		goto err;
+	}
+
+	if (is_siso(rate)) {
+		if (rate->ant == ANT_A || rate->stbc || rate->bfer)
+			return rate->sgi ? RS_COLUMN_SISO_ANT_A_SGI :
+				RS_COLUMN_SISO_ANT_A;
+
+		if (rate->ant == ANT_B)
+			return rate->sgi ? RS_COLUMN_SISO_ANT_B_SGI :
+				RS_COLUMN_SISO_ANT_B;
+
+		goto err;
+	}
+
+	if (is_mimo(rate))
+		return rate->sgi ? RS_COLUMN_MIMO2_SGI : RS_COLUMN_MIMO2;
+
+err:
+	return RS_COLUMN_INVALID;
 }
 
 static u8 rs_get_tid(struct ieee80211_hdr *hdr)
@@ -1048,15 +1116,17 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 {
 	int legacy_success;
 	int retries;
-	int mac_index, i;
+	int i;
 	struct iwl_lq_cmd *table;
-	enum mac80211_rate_control_flags mac_flags;
-	u32 ucode_rate;
-	struct rs_rate rate;
+	u32 lq_hwrate;
+	struct rs_rate lq_rate, tx_resp_rate;
 	struct iwl_scale_tbl_info *curr_tbl, *other_tbl, *tmp_tbl;
 	u8 reduced_txp = (uintptr_t)info->status.status_driver_data[0];
+	u32 tx_resp_hwrate = (uintptr_t)info->status.status_driver_data[1];
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_lq_sta *lq_sta = &mvmsta->lq_sta;
+	bool allow_ant_mismatch = mvm->fw->ucode_capa.api[0] &
+		IWL_UCODE_TLV_API_LQ_SS_PARAMS;
 
 	/* Treat uninitialized rate scaling data same as non-existing. */
 	if (!lq_sta) {
@@ -1067,50 +1137,43 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		return;
 	}
 
-#ifdef CONFIG_MAC80211_DEBUGFS
-	/* Disable last tx check if we are debugging with fixed rate */
-	if (lq_sta->pers.dbg_fixed_rate) {
-		IWL_DEBUG_RATE(mvm, "Fixed rate. avoid rate scaling\n");
-		return;
-	}
-#endif
 	/* This packet was aggregated but doesn't carry status info */
 	if ((info->flags & IEEE80211_TX_CTL_AMPDU) &&
 	    !(info->flags & IEEE80211_TX_STAT_AMPDU))
 		return;
 
-	/*
-	 * Ignore this Tx frame response if its initial rate doesn't match
-	 * that of latest Link Quality command.  There may be stragglers
-	 * from a previous Link Quality command, but we're no longer interested
-	 * in those; they're either from the "active" mode while we're trying
-	 * to check "search" mode, or a prior "search" mode after we've moved
-	 * to a new "search" mode (which might become the new "active" mode).
-	 */
-	table = &lq_sta->lq;
-	ucode_rate = le32_to_cpu(table->rs_table[0]);
-	rs_rate_from_ucode_rate(ucode_rate, info->band, &rate);
-	if (info->band == IEEE80211_BAND_5GHZ)
-		rate.index -= IWL_FIRST_OFDM_RATE;
-	mac_flags = info->status.rates[0].flags;
-	mac_index = info->status.rates[0].idx;
-	/* For HT packets, map MCS to PLCP */
-	if (mac_flags & IEEE80211_TX_RC_MCS) {
-		/* Remove # of streams */
-		mac_index &= RATE_HT_MCS_RATE_CODE_MSK;
-		if (mac_index >= (IWL_RATE_9M_INDEX - IWL_FIRST_OFDM_RATE))
-			mac_index++;
-		/*
-		 * mac80211 HT index is always zero-indexed; we need to move
-		 * HT OFDM rates after CCK rates in 2.4 GHz band
-		 */
-		if (info->band == IEEE80211_BAND_2GHZ)
-			mac_index += IWL_FIRST_OFDM_RATE;
-	} else if (mac_flags & IEEE80211_TX_RC_VHT_MCS) {
-		mac_index &= RATE_VHT_MCS_RATE_CODE_MSK;
-		if (mac_index >= (IWL_RATE_9M_INDEX - IWL_FIRST_OFDM_RATE))
-			mac_index++;
+	rs_rate_from_ucode_rate(tx_resp_hwrate, info->band, &tx_resp_rate);
+
+#ifdef CONFIG_MAC80211_DEBUGFS
+	/* Disable last tx check if we are debugging with fixed rate but
+	 * update tx stats */
+	if (lq_sta->pers.dbg_fixed_rate) {
+		int index = tx_resp_rate.index;
+		enum rs_column column;
+		int attempts, success;
+
+		column = rs_get_column_from_rate(&tx_resp_rate);
+		if (WARN_ONCE(column == RS_COLUMN_INVALID,
+			      "Can't map rate 0x%x to column",
+			      tx_resp_hwrate))
+			return;
+
+		if (info->flags & IEEE80211_TX_STAT_AMPDU) {
+			attempts = info->status.ampdu_len;
+			success = info->status.ampdu_ack_len;
+		} else {
+			attempts = info->status.rates[0].count;
+			success = !!(info->flags & IEEE80211_TX_STAT_ACK);
+		}
+
+		lq_sta->pers.tx_stats[column][index].total += attempts;
+		lq_sta->pers.tx_stats[column][index].success += success;
+
+		IWL_DEBUG_RATE(mvm, "Fixed rate 0x%x success %d attempts %d\n",
+			       tx_resp_hwrate, success, attempts);
+		return;
 	}
+#endif
 
 	if (time_after(jiffies,
 		       (unsigned long)(lq_sta->last_tx +
@@ -1126,21 +1189,23 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	}
 	lq_sta->last_tx = jiffies;
 
+	/* Ignore this Tx frame response if its initial rate doesn't match
+	 * that of latest Link Quality command.  There may be stragglers
+	 * from a previous Link Quality command, but we're no longer interested
+	 * in those; they're either from the "active" mode while we're trying
+	 * to check "search" mode, or a prior "search" mode after we've moved
+	 * to a new "search" mode (which might become the new "active" mode).
+	 */
+	table = &lq_sta->lq;
+	lq_hwrate = le32_to_cpu(table->rs_table[0]);
+	rs_rate_from_ucode_rate(lq_hwrate, info->band, &lq_rate);
+
 	/* Here we actually compare this rate to the latest LQ command */
-	if ((mac_index < 0) ||
-	    (rate.sgi != !!(mac_flags & IEEE80211_TX_RC_SHORT_GI)) ||
-	    (rate.bw != rs_ch_width_from_mac_flags(mac_flags)) ||
-	    (rate.ant != info->status.antenna) ||
-	    (!!(ucode_rate & RATE_MCS_HT_MSK) !=
-	     !!(mac_flags & IEEE80211_TX_RC_MCS)) ||
-	    (!!(ucode_rate & RATE_MCS_VHT_MSK) !=
-	     !!(mac_flags & IEEE80211_TX_RC_VHT_MCS)) ||
-	    (!!(ucode_rate & RATE_HT_MCS_GF_MSK) !=
-	     !!(mac_flags & IEEE80211_TX_RC_GREEN_FIELD)) ||
-	    (rate.index != mac_index)) {
+	if (!rs_rate_equal(&tx_resp_rate, &lq_rate, allow_ant_mismatch)) {
 		IWL_DEBUG_RATE(mvm,
-			       "initial rate %d does not match %d (0x%x)\n",
-			       mac_index, rate.index, ucode_rate);
+			       "initial tx resp rate 0x%x does not match 0x%x\n",
+			       tx_resp_hwrate, lq_hwrate);
+
 		/*
 		 * Since rates mis-match, the last LQ command may have failed.
 		 * After IWL_MISSED_RATE_MAX mis-matches, resync the uCode with
@@ -1168,14 +1233,14 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		other_tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
 	}
 
-	if (WARN_ON_ONCE(!rs_rate_match(&rate, &curr_tbl->rate))) {
+	if (WARN_ON_ONCE(!rs_rate_column_match(&lq_rate, &curr_tbl->rate))) {
 		IWL_DEBUG_RATE(mvm,
 			       "Neither active nor search matches tx rate\n");
 		tmp_tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
 		rs_dump_rate(mvm, &tmp_tbl->rate, "ACTIVE");
 		tmp_tbl = &(lq_sta->lq_info[1 - lq_sta->active_tbl]);
 		rs_dump_rate(mvm, &tmp_tbl->rate, "SEARCH");
-		rs_dump_rate(mvm, &rate, "ACTUAL");
+		rs_dump_rate(mvm, &lq_rate, "ACTUAL");
 
 		/*
 		 * no matching table found, let's by-pass the data collection
@@ -1200,9 +1265,7 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		if (info->status.ampdu_ack_len == 0)
 			info->status.ampdu_len = 1;
 
-		ucode_rate = le32_to_cpu(table->rs_table[0]);
-		rs_rate_from_ucode_rate(ucode_rate, info->band, &rate);
-		rs_collect_tx_data(mvm, lq_sta, curr_tbl, rate.index,
+		rs_collect_tx_data(mvm, lq_sta, curr_tbl, lq_rate.index,
 				   info->status.ampdu_len,
 				   info->status.ampdu_ack_len,
 				   reduced_txp);
@@ -1214,9 +1277,7 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 					info->status.ampdu_ack_len);
 		}
 	} else {
-	/*
-	 * For legacy, update frame history with for each Tx retry.
-	 */
+		/* For legacy, update frame history with for each Tx retry. */
 		retries = info->status.rates[0].count - 1;
 		/* HW doesn't send more than 15 retries */
 		retries = min(retries, 15);
@@ -1225,21 +1286,23 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		legacy_success = !!(info->flags & IEEE80211_TX_STAT_ACK);
 		/* Collect data for each rate used during failed TX attempts */
 		for (i = 0; i <= retries; ++i) {
-			ucode_rate = le32_to_cpu(table->rs_table[i]);
-			rs_rate_from_ucode_rate(ucode_rate, info->band, &rate);
+			lq_hwrate = le32_to_cpu(table->rs_table[i]);
+			rs_rate_from_ucode_rate(lq_hwrate, info->band,
+						&lq_rate);
 			/*
 			 * Only collect stats if retried rate is in the same RS
 			 * table as active/search.
 			 */
-			if (rs_rate_match(&rate, &curr_tbl->rate))
+			if (rs_rate_column_match(&lq_rate, &curr_tbl->rate))
 				tmp_tbl = curr_tbl;
-			else if (rs_rate_match(&rate, &other_tbl->rate))
+			else if (rs_rate_column_match(&lq_rate,
+						      &other_tbl->rate))
 				tmp_tbl = other_tbl;
 			else
 				continue;
 
-			rs_collect_tx_data(mvm, lq_sta, tmp_tbl, rate.index, 1,
-					   i < retries ? 0 : legacy_success,
+			rs_collect_tx_data(mvm, lq_sta, tmp_tbl, lq_rate.index,
+					   1, i < retries ? 0 : legacy_success,
 					   reduced_txp);
 		}
 
@@ -1250,7 +1313,7 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		}
 	}
 	/* The last TX rate is cached in lq_sta; it's set in if/else above */
-	lq_sta->last_rate_n_flags = ucode_rate;
+	lq_sta->last_rate_n_flags = lq_hwrate;
 	IWL_DEBUG_RATE(mvm, "reduced txpower: %d\n", reduced_txp);
 done:
 	/* See if there's a better rate or modulation mode to try. */
@@ -1271,6 +1334,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;
@@ -1547,9 +1613,9 @@ static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search)
 static void rs_update_rate_tbl(struct iwl_mvm *mvm,
 			       struct ieee80211_sta *sta,
 			       struct iwl_lq_sta *lq_sta,
-			       struct rs_rate *rate)
+			       struct iwl_scale_tbl_info *tbl)
 {
-	rs_fill_lq_cmd(mvm, sta, lq_sta, rate);
+	rs_fill_lq_cmd(mvm, sta, lq_sta, &tbl->rate);
 	iwl_mvm_send_lq_cmd(mvm, &lq_sta->lq, false);
 }
 
@@ -1590,7 +1656,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;
 		}
 
@@ -2079,7 +2145,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 			rate->type = LQ_NONE;
 			lq_sta->search_better_tbl = 0;
 			tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
-			rs_update_rate_tbl(mvm, sta, lq_sta, &tbl->rate);
+			rs_update_rate_tbl(mvm, sta, lq_sta, tbl);
 		}
 		return;
 	}
@@ -2242,7 +2308,7 @@ lq_update:
 	/* Replace uCode's rate table for the destination station. */
 	if (update_lq) {
 		tbl->rate.index = index;
-		rs_update_rate_tbl(mvm, sta, lq_sta, &tbl->rate);
+		rs_update_rate_tbl(mvm, sta, lq_sta, tbl);
 	}
 
 	rs_stay_in_table(lq_sta, false);
@@ -2289,8 +2355,7 @@ lq_update:
 
 			rs_dump_rate(mvm, &tbl->rate,
 				     "Switch to SEARCH TABLE:");
-			rs_fill_lq_cmd(mvm, sta, lq_sta, &tbl->rate);
-			iwl_mvm_send_lq_cmd(mvm, &lq_sta->lq, false);
+			rs_update_rate_tbl(mvm, sta, lq_sta, tbl);
 		} else {
 			done_search = 1;
 		}
@@ -2504,6 +2569,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. */
@@ -2536,6 +2609,7 @@ static void *rs_alloc_sta(void *mvm_rate, struct ieee80211_sta *sta,
 #ifdef CONFIG_MAC80211_DEBUGFS
 	lq_sta->pers.dbg_fixed_rate = 0;
 	lq_sta->pers.dbg_fixed_txp_reduction = TPC_INVALID;
+	lq_sta->pers.ss_force = RS_SS_FORCE_NONE;
 #endif
 	lq_sta->pers.chains = 0;
 	memset(lq_sta->pers.chain_signal, 0, sizeof(lq_sta->pers.chain_signal));
@@ -2820,6 +2894,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);
@@ -3058,19 +3135,21 @@ static void rs_set_lq_ss_params(struct iwl_mvm *mvm,
 	if (!iwl_mvm_bt_coex_is_mimo_allowed(mvm, sta))
 		goto out;
 
+#ifdef CONFIG_MAC80211_DEBUGFS
 	/* Check if forcing the decision is configured.
 	 * Note that SISO is forced by not allowing STBC or BFER
 	 */
-	if (lq_sta->ss_force == RS_SS_FORCE_STBC)
+	if (lq_sta->pers.ss_force == RS_SS_FORCE_STBC)
 		ss_params |= (LQ_SS_STBC_1SS_ALLOWED | LQ_SS_FORCE);
-	else if (lq_sta->ss_force == RS_SS_FORCE_BFER)
+	else if (lq_sta->pers.ss_force == RS_SS_FORCE_BFER)
 		ss_params |= (LQ_SS_BFER_ALLOWED | LQ_SS_FORCE);
 
-	if (lq_sta->ss_force != RS_SS_FORCE_NONE) {
+	if (lq_sta->pers.ss_force != RS_SS_FORCE_NONE) {
 		IWL_DEBUG_RATE(mvm, "Forcing single stream Tx decision %d\n",
-			       lq_sta->ss_force);
+			       lq_sta->pers.ss_force);
 		goto out;
 	}
+#endif
 
 	if (lq_sta->stbc_capable)
 		ss_params |= LQ_SS_STBC_1SS_ALLOWED;
@@ -3156,7 +3235,7 @@ static void rs_fill_lq_cmd(struct iwl_mvm *mvm,
 	lq_cmd->agg_frame_cnt_limit = mvmsta->max_agg_bufsize;
 
 	/*
-	 * In case of low latency, tell the firwmare to leave a frame in the
+	 * In case of low latency, tell the firmware to leave a frame in the
 	 * Tx Fifo so that it can start a transaction in the same TxOP. This
 	 * basically allows the firmware to send bursts.
 	 */
@@ -3311,6 +3390,7 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 	struct iwl_mvm *mvm;
 	struct iwl_scale_tbl_info *tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
 	struct rs_rate *rate = &tbl->rate;
+	u32 ss_params;
 	mvm = lq_sta->pers.drv;
 	buff = kmalloc(2048, GFP_KERNEL);
 	if (!buff)
@@ -3330,16 +3410,16 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 			(is_legacy(rate)) ? "legacy" :
 			is_vht(rate) ? "VHT" : "HT");
 	if (!is_legacy(rate)) {
-		desc += sprintf(buff+desc, " %s",
+		desc += sprintf(buff + desc, " %s",
 		   (is_siso(rate)) ? "SISO" : "MIMO2");
-		   desc += sprintf(buff+desc, " %s",
-				   (is_ht20(rate)) ? "20MHz" :
-				   (is_ht40(rate)) ? "40MHz" :
-				   (is_ht80(rate)) ? "80Mhz" : "BAD BW");
-		   desc += sprintf(buff+desc, " %s %s %s\n",
-				   (rate->sgi) ? "SGI" : "NGI",
-				   (rate->ldpc) ? "LDPC" : "BCC",
-				   (lq_sta->is_agg) ? "AGG on" : "");
+		desc += sprintf(buff + desc, " %s",
+				(is_ht20(rate)) ? "20MHz" :
+				(is_ht40(rate)) ? "40MHz" :
+				(is_ht80(rate)) ? "80Mhz" : "BAD BW");
+		desc += sprintf(buff + desc, " %s %s %s\n",
+				(rate->sgi) ? "SGI" : "NGI",
+				(rate->ldpc) ? "LDPC" : "BCC",
+				(lq_sta->is_agg) ? "AGG on" : "");
 	}
 	desc += sprintf(buff+desc, "last tx rate=0x%X\n",
 			lq_sta->last_rate_n_flags);
@@ -3357,6 +3437,16 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 			lq_sta->lq.agg_frame_cnt_limit);
 
 	desc += sprintf(buff+desc, "reduced tpc=%d\n", lq_sta->lq.reduced_tpc);
+	ss_params = le32_to_cpu(lq_sta->lq.ss_params);
+	desc += sprintf(buff+desc, "single stream params: %s%s%s%s\n",
+			(ss_params & LQ_SS_PARAMS_VALID) ?
+			"VALID" : "INVALID",
+			(ss_params & LQ_SS_BFER_ALLOWED) ?
+			", BFER" : "",
+			(ss_params & LQ_SS_STBC_1SS_ALLOWED) ?
+			", STBC" : "",
+			(ss_params & LQ_SS_FORCE) ?
+			", FORCE" : "");
 	desc += sprintf(buff+desc,
 			"Start idx [0]=0x%x [1]=0x%x [2]=0x%x [3]=0x%x\n",
 			lq_sta->lq.initial_rate_index[0],
@@ -3533,7 +3623,7 @@ static ssize_t iwl_dbgfs_ss_force_read(struct file *file,
 	};
 
 	pos += scnprintf(buf+pos, bufsz-pos, "%s\n",
-			 ss_force_name[lq_sta->ss_force]);
+			 ss_force_name[lq_sta->pers.ss_force]);
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
@@ -3544,12 +3634,12 @@ static ssize_t iwl_dbgfs_ss_force_write(struct iwl_lq_sta *lq_sta, char *buf,
 	int ret = 0;
 
 	if (!strncmp("none", buf, 4)) {
-		lq_sta->ss_force = RS_SS_FORCE_NONE;
+		lq_sta->pers.ss_force = RS_SS_FORCE_NONE;
 	} else if (!strncmp("siso", buf, 4)) {
-		lq_sta->ss_force = RS_SS_FORCE_SISO;
+		lq_sta->pers.ss_force = RS_SS_FORCE_SISO;
 	} else if (!strncmp("stbc", buf, 4)) {
 		if (lq_sta->stbc_capable) {
-			lq_sta->ss_force = RS_SS_FORCE_STBC;
+			lq_sta->pers.ss_force = RS_SS_FORCE_STBC;
 		} else {
 			IWL_ERR(mvm,
 				"can't force STBC. peer doesn't support\n");
@@ -3557,7 +3647,7 @@ static ssize_t iwl_dbgfs_ss_force_write(struct iwl_lq_sta *lq_sta, char *buf,
 		}
 	} else if (!strncmp("bfer", buf, 4)) {
 		if (lq_sta->bfer_capable) {
-			lq_sta->ss_force = RS_SS_FORCE_BFER;
+			lq_sta->pers.ss_force = RS_SS_FORCE_BFER;
 		} else {
 			IWL_ERR(mvm,
 				"can't force BFER. peer doesn't support\n");
@@ -3580,9 +3670,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);
@@ -3646,7 +3742,7 @@ void iwl_mvm_rate_control_unregister(void)
 
 /**
  * iwl_mvm_tx_protection - Gets LQ command, change it to enable/disable
- * Tx protection, according to this rquest and previous requests,
+ * Tx protection, according to this request and previous requests,
  * and send the LQ command.
  * @mvmsta: The station
  * @enable: Enable Tx protection?
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/iwlwifi/mvm/rs.h
index dc4ef3dfafe1..e4aa9346a231 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.h
@@ -170,6 +170,7 @@ struct rs_rate {
 	bool sgi;
 	bool ldpc;
 	bool stbc;
+	bool bfer;
 };
 
 
@@ -331,14 +332,14 @@ struct iwl_lq_sta {
 	/* tx power reduce for this sta */
 	int tpc_reduce;
 
-	/* force STBC/BFER/SISO for testing */
-	enum rs_ss_force_opt ss_force;
-
 	/* persistent fields - initialized only once - keep last! */
 	struct lq_sta_pers {
 #ifdef CONFIG_MAC80211_DEBUGFS
 		u32 dbg_fixed_rate;
 		u8 dbg_fixed_txp_reduction;
+
+		/* force STBC/BFER/SISO for testing */
+		enum rs_ss_force_opt ss_force;
 #endif
 		u8 chains;
 		s8 chain_signal[IEEE80211_MAX_CHAINS];
diff --git a/drivers/net/wireless/iwlwifi/mvm/rx.c b/drivers/net/wireless/iwlwifi/mvm/rx.c
index f922131b4eab..78ec7db64ba5 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rx.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -345,6 +345,25 @@ int iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 		struct iwl_mvm_sta *mvmsta;
 		mvmsta = iwl_mvm_sta_from_mac80211(sta);
 		rs_update_last_rssi(mvm, &mvmsta->lq_sta, rx_status);
+
+		if (iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_RSSI) &&
+		    ieee80211_is_beacon(hdr->frame_control)) {
+			struct iwl_fw_dbg_trigger_tlv *trig;
+			struct iwl_fw_dbg_trigger_low_rssi *rssi_trig;
+			bool trig_check;
+			s32 rssi;
+
+			trig = iwl_fw_dbg_get_trigger(mvm->fw,
+						      FW_DBG_TRIGGER_RSSI);
+			rssi_trig = (void *)trig->data;
+			rssi = le32_to_cpu(rssi_trig->rssi);
+
+			trig_check =
+				iwl_fw_dbg_trigger_check_stop(mvm, mvmsta->vif,
+							      trig);
+			if (trig_check && rx_status->signal < rssi)
+				iwl_mvm_fw_dbg_collect_trig(mvm, trig, NULL);
+		}
 	}
 
 	rcu_read_unlock();
@@ -416,35 +435,43 @@ int iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 }
 
 static void iwl_mvm_update_rx_statistics(struct iwl_mvm *mvm,
-					 struct iwl_notif_statistics *stats)
+					 struct mvm_statistics_rx *rx_stats)
 {
-	/*
-	 * NOTE FW aggregates the statistics - BUT the statistics are cleared
-	 * when the driver issues REPLY_STATISTICS_CMD 0x9c with CLEAR_STATS
-	 * bit set.
-	 */
 	lockdep_assert_held(&mvm->mutex);
-	memcpy(&mvm->rx_stats, &stats->rx, sizeof(struct mvm_statistics_rx));
+
+	mvm->rx_stats = *rx_stats;
 }
 
 struct iwl_mvm_stat_data {
-	struct iwl_notif_statistics *stats;
 	struct iwl_mvm *mvm;
+	__le32 mac_id;
+	__s8 beacon_filter_average_energy;
+	struct mvm_statistics_general_v8 *general;
 };
 
 static void iwl_mvm_stat_iterator(void *_data, u8 *mac,
 				  struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_stat_data *data = _data;
-	struct iwl_notif_statistics *stats = data->stats;
 	struct iwl_mvm *mvm = data->mvm;
-	int sig = -stats->general.beacon_filter_average_energy;
+	int sig = -data->beacon_filter_average_energy;
 	int last_event;
 	int thold = vif->bss_conf.cqm_rssi_thold;
 	int hyst = vif->bss_conf.cqm_rssi_hyst;
-	u16 id = le32_to_cpu(stats->rx.general.mac_id);
+	u16 id = le32_to_cpu(data->mac_id);
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
+	/* This doesn't need the MAC ID check since it's not taking the
+	 * data copied into the "data" struct, but rather the data from
+	 * the notification directly.
+	 */
+	if (data->general) {
+		mvmvif->beacon_stats.num_beacons =
+			le32_to_cpu(data->general->beacon_counter[mvmvif->id]);
+		mvmvif->beacon_stats.avg_signal =
+			-data->general->beacon_average_energy[mvmvif->id];
+	}
+
 	if (mvmvif->id != id)
 		return;
 
@@ -500,34 +527,101 @@ static void iwl_mvm_stat_iterator(void *_data, u8 *mac,
 	}
 }
 
-/*
- * iwl_mvm_rx_statistics - STATISTICS_NOTIFICATION handler
- *
- * TODO: This handler is implemented partially.
- */
-int iwl_mvm_rx_statistics(struct iwl_mvm *mvm,
-			  struct iwl_rx_cmd_buffer *rxb,
-			  struct iwl_device_cmd *cmd)
+static inline void
+iwl_mvm_rx_stats_check_trigger(struct iwl_mvm *mvm, struct iwl_rx_packet *pkt)
 {
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_notif_statistics *stats = (void *)&pkt->data;
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_stats *trig_stats;
+	u32 trig_offset, trig_thold;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_STATS))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_STATS);
+	trig_stats = (void *)trig->data;
+
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, NULL, trig))
+		return;
+
+	trig_offset = le32_to_cpu(trig_stats->stop_offset);
+	trig_thold = le32_to_cpu(trig_stats->stop_threshold);
+
+	if (WARN_ON_ONCE(trig_offset >= iwl_rx_packet_payload_len(pkt)))
+		return;
+
+	if (le32_to_cpup((__le32 *) (pkt->data + trig_offset)) < trig_thold)
+		return;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trig, NULL);
+}
+
+void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
+				  struct iwl_rx_packet *pkt)
+{
+	size_t v8_len = sizeof(struct iwl_notif_statistics_v8);
+	size_t v10_len = sizeof(struct iwl_notif_statistics_v10);
 	struct iwl_mvm_stat_data data = {
-		.stats = stats,
 		.mvm = mvm,
 	};
+	u32 temperature;
+
+	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_STATS_V10) {
+		struct iwl_notif_statistics_v10 *stats = (void *)&pkt->data;
+
+		if (iwl_rx_packet_payload_len(pkt) != v10_len)
+			goto invalid;
+
+		temperature = le32_to_cpu(stats->general.radio_temperature);
+		data.mac_id = stats->rx.general.mac_id;
+		data.beacon_filter_average_energy =
+			stats->general.beacon_filter_average_energy;
+
+		iwl_mvm_update_rx_statistics(mvm, &stats->rx);
+
+		mvm->radio_stats.rx_time = le64_to_cpu(stats->general.rx_time);
+		mvm->radio_stats.tx_time = le64_to_cpu(stats->general.tx_time);
+		mvm->radio_stats.on_time_rf =
+			le64_to_cpu(stats->general.on_time_rf);
+		mvm->radio_stats.on_time_scan =
+			le64_to_cpu(stats->general.on_time_scan);
+
+		data.general = &stats->general;
+	} else {
+		struct iwl_notif_statistics_v8 *stats = (void *)&pkt->data;
+
+		if (iwl_rx_packet_payload_len(pkt) != v8_len)
+			goto invalid;
+
+		temperature = le32_to_cpu(stats->general.radio_temperature);
+		data.mac_id = stats->rx.general.mac_id;
+		data.beacon_filter_average_energy =
+			stats->general.beacon_filter_average_energy;
+
+		iwl_mvm_update_rx_statistics(mvm, &stats->rx);
+	}
+
+	iwl_mvm_rx_stats_check_trigger(mvm, pkt);
 
 	/* Only handle rx statistics temperature changes if async temp
 	 * notifications are not supported
 	 */
 	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_ASYNC_DTM))
-		iwl_mvm_tt_temp_changed(mvm,
-				le32_to_cpu(stats->general.radio_temperature));
-
-	iwl_mvm_update_rx_statistics(mvm, stats);
+		iwl_mvm_tt_temp_changed(mvm, temperature);
 
 	ieee80211_iterate_active_interfaces(mvm->hw,
 					    IEEE80211_IFACE_ITER_NORMAL,
 					    iwl_mvm_stat_iterator,
 					    &data);
+	return;
+ invalid:
+	IWL_ERR(mvm, "received invalid statistics size (%d)!\n",
+		iwl_rx_packet_payload_len(pkt));
+}
+
+int iwl_mvm_rx_statistics(struct iwl_mvm *mvm,
+			  struct iwl_rx_cmd_buffer *rxb,
+			  struct iwl_device_cmd *cmd)
+{
+	iwl_mvm_handle_rx_statistics(mvm, rxb_addr(rxb));
 	return 0;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index 7e9aa3cb3254..74e1c86289dc 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -82,6 +82,7 @@ struct iwl_mvm_scan_params {
 	struct _dwell {
 		u16 passive;
 		u16 active;
+		u16 fragmented;
 	} dwell[IEEE80211_NUM_BANDS];
 };
 
@@ -191,101 +192,6 @@ static u16 iwl_mvm_get_passive_dwell(struct iwl_mvm *mvm,
 	return band == IEEE80211_BAND_2GHZ ? 100 + 20 : 100 + 10;
 }
 
-static void iwl_mvm_scan_fill_channels(struct iwl_scan_cmd *cmd,
-				       struct cfg80211_scan_request *req,
-				       bool basic_ssid,
-				       struct iwl_mvm_scan_params *params)
-{
-	struct iwl_scan_channel *chan = (struct iwl_scan_channel *)
-		(cmd->data + le16_to_cpu(cmd->tx_cmd.len));
-	int i;
-	int type = BIT(req->n_ssids) - 1;
-	enum ieee80211_band band = req->channels[0]->band;
-
-	if (!basic_ssid)
-		type |= BIT(req->n_ssids);
-
-	for (i = 0; i < cmd->channel_count; i++) {
-		chan->channel = cpu_to_le16(req->channels[i]->hw_value);
-		chan->type = cpu_to_le32(type);
-		if (req->channels[i]->flags & IEEE80211_CHAN_NO_IR)
-			chan->type &= cpu_to_le32(~SCAN_CHANNEL_TYPE_ACTIVE);
-		chan->active_dwell = cpu_to_le16(params->dwell[band].active);
-		chan->passive_dwell = cpu_to_le16(params->dwell[band].passive);
-		chan->iteration_count = cpu_to_le16(1);
-		chan++;
-	}
-}
-
-/*
- * Fill in probe request with the following parameters:
- * TA is our vif HW address, which mac80211 ensures we have.
- * Packet is broadcasted, so this is both SA and DA.
- * The probe request IE is made out of two: first comes the most prioritized
- * SSID if a directed scan is requested. Second comes whatever extra
- * information was given to us as the scan request IE.
- */
-static u16 iwl_mvm_fill_probe_req(struct ieee80211_mgmt *frame, const u8 *ta,
-				  int n_ssids, const u8 *ssid, int ssid_len,
-				  const u8 *band_ie, int band_ie_len,
-				  const u8 *common_ie, int common_ie_len,
-				  int left)
-{
-	int len = 0;
-	u8 *pos = NULL;
-
-	/* Make sure there is enough space for the probe request,
-	 * two mandatory IEs and the data */
-	left -= 24;
-	if (left < 0)
-		return 0;
-
-	frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
-	eth_broadcast_addr(frame->da);
-	memcpy(frame->sa, ta, ETH_ALEN);
-	eth_broadcast_addr(frame->bssid);
-	frame->seq_ctrl = 0;
-
-	len += 24;
-
-	/* for passive scans, no need to fill anything */
-	if (n_ssids == 0)
-		return (u16)len;
-
-	/* points to the payload of the request */
-	pos = &frame->u.probe_req.variable[0];
-
-	/* fill in our SSID IE */
-	left -= ssid_len + 2;
-	if (left < 0)
-		return 0;
-	*pos++ = WLAN_EID_SSID;
-	*pos++ = ssid_len;
-	if (ssid && ssid_len) { /* ssid_len may be == 0 even if ssid is valid */
-		memcpy(pos, ssid, ssid_len);
-		pos += ssid_len;
-	}
-
-	len += ssid_len + 2;
-
-	if (WARN_ON(left < band_ie_len + common_ie_len))
-		return len;
-
-	if (band_ie && band_ie_len) {
-		memcpy(pos, band_ie, band_ie_len);
-		pos += band_ie_len;
-		len += band_ie_len;
-	}
-
-	if (common_ie && common_ie_len) {
-		memcpy(pos, common_ie, common_ie_len);
-		pos += common_ie_len;
-		len += common_ie_len;
-	}
-
-	return (u16)len;
-}
-
 static void iwl_mvm_scan_condition_iterator(void *data, u8 *mac,
 					    struct ieee80211_vif *vif)
 {
@@ -325,7 +231,7 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 			 * If there is more than one active interface make
 			 * passive scan more fragmented.
 			 */
-			frag_passive_dwell = (global_cnt < 2) ? 40 : 20;
+			frag_passive_dwell = 40;
 			params->max_out_time = frag_passive_dwell;
 		} else {
 			params->suspend_time = 120;
@@ -358,10 +264,10 @@ not_bound:
 
 	for (band = IEEE80211_BAND_2GHZ; band < IEEE80211_NUM_BANDS; band++) {
 		if (params->passive_fragmented)
-			params->dwell[band].passive = frag_passive_dwell;
-		else
-			params->dwell[band].passive =
-				iwl_mvm_get_passive_dwell(mvm, band);
+			params->dwell[band].fragmented = frag_passive_dwell;
+
+		params->dwell[band].passive = iwl_mvm_get_passive_dwell(mvm,
+									band);
 		params->dwell[band].active = iwl_mvm_get_active_dwell(mvm, band,
 								      n_ssids);
 	}
@@ -379,20 +285,11 @@ static int iwl_mvm_max_scan_ie_fw_cmd_room(struct iwl_mvm *mvm,
 {
 	int max_probe_len;
 
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN)
-		max_probe_len = SCAN_OFFLOAD_PROBE_REQ_SIZE;
-	else
-		max_probe_len = mvm->fw->ucode_capa.max_probe_length;
+	max_probe_len = SCAN_OFFLOAD_PROBE_REQ_SIZE;
 
 	/* we create the 802.11 header and SSID element */
 	max_probe_len -= 24 + 2;
 
-	/* basic ssid is added only for hw_scan with and old api */
-	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID) &&
-	    !(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN) &&
-	    !is_sched_scan)
-		max_probe_len -= 32;
-
 	/* DS parameter set element is added on 2.4GHZ band if required */
 	if (iwl_mvm_rrm_scan_needed(mvm))
 		max_probe_len -= 3;
@@ -404,9 +301,6 @@ int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan)
 {
 	int max_ie_len = iwl_mvm_max_scan_ie_fw_cmd_room(mvm, is_sched_scan);
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN))
-		return max_ie_len;
-
 	/* TODO: [BUG] This function should return the maximum allowed size of
 	 * scan IEs, however the LMAC scan api contains both 2GHZ and 5GHZ IEs
 	 * in the same command. So the correct implementation of this function
@@ -420,129 +314,6 @@ int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan)
 	return max_ie_len;
 }
 
-int iwl_mvm_scan_request(struct iwl_mvm *mvm,
-			 struct ieee80211_vif *vif,
-			 struct cfg80211_scan_request *req)
-{
-	struct iwl_host_cmd hcmd = {
-		.id = SCAN_REQUEST_CMD,
-		.len = { 0, },
-		.data = { mvm->scan_cmd, },
-		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
-	};
-	struct iwl_scan_cmd *cmd = mvm->scan_cmd;
-	int ret;
-	u32 status;
-	int ssid_len = 0;
-	u8 *ssid = NULL;
-	bool basic_ssid = !(mvm->fw->ucode_capa.flags &
-			   IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID);
-	struct iwl_mvm_scan_params params = {};
-
-	lockdep_assert_held(&mvm->mutex);
-
-	/* we should have failed registration if scan_cmd was NULL */
-	if (WARN_ON(mvm->scan_cmd == NULL))
-		return -ENOMEM;
-
-	IWL_DEBUG_SCAN(mvm, "Handling mac80211 scan request\n");
-	mvm->scan_status = IWL_MVM_SCAN_OS;
-	memset(cmd, 0, ksize(cmd));
-
-	cmd->channel_count = (u8)req->n_channels;
-	cmd->quiet_time = cpu_to_le16(IWL_ACTIVE_QUIET_TIME);
-	cmd->quiet_plcp_th = cpu_to_le16(IWL_PLCP_QUIET_THRESH);
-	cmd->rxchain_sel_flags = iwl_mvm_scan_rx_chain(mvm);
-
-	iwl_mvm_scan_calc_params(mvm, vif, req->n_ssids, req->flags, &params);
-	cmd->max_out_time = cpu_to_le32(params.max_out_time);
-	cmd->suspend_time = cpu_to_le32(params.suspend_time);
-	if (params.passive_fragmented)
-		cmd->scan_flags |= SCAN_FLAGS_FRAGMENTED_SCAN;
-
-	cmd->rxon_flags = iwl_mvm_scan_rxon_flags(req->channels[0]->band);
-	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
-					MAC_FILTER_IN_BEACON);
-
-	if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
-		cmd->type = cpu_to_le32(SCAN_TYPE_DISCOVERY_FORCED);
-	else
-		cmd->type = cpu_to_le32(SCAN_TYPE_FORCED);
-
-	cmd->repeats = cpu_to_le32(1);
-
-	/*
-	 * If the user asked for passive scan, don't change to active scan if
-	 * you see any activity on the channel - remain passive.
-	 */
-	if (req->n_ssids > 0) {
-		cmd->passive2active = cpu_to_le16(1);
-		cmd->scan_flags |= SCAN_FLAGS_PASSIVE2ACTIVE;
-		if (basic_ssid) {
-			ssid = req->ssids[0].ssid;
-			ssid_len = req->ssids[0].ssid_len;
-		}
-	} else {
-		cmd->passive2active = 0;
-		cmd->scan_flags &= ~SCAN_FLAGS_PASSIVE2ACTIVE;
-	}
-
-	iwl_mvm_scan_fill_ssids(cmd->direct_scan, req->ssids, req->n_ssids,
-				basic_ssid ? 1 : 0);
-
-	cmd->tx_cmd.tx_flags = cpu_to_le32(TX_CMD_FLG_SEQ_CTL |
-					   3 << TX_CMD_FLG_BT_PRIO_POS);
-
-	cmd->tx_cmd.sta_id = mvm->aux_sta.sta_id;
-	cmd->tx_cmd.life_time = cpu_to_le32(TX_CMD_LIFE_TIME_INFINITE);
-	cmd->tx_cmd.rate_n_flags =
-			iwl_mvm_scan_rate_n_flags(mvm, req->channels[0]->band,
-						  req->no_cck);
-
-	cmd->tx_cmd.len =
-		cpu_to_le16(iwl_mvm_fill_probe_req(
-			    (struct ieee80211_mgmt *)cmd->data,
-			    vif->addr,
-			    req->n_ssids, ssid, ssid_len,
-			    req->ie, req->ie_len, NULL, 0,
-			    mvm->fw->ucode_capa.max_probe_length));
-
-	iwl_mvm_scan_fill_channels(cmd, req, basic_ssid, &params);
-
-	cmd->len = cpu_to_le16(sizeof(struct iwl_scan_cmd) +
-		le16_to_cpu(cmd->tx_cmd.len) +
-		(cmd->channel_count * sizeof(struct iwl_scan_channel)));
-	hcmd.len[0] = le16_to_cpu(cmd->len);
-
-	status = SCAN_RESPONSE_OK;
-	ret = iwl_mvm_send_cmd_status(mvm, &hcmd, &status);
-	if (!ret && status == SCAN_RESPONSE_OK) {
-		IWL_DEBUG_SCAN(mvm, "Scan request was sent successfully\n");
-	} else {
-		/*
-		 * If the scan failed, it usually means that the FW was unable
-		 * to allocate the time events. Warn on it, but maybe we
-		 * should try to send the command again with different params.
-		 */
-		IWL_ERR(mvm, "Scan failed! status 0x%x ret %d\n",
-			status, ret);
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
-		ret = -EIO;
-	}
-	return ret;
-}
-
-int iwl_mvm_rx_scan_response(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
-			  struct iwl_device_cmd *cmd)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_cmd_response *resp = (void *)pkt->data;
-
-	IWL_DEBUG_SCAN(mvm, "Scan response received. status 0x%x\n",
-		       le32_to_cpu(resp->status));
-	return 0;
-}
-
 int iwl_mvm_rx_scan_offload_iter_complete_notif(struct iwl_mvm *mvm,
 						struct iwl_rx_cmd_buffer *rxb,
 						struct iwl_device_cmd *cmd)
@@ -556,130 +327,25 @@ int iwl_mvm_rx_scan_offload_iter_complete_notif(struct iwl_mvm *mvm,
 	return 0;
 }
 
-int iwl_mvm_rx_scan_complete(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
-			  struct iwl_device_cmd *cmd)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_scan_complete_notif *notif = (void *)pkt->data;
-
-	lockdep_assert_held(&mvm->mutex);
-
-	IWL_DEBUG_SCAN(mvm, "Scan complete: status=0x%x scanned channels=%d\n",
-		       notif->status, notif->scanned_channels);
-
-	if (mvm->scan_status == IWL_MVM_SCAN_OS)
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
-	ieee80211_scan_completed(mvm->hw, notif->status != SCAN_COMP_STATUS_OK);
-
-	iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
-
-	return 0;
-}
-
 int iwl_mvm_rx_scan_offload_results(struct iwl_mvm *mvm,
 				    struct iwl_rx_cmd_buffer *rxb,
 				    struct iwl_device_cmd *cmd)
 {
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-
-	if (!(mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) &&
-	    !(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN)) {
-		struct iwl_sched_scan_results *notif = (void *)pkt->data;
-
-		if (!(notif->client_bitmap & SCAN_CLIENT_SCHED_SCAN))
-			return 0;
-	}
-
 	IWL_DEBUG_SCAN(mvm, "Scheduled scan results\n");
 	ieee80211_sched_scan_results(mvm->hw);
 
 	return 0;
 }
 
-static bool iwl_mvm_scan_abort_notif(struct iwl_notif_wait_data *notif_wait,
-				     struct iwl_rx_packet *pkt, void *data)
-{
-	struct iwl_mvm *mvm =
-		container_of(notif_wait, struct iwl_mvm, notif_wait);
-	struct iwl_scan_complete_notif *notif;
-	u32 *resp;
-
-	switch (pkt->hdr.cmd) {
-	case SCAN_ABORT_CMD:
-		resp = (void *)pkt->data;
-		if (*resp == CAN_ABORT_STATUS) {
-			IWL_DEBUG_SCAN(mvm,
-				       "Scan can be aborted, wait until completion\n");
-			return false;
-		}
-
-		/*
-		 * If scan cannot be aborted, it means that we had a
-		 * SCAN_COMPLETE_NOTIFICATION in the pipe and it called
-		 * ieee80211_scan_completed already.
-		 */
-		IWL_DEBUG_SCAN(mvm, "Scan cannot be aborted, exit now: %d\n",
-			       *resp);
-		return true;
-
-	case SCAN_COMPLETE_NOTIFICATION:
-		notif = (void *)pkt->data;
-		IWL_DEBUG_SCAN(mvm, "Scan aborted: status 0x%x\n",
-			       notif->status);
-		return true;
-
-	default:
-		WARN_ON(1);
-		return false;
-	};
-}
-
-static int iwl_mvm_cancel_regular_scan(struct iwl_mvm *mvm)
-{
-	struct iwl_notification_wait wait_scan_abort;
-	static const u8 scan_abort_notif[] = { SCAN_ABORT_CMD,
-					       SCAN_COMPLETE_NOTIFICATION };
-	int ret;
-
-	iwl_init_notification_wait(&mvm->notif_wait, &wait_scan_abort,
-				   scan_abort_notif,
-				   ARRAY_SIZE(scan_abort_notif),
-				   iwl_mvm_scan_abort_notif, NULL);
-
-	ret = iwl_mvm_send_cmd_pdu(mvm, SCAN_ABORT_CMD, 0, 0, NULL);
-	if (ret) {
-		IWL_ERR(mvm, "Couldn't send SCAN_ABORT_CMD: %d\n", ret);
-		/* mac80211's state will be cleaned in the nic_restart flow */
-		goto out_remove_notif;
-	}
-
-	return iwl_wait_notification(&mvm->notif_wait, &wait_scan_abort, HZ);
-
-out_remove_notif:
-	iwl_remove_notification(&mvm->notif_wait, &wait_scan_abort);
-	return ret;
-}
-
 int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
 					   struct iwl_rx_cmd_buffer *rxb,
 					   struct iwl_device_cmd *cmd)
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	u8 status, ebs_status;
-
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN) {
-		struct iwl_periodic_scan_complete *scan_notif;
+	struct iwl_periodic_scan_complete *scan_notif;
 
-		scan_notif = (void *)pkt->data;
-		status = scan_notif->status;
-		ebs_status = scan_notif->ebs_status;
-	} else  {
-		struct iwl_scan_offload_complete *scan_notif;
+	scan_notif = (void *)pkt->data;
 
-		scan_notif = (void *)pkt->data;
-		status = scan_notif->status;
-		ebs_status = scan_notif->ebs_status;
-	}
 	/* scan status must be locked for proper checking */
 	lockdep_assert_held(&mvm->mutex);
 
@@ -687,9 +353,9 @@ int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
 		       "%s completed, status %s, EBS status %s\n",
 		       mvm->scan_status == IWL_MVM_SCAN_SCHED ?
 				"Scheduled scan" : "Scan",
-		       status == IWL_SCAN_OFFLOAD_COMPLETED ?
+		       scan_notif->status == IWL_SCAN_OFFLOAD_COMPLETED ?
 				"completed" : "aborted",
-		       ebs_status == IWL_SCAN_EBS_SUCCESS ?
+		       scan_notif->ebs_status == IWL_SCAN_EBS_SUCCESS ?
 				"success" : "failed");
 
 
@@ -700,64 +366,16 @@ int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
 	} else if (mvm->scan_status == IWL_MVM_SCAN_OS) {
 		mvm->scan_status = IWL_MVM_SCAN_NONE;
 		ieee80211_scan_completed(mvm->hw,
-					 status == IWL_SCAN_OFFLOAD_ABORTED);
+				scan_notif->status == IWL_SCAN_OFFLOAD_ABORTED);
 		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
 	}
 
-	if (ebs_status)
+	if (scan_notif->ebs_status)
 		mvm->last_ebs_successful = false;
 
 	return 0;
 }
 
-static void iwl_scan_offload_build_tx_cmd(struct iwl_mvm *mvm,
-					  struct ieee80211_vif *vif,
-					  struct ieee80211_scan_ies *ies,
-					  enum ieee80211_band band,
-					  struct iwl_tx_cmd *cmd,
-					  u8 *data)
-{
-	u16 cmd_len;
-
-	cmd->tx_flags = cpu_to_le32(TX_CMD_FLG_SEQ_CTL);
-	cmd->life_time = cpu_to_le32(TX_CMD_LIFE_TIME_INFINITE);
-	cmd->sta_id = mvm->aux_sta.sta_id;
-
-	cmd->rate_n_flags = iwl_mvm_scan_rate_n_flags(mvm, band, false);
-
-	cmd_len = iwl_mvm_fill_probe_req((struct ieee80211_mgmt *)data,
-					 vif->addr,
-					 1, NULL, 0,
-					 ies->ies[band], ies->len[band],
-					 ies->common_ies, ies->common_ie_len,
-					 SCAN_OFFLOAD_PROBE_REQ_SIZE);
-	cmd->len = cpu_to_le16(cmd_len);
-}
-
-static void iwl_build_scan_cmd(struct iwl_mvm *mvm,
-			       struct ieee80211_vif *vif,
-			       struct cfg80211_sched_scan_request *req,
-			       struct iwl_scan_offload_cmd *scan,
-			       struct iwl_mvm_scan_params *params)
-{
-	scan->channel_count = req->n_channels;
-	scan->quiet_time = cpu_to_le16(IWL_ACTIVE_QUIET_TIME);
-	scan->quiet_plcp_th = cpu_to_le16(IWL_PLCP_QUIET_THRESH);
-	scan->good_CRC_th = IWL_GOOD_CRC_TH_DEFAULT;
-	scan->rx_chain = iwl_mvm_scan_rx_chain(mvm);
-
-	scan->max_out_time = cpu_to_le32(params->max_out_time);
-	scan->suspend_time = cpu_to_le32(params->suspend_time);
-
-	scan->filter_flags |= cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
-					  MAC_FILTER_IN_BEACON);
-	scan->scan_type = cpu_to_le32(SCAN_TYPE_BACKGROUND);
-	scan->rep_count = cpu_to_le32(1);
-
-	if (params->passive_fragmented)
-		scan->scan_flags |= SCAN_FLAGS_FRAGMENTED_SCAN;
-}
-
 static int iwl_ssid_exist(u8 *ssid, u8 ssid_len, struct iwl_ssid_ie *ssid_list)
 {
 	int i;
@@ -815,127 +433,6 @@ static void iwl_scan_offload_build_ssid(struct cfg80211_sched_scan_request *req,
 	}
 }
 
-static void iwl_build_channel_cfg(struct iwl_mvm *mvm,
-				  struct cfg80211_sched_scan_request *req,
-				  u8 *channels_buffer,
-				  enum ieee80211_band band,
-				  int *head,
-				  u32 ssid_bitmap,
-				  struct iwl_mvm_scan_params *params)
-{
-	u32 n_channels = mvm->fw->ucode_capa.n_scan_channels;
-	__le32 *type = (__le32 *)channels_buffer;
-	__le16 *channel_number = (__le16 *)(type + n_channels);
-	__le16 *iter_count = channel_number + n_channels;
-	__le32 *iter_interval = (__le32 *)(iter_count + n_channels);
-	u8 *active_dwell = (u8 *)(iter_interval + n_channels);
-	u8 *passive_dwell = active_dwell + n_channels;
-	int i, index = 0;
-
-	for (i = 0; i < req->n_channels; i++) {
-		struct ieee80211_channel *chan = req->channels[i];
-
-		if (chan->band != band)
-			continue;
-
-		index = *head;
-		(*head)++;
-
-		channel_number[index] = cpu_to_le16(chan->hw_value);
-		active_dwell[index] = params->dwell[band].active;
-		passive_dwell[index] = params->dwell[band].passive;
-
-		iter_count[index] = cpu_to_le16(1);
-		iter_interval[index] = 0;
-
-		if (!(chan->flags & IEEE80211_CHAN_NO_IR))
-			type[index] |=
-				cpu_to_le32(IWL_SCAN_OFFLOAD_CHANNEL_ACTIVE);
-
-		type[index] |= cpu_to_le32(IWL_SCAN_OFFLOAD_CHANNEL_FULL |
-					   IWL_SCAN_OFFLOAD_CHANNEL_PARTIAL);
-
-		if (chan->flags & IEEE80211_CHAN_NO_HT40)
-			type[index] |=
-				cpu_to_le32(IWL_SCAN_OFFLOAD_CHANNEL_NARROW);
-
-		/* scan for all SSIDs from req->ssids */
-		type[index] |= cpu_to_le32(ssid_bitmap);
-	}
-}
-
-int iwl_mvm_config_sched_scan(struct iwl_mvm *mvm,
-			      struct ieee80211_vif *vif,
-			      struct cfg80211_sched_scan_request *req,
-			      struct ieee80211_scan_ies *ies)
-{
-	int band_2ghz = mvm->nvm_data->bands[IEEE80211_BAND_2GHZ].n_channels;
-	int band_5ghz = mvm->nvm_data->bands[IEEE80211_BAND_5GHZ].n_channels;
-	int head = 0;
-	u32 ssid_bitmap;
-	int cmd_len;
-	int ret;
-	u8 *probes;
-	bool basic_ssid = !(mvm->fw->ucode_capa.flags &
-			    IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID);
-
-	struct iwl_scan_offload_cfg *scan_cfg;
-	struct iwl_host_cmd cmd = {
-		.id = SCAN_OFFLOAD_CONFIG_CMD,
-	};
-	struct iwl_mvm_scan_params params = {};
-
-	lockdep_assert_held(&mvm->mutex);
-
-	cmd_len = sizeof(struct iwl_scan_offload_cfg) +
-		  mvm->fw->ucode_capa.n_scan_channels * IWL_SCAN_CHAN_SIZE +
-		  2 * SCAN_OFFLOAD_PROBE_REQ_SIZE;
-
-	scan_cfg = kzalloc(cmd_len, GFP_KERNEL);
-	if (!scan_cfg)
-		return -ENOMEM;
-
-	probes = scan_cfg->data +
-		mvm->fw->ucode_capa.n_scan_channels * IWL_SCAN_CHAN_SIZE;
-
-	iwl_mvm_scan_calc_params(mvm, vif, req->n_ssids, 0, &params);
-	iwl_build_scan_cmd(mvm, vif, req, &scan_cfg->scan_cmd, &params);
-	scan_cfg->scan_cmd.len = cpu_to_le16(cmd_len);
-
-	iwl_scan_offload_build_ssid(req, scan_cfg->scan_cmd.direct_scan,
-				    &ssid_bitmap, basic_ssid);
-	/* build tx frames for supported bands */
-	if (band_2ghz) {
-		iwl_scan_offload_build_tx_cmd(mvm, vif, ies,
-					      IEEE80211_BAND_2GHZ,
-					      &scan_cfg->scan_cmd.tx_cmd[0],
-					      probes);
-		iwl_build_channel_cfg(mvm, req, scan_cfg->data,
-				      IEEE80211_BAND_2GHZ, &head,
-				      ssid_bitmap, &params);
-	}
-	if (band_5ghz) {
-		iwl_scan_offload_build_tx_cmd(mvm, vif, ies,
-					      IEEE80211_BAND_5GHZ,
-					      &scan_cfg->scan_cmd.tx_cmd[1],
-					      probes +
-						SCAN_OFFLOAD_PROBE_REQ_SIZE);
-		iwl_build_channel_cfg(mvm, req, scan_cfg->data,
-				      IEEE80211_BAND_5GHZ, &head,
-				      ssid_bitmap, &params);
-	}
-
-	cmd.data[0] = scan_cfg;
-	cmd.len[0] = cmd_len;
-	cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
-
-	IWL_DEBUG_SCAN(mvm, "Sending scheduled scan config\n");
-
-	ret = iwl_mvm_send_cmd(mvm, &cmd);
-	kfree(scan_cfg);
-	return ret;
-}
-
 int iwl_mvm_config_sched_scan_profiles(struct iwl_mvm *mvm,
 				       struct cfg80211_sched_scan_request *req)
 {
@@ -1018,33 +515,6 @@ static bool iwl_mvm_scan_pass_all(struct iwl_mvm *mvm,
 	return true;
 }
 
-int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
-			     struct cfg80211_sched_scan_request *req)
-{
-	struct iwl_scan_offload_req scan_req = {
-		.watchdog = IWL_SCHED_SCAN_WATCHDOG,
-
-		.schedule_line[0].iterations = IWL_FAST_SCHED_SCAN_ITERATIONS,
-		.schedule_line[0].delay = cpu_to_le16(req->interval / 1000),
-		.schedule_line[0].full_scan_mul = 1,
-
-		.schedule_line[1].iterations = 0xff,
-		.schedule_line[1].delay = cpu_to_le16(req->interval / 1000),
-		.schedule_line[1].full_scan_mul = IWL_FULL_SCAN_MULTIPLIER,
-	};
-
-	if (iwl_mvm_scan_pass_all(mvm, req))
-		scan_req.flags |= cpu_to_le16(IWL_SCAN_OFFLOAD_FLAG_PASS_ALL);
-
-	if (mvm->last_ebs_successful &&
-	    mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_EBS_SUPPORT)
-		scan_req.flags |=
-			cpu_to_le16(IWL_SCAN_OFFLOAD_FLAG_EBS_ACCURATE_MODE);
-
-	return iwl_mvm_send_cmd_pdu(mvm, SCAN_OFFLOAD_REQUEST_CMD, 0,
-				    sizeof(scan_req), &scan_req);
-}
-
 int iwl_mvm_scan_offload_start(struct iwl_mvm *mvm,
 			       struct ieee80211_vif *vif,
 			       struct cfg80211_sched_scan_request *req,
@@ -1057,21 +527,12 @@ int iwl_mvm_scan_offload_start(struct iwl_mvm *mvm,
 		if (ret)
 			return ret;
 		ret = iwl_mvm_sched_scan_umac(mvm, vif, req, ies);
-	} else if ((mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN)) {
-		mvm->scan_status = IWL_MVM_SCAN_SCHED;
-		ret = iwl_mvm_config_sched_scan_profiles(mvm, req);
-		if (ret)
-			return ret;
-		ret = iwl_mvm_unified_sched_scan_lmac(mvm, vif, req, ies);
 	} else {
 		mvm->scan_status = IWL_MVM_SCAN_SCHED;
-		ret = iwl_mvm_config_sched_scan(mvm, vif, req, ies);
-		if (ret)
-			return ret;
 		ret = iwl_mvm_config_sched_scan_profiles(mvm, req);
 		if (ret)
 			return ret;
-		ret = iwl_mvm_sched_scan_start(mvm, req);
+		ret = iwl_mvm_unified_sched_scan_lmac(mvm, vif, req, ies);
 	}
 
 	return ret;
@@ -1088,9 +549,7 @@ static int iwl_mvm_send_scan_offload_abort(struct iwl_mvm *mvm)
 	/* Exit instantly with error when device is not ready
 	 * to receive scan abort command or it does not perform
 	 * scheduled scan currently */
-	if (mvm->scan_status != IWL_MVM_SCAN_SCHED &&
-	    (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN) ||
-	     mvm->scan_status != IWL_MVM_SCAN_OS))
+	if (mvm->scan_status == IWL_MVM_SCAN_NONE)
 		return -EIO;
 
 	ret = iwl_mvm_send_cmd_status(mvm, &cmd, &status);
@@ -1128,14 +587,9 @@ 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) ||
-	     mvm->scan_status != IWL_MVM_SCAN_OS)) {
-		IWL_DEBUG_SCAN(mvm, "No scan to stop\n");
-		return 0;
 	}
 
 	iwl_init_notification_wait(&mvm->notif_wait, &wait_scan_done,
@@ -1148,16 +602,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 +619,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 +628,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,
@@ -1317,7 +768,7 @@ iwl_mvm_build_generic_unified_scan_cmd(struct iwl_mvm *mvm,
 	cmd->passive_dwell = params->dwell[IEEE80211_BAND_2GHZ].passive;
 	if (params->passive_fragmented)
 		cmd->fragmented_dwell =
-				params->dwell[IEEE80211_BAND_2GHZ].passive;
+				params->dwell[IEEE80211_BAND_2GHZ].fragmented;
 	cmd->rx_chain_select = iwl_mvm_scan_rx_chain(mvm);
 	cmd->max_out_time = cpu_to_le32(params->max_out_time);
 	cmd->suspend_time = cpu_to_le32(params->suspend_time);
@@ -1484,6 +935,8 @@ int iwl_mvm_unified_sched_scan_lmac(struct iwl_mvm *mvm,
 
 	cmd->n_channels = (u8)req->n_channels;
 
+	cmd->delay = cpu_to_le32(req->delay);
+
 	if (iwl_mvm_scan_pass_all(mvm, req))
 		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASS_ALL;
 	else
@@ -1580,9 +1033,7 @@ int iwl_mvm_cancel_scan(struct iwl_mvm *mvm)
 		return 0;
 	}
 
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN)
-		return iwl_mvm_scan_offload_stop(mvm, true);
-	return iwl_mvm_cancel_regular_scan(mvm);
+	return iwl_mvm_scan_offload_stop(mvm, true);
 }
 
 /* UMAC scan API */
@@ -1728,6 +1179,18 @@ static bool iwl_mvm_find_scan_type(struct iwl_mvm *mvm,
 	return false;
 }
 
+static int iwl_mvm_find_first_scan(struct iwl_mvm *mvm,
+				   enum iwl_umac_scan_uid_type type)
+{
+	int i;
+
+	for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++)
+		if (mvm->scan_uid[i] & type)
+			return i;
+
+	return i;
+}
+
 static u32 iwl_generate_scan_uid(struct iwl_mvm *mvm,
 				 enum iwl_umac_scan_uid_type type)
 {
@@ -1765,7 +1228,7 @@ iwl_mvm_build_generic_umac_scan_cmd(struct iwl_mvm *mvm,
 	cmd->passive_dwell = params->dwell[IEEE80211_BAND_2GHZ].passive;
 	if (params->passive_fragmented)
 		cmd->fragmented_dwell =
-				params->dwell[IEEE80211_BAND_2GHZ].passive;
+				params->dwell[IEEE80211_BAND_2GHZ].fragmented;
 	cmd->max_out_time = cpu_to_le32(params->max_out_time);
 	cmd->suspend_time = cpu_to_le32(params->suspend_time);
 	cmd->scan_priority = cpu_to_le32(IWL_SCAN_PRIORITY_HIGH);
@@ -1987,7 +1450,13 @@ int iwl_mvm_sched_scan_umac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 				cpu_to_le16(req->interval / MSEC_PER_SEC);
 	sec_part->schedule[0].iter_count = 0xff;
 
-	sec_part->delay = 0;
+	if (req->delay > U16_MAX) {
+		IWL_DEBUG_SCAN(mvm,
+			       "delay value is > 16-bits, set to max possible\n");
+		sec_part->delay = cpu_to_le16(U16_MAX);
+	} else {
+		sec_part->delay = cpu_to_le16(req->delay);
+	}
 
 	iwl_mvm_build_unified_scan_probe(mvm, vif, ies, &sec_part->preq,
 		req->flags & NL80211_SCAN_FLAG_RANDOM_ADDR ?
@@ -2159,14 +1628,59 @@ int iwl_mvm_scan_size(struct iwl_mvm *mvm)
 				mvm->fw->ucode_capa.n_scan_channels +
 			sizeof(struct iwl_scan_req_umac_tail);
 
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN)
-		return sizeof(struct iwl_scan_req_unified_lmac) +
-			sizeof(struct iwl_scan_channel_cfg_lmac) *
-				mvm->fw->ucode_capa.n_scan_channels +
-			sizeof(struct iwl_scan_probe_req);
+	return sizeof(struct iwl_scan_req_unified_lmac) +
+		sizeof(struct iwl_scan_channel_cfg_lmac) *
+		mvm->fw->ucode_capa.n_scan_channels +
+		sizeof(struct iwl_scan_probe_req);
+}
+
+/*
+ * This function is used in nic restart flow, to inform mac80211 about scans
+ * that was aborted by restart flow or by an assert.
+ */
+void iwl_mvm_report_scan_aborted(struct iwl_mvm *mvm)
+{
+	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
+		u32 uid, i;
 
-	return sizeof(struct iwl_scan_cmd) +
-		mvm->fw->ucode_capa.max_probe_length +
-			mvm->fw->ucode_capa.n_scan_channels *
-		sizeof(struct iwl_scan_channel);
+		uid = iwl_mvm_find_first_scan(mvm, IWL_UMAC_SCAN_UID_REG_SCAN);
+		if (uid < IWL_MVM_MAX_SIMULTANEOUS_SCANS) {
+			ieee80211_scan_completed(mvm->hw, true);
+			mvm->scan_uid[uid] = 0;
+		}
+		uid = iwl_mvm_find_first_scan(mvm,
+					      IWL_UMAC_SCAN_UID_SCHED_SCAN);
+		if (uid < IWL_MVM_MAX_SIMULTANEOUS_SCANS && !mvm->restart_fw) {
+			ieee80211_sched_scan_stopped(mvm->hw);
+			mvm->scan_uid[uid] = 0;
+		}
+
+		/* We shouldn't have any UIDs still set.  Loop over all the
+		 * UIDs to make sure there's nothing left there and warn if
+		 * any is found.
+		 */
+		for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++) {
+			if (WARN_ONCE(mvm->scan_uid[i],
+				      "UMAC scan UID %d was not cleaned\n",
+				      mvm->scan_uid[i]))
+				mvm->scan_uid[i] = 0;
+		}
+	} else {
+		switch (mvm->scan_status) {
+		case IWL_MVM_SCAN_NONE:
+			break;
+		case IWL_MVM_SCAN_OS:
+			ieee80211_scan_completed(mvm->hw, true);
+			break;
+		case IWL_MVM_SCAN_SCHED:
+			/*
+			 * Sched scan will be restarted by mac80211 in
+			 * restart_hw, so do not report if FW is about to be
+			 * restarted.
+			 */
+			if (!mvm->restart_fw)
+				ieee80211_sched_scan_stopped(mvm->hw);
+			break;
+		}
+	}
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/sf.c b/drivers/net/wireless/iwlwifi/mvm/sf.c
index 7eb78e2c240a..b0f59fdd287c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sf.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sf.c
@@ -99,7 +99,35 @@ static void iwl_mvm_bound_iface_iterator(void *_data, u8 *mac,
 
 /*
  * Aging and idle timeouts for the different possible scenarios
- * in SF_FULL_ON state.
+ * in default configuration
+ */
+static const
+__le32 sf_full_timeout_def[SF_NUM_SCENARIO][SF_NUM_TIMEOUT_TYPES] = {
+	{
+		cpu_to_le32(SF_SINGLE_UNICAST_AGING_TIMER_DEF),
+		cpu_to_le32(SF_SINGLE_UNICAST_IDLE_TIMER_DEF)
+	},
+	{
+		cpu_to_le32(SF_AGG_UNICAST_AGING_TIMER_DEF),
+		cpu_to_le32(SF_AGG_UNICAST_IDLE_TIMER_DEF)
+	},
+	{
+		cpu_to_le32(SF_MCAST_AGING_TIMER_DEF),
+		cpu_to_le32(SF_MCAST_IDLE_TIMER_DEF)
+	},
+	{
+		cpu_to_le32(SF_BA_AGING_TIMER_DEF),
+		cpu_to_le32(SF_BA_IDLE_TIMER_DEF)
+	},
+	{
+		cpu_to_le32(SF_TX_RE_AGING_TIMER_DEF),
+		cpu_to_le32(SF_TX_RE_IDLE_TIMER_DEF)
+	},
+};
+
+/*
+ * Aging and idle timeouts for the different possible scenarios
+ * in single BSS MAC configuration.
  */
 static const __le32 sf_full_timeout[SF_NUM_SCENARIO][SF_NUM_TIMEOUT_TYPES] = {
 	{
@@ -124,7 +152,8 @@ static const __le32 sf_full_timeout[SF_NUM_SCENARIO][SF_NUM_TIMEOUT_TYPES] = {
 	},
 };
 
-static void iwl_mvm_fill_sf_command(struct iwl_sf_cfg_cmd *sf_cmd,
+static void iwl_mvm_fill_sf_command(struct iwl_mvm *mvm,
+				    struct iwl_sf_cfg_cmd *sf_cmd,
 				    struct ieee80211_sta *sta)
 {
 	int i, j, watermark;
@@ -163,24 +192,38 @@ static void iwl_mvm_fill_sf_command(struct iwl_sf_cfg_cmd *sf_cmd,
 					cpu_to_le32(SF_LONG_DELAY_AGING_TIMER);
 		}
 	}
-	BUILD_BUG_ON(sizeof(sf_full_timeout) !=
-		     sizeof(__le32) * SF_NUM_SCENARIO * SF_NUM_TIMEOUT_TYPES);
 
-	memcpy(sf_cmd->full_on_timeouts, sf_full_timeout,
-	       sizeof(sf_full_timeout));
+	if (sta || IWL_UCODE_API(mvm->fw->ucode_ver) < 13) {
+		BUILD_BUG_ON(sizeof(sf_full_timeout) !=
+			     sizeof(__le32) * SF_NUM_SCENARIO *
+			     SF_NUM_TIMEOUT_TYPES);
+
+		memcpy(sf_cmd->full_on_timeouts, sf_full_timeout,
+		       sizeof(sf_full_timeout));
+	} else {
+		BUILD_BUG_ON(sizeof(sf_full_timeout_def) !=
+			     sizeof(__le32) * SF_NUM_SCENARIO *
+			     SF_NUM_TIMEOUT_TYPES);
+
+		memcpy(sf_cmd->full_on_timeouts, sf_full_timeout_def,
+		       sizeof(sf_full_timeout_def));
+	}
+
 }
 
 static int iwl_mvm_sf_config(struct iwl_mvm *mvm, u8 sta_id,
 			     enum iwl_sf_state new_state)
 {
 	struct iwl_sf_cfg_cmd sf_cmd = {
-		.state = cpu_to_le32(new_state),
+		.state = cpu_to_le32(SF_FULL_ON),
 	};
 	struct ieee80211_sta *sta;
 	int ret = 0;
 
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_SF_NO_DUMMY_NOTIF &&
-	    mvm->cfg->disable_dummy_notification)
+	if (IWL_UCODE_API(mvm->fw->ucode_ver) < 13)
+		sf_cmd.state = cpu_to_le32(new_state);
+
+	if (mvm->cfg->disable_dummy_notification)
 		sf_cmd.state |= cpu_to_le32(SF_CFG_DUMMY_NOTIF_OFF);
 
 	/*
@@ -192,6 +235,8 @@ static int iwl_mvm_sf_config(struct iwl_mvm *mvm, u8 sta_id,
 
 	switch (new_state) {
 	case SF_UNINIT:
+		if (IWL_UCODE_API(mvm->fw->ucode_ver) >= 13)
+			iwl_mvm_fill_sf_command(mvm, &sf_cmd, NULL);
 		break;
 	case SF_FULL_ON:
 		if (sta_id == IWL_MVM_STATION_COUNT) {
@@ -206,11 +251,11 @@ static int iwl_mvm_sf_config(struct iwl_mvm *mvm, u8 sta_id,
 			rcu_read_unlock();
 			return -EINVAL;
 		}
-		iwl_mvm_fill_sf_command(&sf_cmd, sta);
+		iwl_mvm_fill_sf_command(mvm, &sf_cmd, sta);
 		rcu_read_unlock();
 		break;
 	case SF_INIT_OFF:
-		iwl_mvm_fill_sf_command(&sf_cmd, NULL);
+		iwl_mvm_fill_sf_command(mvm, &sf_cmd, NULL);
 		break;
 	default:
 		WARN_ONCE(1, "Invalid state: %d. not sending Smart Fifo cmd\n",
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index 5c23cddaaae3..1845b79487c8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -209,9 +209,8 @@ static int iwl_mvm_tdls_sta_init(struct iwl_mvm *mvm,
 {
 	unsigned long used_hw_queues;
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
-	unsigned int wdg_timeout = iwlmvm_mod_params.tfd_q_hang_detect ?
-					mvm->cfg->base_params->wd_timeout :
-					IWL_WATCHDOG_DISABLED;
+	unsigned int wdg_timeout =
+		iwl_mvm_get_wd_timeout(mvm, NULL, true, false);
 	u32 ac;
 
 	lockdep_assert_held(&mvm->mutex);
@@ -273,7 +272,7 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 	else
 		sta_id = mvm_sta->sta_id;
 
-	if (WARN_ON_ONCE(sta_id == IWL_MVM_STATION_COUNT))
+	if (sta_id == IWL_MVM_STATION_COUNT)
 		return -ENOSPC;
 
 	spin_lock_init(&mvm_sta->lock);
@@ -491,8 +490,18 @@ int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 
 	if (vif->type == NL80211_IFTYPE_STATION &&
 	    mvmvif->ap_sta_id == mvm_sta->sta_id) {
+		ret = iwl_mvm_drain_sta(mvm, mvm_sta, true);
+		if (ret)
+			return ret;
 		/* flush its queues here since we are freeing mvm_sta */
 		ret = iwl_mvm_flush_tx_path(mvm, mvm_sta->tfd_queue_msk, true);
+		if (ret)
+			return ret;
+		ret = iwl_trans_wait_tx_queue_empty(mvm->trans,
+						    mvm_sta->tfd_queue_msk);
+		if (ret)
+			return ret;
+		ret = iwl_mvm_drain_sta(mvm, mvm_sta, false);
 
 		/* if we are associated - we can't remove the AP STA now */
 		if (vif->bss_conf.assoc)
@@ -971,9 +980,8 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 {
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
-	unsigned int wdg_timeout = iwlmvm_mod_params.tfd_q_hang_detect ?
-					mvm->cfg->base_params->wd_timeout :
-					IWL_WATCHDOG_DISABLED;
+	unsigned int wdg_timeout =
+		iwl_mvm_get_wd_timeout(mvm, vif, sta->tdls, false);
 	int queue, fifo, ret;
 	u16 ssn;
 
@@ -1120,8 +1128,12 @@ int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	spin_unlock_bh(&mvmsta->lock);
 
 	if (old_state >= IWL_AGG_ON) {
+		iwl_mvm_drain_sta(mvm, mvmsta, true);
 		if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id), true))
 			IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
+		iwl_trans_wait_tx_queue_empty(mvm->trans,
+					      mvmsta->tfd_queue_msk);
+		iwl_mvm_drain_sta(mvm, mvmsta, false);
 
 		iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
 
@@ -1681,9 +1693,6 @@ void iwl_mvm_sta_modify_disable_tx(struct iwl_mvm *mvm,
 	};
 	int ret;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_DISABLE_STA_TX))
-		return;
-
 	ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
 	if (ret)
 		IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
@@ -1705,8 +1714,8 @@ void iwl_mvm_sta_modify_disable_tx_ap(struct iwl_mvm *mvm,
 	mvm_sta->disable_tx = disable;
 
 	/*
-	 * Tell mac80211 to start/stop queueing tx for this station,
-	 * but don't stop queueing if there are still pending frames
+	 * Tell mac80211 to start/stop queuing tx for this station,
+	 * but don't stop queuing if there are still pending frames
 	 * for this station.
 	 */
 	if (disable || !atomic_read(&mvm->pending_frames[mvm_sta->sta_id]))
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.h b/drivers/net/wireless/iwlwifi/mvm/sta.h
index d8f48975ad08..748f5dc3f9f4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.h
@@ -150,7 +150,7 @@ struct iwl_mvm_vif;
  * DOC: station table - AP Station in STA mode
  *
  * %iwl_mvm_vif includes the index of the AP station in the fw's STA table:
- * %ap_sta_id. To get the point to the coresponsding %ieee80211_sta,
+ * %ap_sta_id. To get the point to the corresponding %ieee80211_sta,
  * &fw_id_to_mac_id can be used. Due to the way the fw works, we must not remove
  * the AP station from the fw before setting the MAC context as unassociated.
  * Hence, %fw_id_to_mac_id[%ap_sta_id] will be NULLed when the AP station is
@@ -209,14 +209,14 @@ struct iwl_mvm_vif;
  * When a trigger frame is received, mac80211 tells the driver to send frames
  * from the AMPDU queues or sends frames to non-aggregation queues itself,
  * depending on which ACs are delivery-enabled and what TID has frames to
- * transmit. Note that mac80211 has all the knowledege since all the non-agg
+ * transmit. Note that mac80211 has all the knowledge since all the non-agg
  * frames are buffered / filtered, and the driver tells mac80211 about agg
  * frames). The driver needs to tell the fw to let frames out even if the
  * station is asleep. This is done by %iwl_mvm_sta_modify_sleep_tx_count.
  *
  * When we receive a frame from that station with PM bit unset, the driver
  * needs to let the fw know that this station isn't asleep any more. This is
- * done by %iwl_mvm_sta_modify_ps_wake in response to mac80211 signalling the
+ * done by %iwl_mvm_sta_modify_ps_wake in response to mac80211 signaling the
  * station's wakeup.
  *
  * For a GO, the Service Period might be cut short due to an absence period
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/iwlwifi/mvm/time-event.c
index 54fafbf9a711..fd7b0d36f9a6 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.c
@@ -119,7 +119,7 @@ void iwl_mvm_roc_done_wk(struct work_struct *wk)
 
 	/*
 	 * Flush the offchannel queue -- this is called when the time
-	 * event finishes or is cancelled, so that frames queued for it
+	 * event finishes or is canceled, so that frames queued for it
 	 * won't get stuck on the queue and be transmitted in the next
 	 * time event.
 	 * We have to send the command asynchronously since this cannot
@@ -187,7 +187,8 @@ static bool iwl_mvm_te_check_disconnect(struct iwl_mvm *mvm,
 		return false;
 	if (errmsg)
 		IWL_ERR(mvm, "%s\n", errmsg);
-	ieee80211_connection_loss(vif);
+
+	iwl_mvm_connection_loss(mvm, vif, errmsg);
 	return true;
 }
 
@@ -196,17 +197,24 @@ iwl_mvm_te_handle_notify_csa(struct iwl_mvm *mvm,
 			     struct iwl_mvm_time_event_data *te_data,
 			     struct iwl_time_event_notif *notif)
 {
-	if (!le32_to_cpu(notif->status)) {
+	struct ieee80211_vif *vif = te_data->vif;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+	if (!notif->status)
 		IWL_DEBUG_TE(mvm, "CSA time event failed to start\n");
-		iwl_mvm_te_clear_data(mvm, te_data);
-		return;
-	}
 
 	switch (te_data->vif->type) {
 	case NL80211_IFTYPE_AP:
+		if (!notif->status)
+			mvmvif->csa_failed = true;
 		iwl_mvm_csa_noa_start(mvm);
 		break;
 	case NL80211_IFTYPE_STATION:
+		if (!notif->status) {
+			iwl_mvm_connection_loss(mvm, vif,
+						"CSA TE failed to start");
+			break;
+		}
 		iwl_mvm_csa_client_absent(mvm, te_data->vif);
 		ieee80211_chswitch_done(te_data->vif, true);
 		break;
@@ -220,6 +228,44 @@ iwl_mvm_te_handle_notify_csa(struct iwl_mvm *mvm,
 	iwl_mvm_te_clear_data(mvm, te_data);
 }
 
+static void iwl_mvm_te_check_trigger(struct iwl_mvm *mvm,
+				     struct iwl_time_event_notif *notif,
+				     struct iwl_mvm_time_event_data *te_data)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_time_event *te_trig;
+	int i;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_TIME_EVENT))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_TIME_EVENT);
+	te_trig = (void *)trig->data;
+
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, te_data->vif, trig))
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(te_trig->time_events); i++) {
+		u32 trig_te_id = le32_to_cpu(te_trig->time_events[i].id);
+		u32 trig_action_bitmap =
+			le32_to_cpu(te_trig->time_events[i].action_bitmap);
+		u32 trig_status_bitmap =
+			le32_to_cpu(te_trig->time_events[i].status_bitmap);
+
+		if (trig_te_id != te_data->id ||
+		    !(trig_action_bitmap & le32_to_cpu(notif->action)) ||
+		    !(trig_status_bitmap & BIT(le32_to_cpu(notif->status))))
+			continue;
+
+		iwl_mvm_fw_dbg_collect_trig(mvm, trig,
+					    "Time event %d Action 0x%x received status: %d",
+					    te_data->id,
+					    le32_to_cpu(notif->action),
+					    le32_to_cpu(notif->status));
+		break;
+	}
+}
+
 /*
  * Handles a FW notification for an event that is known to the driver.
  *
@@ -237,6 +283,8 @@ static void iwl_mvm_te_handle_notif(struct iwl_mvm *mvm,
 		     le32_to_cpu(notif->unique_id),
 		     le32_to_cpu(notif->action));
 
+	iwl_mvm_te_check_trigger(mvm, notif, te_data);
+
 	/*
 	 * The FW sends the start/end time event notifications even for events
 	 * that it fails to schedule. This is indicated in the status field of
@@ -246,11 +294,16 @@ static void iwl_mvm_te_handle_notif(struct iwl_mvm *mvm,
 	 * events in the system).
 	 */
 	if (!le32_to_cpu(notif->status)) {
-		bool start = le32_to_cpu(notif->action) &
-				TE_V2_NOTIF_HOST_EVENT_START;
-		IWL_WARN(mvm, "Time Event %s notification failure\n",
-			 start ? "start" : "end");
-		if (iwl_mvm_te_check_disconnect(mvm, te_data->vif, NULL)) {
+		const char *msg;
+
+		if (notif->action & cpu_to_le32(TE_V2_NOTIF_HOST_EVENT_START))
+			msg = "Time Event start notification failure";
+		else
+			msg = "Time Event end notification failure";
+
+		IWL_DEBUG_TE(mvm, "%s\n", msg);
+
+		if (iwl_mvm_te_check_disconnect(mvm, te_data->vif, msg)) {
 			iwl_mvm_te_clear_data(mvm, te_data);
 			return;
 		}
@@ -261,17 +314,23 @@ static void iwl_mvm_te_handle_notif(struct iwl_mvm *mvm,
 			     "TE ended - current time %lu, estimated end %lu\n",
 			     jiffies, te_data->end_jiffies);
 
-		if (te_data->vif->type == NL80211_IFTYPE_P2P_DEVICE) {
+		switch (te_data->vif->type) {
+		case NL80211_IFTYPE_P2P_DEVICE:
 			ieee80211_remain_on_channel_expired(mvm->hw);
 			iwl_mvm_roc_finished(mvm);
+			break;
+		case NL80211_IFTYPE_STATION:
+			/*
+			 * By now, we should have finished association
+			 * and know the dtim period.
+			 */
+			iwl_mvm_te_check_disconnect(mvm, te_data->vif,
+				"No association and the time event is over already...");
+			break;
+		default:
+			break;
 		}
 
-		/*
-		 * By now, we should have finished association
-		 * and know the dtim period.
-		 */
-		iwl_mvm_te_check_disconnect(mvm, te_data->vif,
-			"No association and the time event is over already...");
 		iwl_mvm_te_clear_data(mvm, te_data);
 	} else if (le32_to_cpu(notif->action) & TE_V2_NOTIF_HOST_EVENT_START) {
 		te_data->running = true;
@@ -307,6 +366,8 @@ static int iwl_mvm_aux_roc_te_handle_notif(struct iwl_mvm *mvm,
 	if (!aux_roc_te) /* Not a Aux ROC time event */
 		return -EINVAL;
 
+	iwl_mvm_te_check_trigger(mvm, notif, te_data);
+
 	if (!le32_to_cpu(notif->status)) {
 		IWL_DEBUG_TE(mvm,
 			     "ERROR: Aux ROC Time Event %s notification failure\n",
@@ -750,8 +811,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;
@@ -762,14 +822,12 @@ void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
 	 * Iterate over the list of aux roc time events and find the time
 	 * event that is associated with a BSS interface.
 	 * This assumes that a BSS interface can have only a single time
-	 * event at any given time and this time event coresponds to a ROC
+	 * event at any given time and this time event corresponds to a ROC
 	 * 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/time-event.h b/drivers/net/wireless/iwlwifi/mvm/time-event.h
index 6f6b35db3ab8..de4fbc6d57f1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.h
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.h
@@ -147,7 +147,7 @@ void iwl_mvm_protect_session(struct iwl_mvm *mvm,
  * @vif: the virtual interface for which the session is issued
  *
  * This functions cancels the session protection which is an act of good
- * citizenship. If it is not needed any more it should be cancelled because
+ * citizenship. If it is not needed any more it should be canceled because
  * the other bindings wait for the medium during that time.
  * This funtions doesn't sleep.
  */
@@ -162,7 +162,7 @@ int iwl_mvm_rx_time_event_notif(struct iwl_mvm *mvm,
 				struct iwl_device_cmd *cmd);
 
 /**
- * iwl_mvm_start_p2p_roc - start remain on channel for p2p device functionlity
+ * iwl_mvm_start_p2p_roc - start remain on channel for p2p device functionality
  * @mvm: the mvm component
  * @vif: the virtual interface for which the roc is requested. It is assumed
  * that the vif type is NL80211_IFTYPE_P2P_DEVICE
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index 07304e1fd64a..ef32e177f662 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -664,6 +664,8 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 		info->status.rates[0].count = tx_resp->failure_frame + 1;
 		iwl_mvm_hwrate_to_tx_status(le32_to_cpu(tx_resp->initial_rate),
 					    info);
+		info->status.status_driver_data[1] =
+			(void *)(uintptr_t)le32_to_cpu(tx_resp->initial_rate);
 
 		/* Single frame failure in an AMPDU queue => send BAR */
 		if (txq_id >= mvm->first_agg_queue &&
@@ -909,6 +911,8 @@ static void iwl_mvm_tx_info_from_ba_notif(struct ieee80211_tx_info *info,
 	info->status.tx_time = tid_data->tx_time;
 	info->status.status_driver_data[0] =
 		(void *)(uintptr_t)tid_data->reduced_tpc;
+	info->status.status_driver_data[1] =
+		(void *)(uintptr_t)tid_data->rate_n_flags;
 }
 
 int iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
@@ -949,8 +953,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;
 	}
@@ -1043,6 +1049,14 @@ out:
 	return 0;
 }
 
+/*
+ * Note that there are transports that buffer frames before they reach
+ * the firmware. This means that after flush_tx_path is called, the
+ * queue might not be empty. The race-free way to handle this is to:
+ * 1) set the station as draining
+ * 2) flush the Tx path
+ * 3) wait for the transport queues to be empty
+ */
 int iwl_mvm_flush_tx_path(struct iwl_mvm *mvm, u32 tfd_msk, bool sync)
 {
 	int ret;
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index 8decf9953229..bc55a8b82db6 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -122,7 +122,7 @@ int iwl_mvm_send_cmd_pdu(struct iwl_mvm *mvm, u8 id,
 }
 
 /*
- * We assume that the caller set the status to the sucess value
+ * We assume that the caller set the status to the success value
  */
 int iwl_mvm_send_cmd_status(struct iwl_mvm *mvm, struct iwl_host_cmd *cmd,
 			    u32 *status)
@@ -332,7 +332,7 @@ static const char *desc_lookup(u32 num)
  * read with u32-sized accesses, any members with a different size
  * need to be ordered correctly though!
  */
-struct iwl_error_event_table {
+struct iwl_error_event_table_v1 {
 	u32 valid;		/* (nonzero) valid, (0) log is empty */
 	u32 error_id;		/* type of error */
 	u32 pc;			/* program counter */
@@ -377,7 +377,55 @@ struct iwl_error_event_table {
 	u32 u_timestamp;	/* indicate when the date and time of the
 				 * compilation */
 	u32 flow_handler;	/* FH read/write pointers, RX credit */
-} __packed;
+} __packed /* LOG_ERROR_TABLE_API_S_VER_1 */;
+
+struct iwl_error_event_table {
+	u32 valid;		/* (nonzero) valid, (0) log is empty */
+	u32 error_id;		/* type of error */
+	u32 pc;			/* program counter */
+	u32 blink1;		/* branch link */
+	u32 blink2;		/* branch link */
+	u32 ilink1;		/* interrupt link */
+	u32 ilink2;		/* interrupt link */
+	u32 data1;		/* error-specific data */
+	u32 data2;		/* error-specific data */
+	u32 data3;		/* error-specific data */
+	u32 bcon_time;		/* beacon timer */
+	u32 tsf_low;		/* network timestamp function timer */
+	u32 tsf_hi;		/* network timestamp function timer */
+	u32 gp1;		/* GP1 timer register */
+	u32 gp2;		/* GP2 timer register */
+	u32 gp3;		/* GP3 timer register */
+	u32 major;		/* uCode version major */
+	u32 minor;		/* uCode version minor */
+	u32 hw_ver;		/* HW Silicon version */
+	u32 brd_ver;		/* HW board version */
+	u32 log_pc;		/* log program counter */
+	u32 frame_ptr;		/* frame pointer */
+	u32 stack_ptr;		/* stack pointer */
+	u32 hcmd;		/* last host command header */
+	u32 isr0;		/* isr status register LMPM_NIC_ISR0:
+				 * rxtx_flag */
+	u32 isr1;		/* isr status register LMPM_NIC_ISR1:
+				 * host_flag */
+	u32 isr2;		/* isr status register LMPM_NIC_ISR2:
+				 * enc_flag */
+	u32 isr3;		/* isr status register LMPM_NIC_ISR3:
+				 * time_flag */
+	u32 isr4;		/* isr status register LMPM_NIC_ISR4:
+				 * wico interrupt */
+	u32 isr_pref;		/* isr status register LMPM_NIC_PREF_STAT */
+	u32 wait_event;		/* wait event() caller address */
+	u32 l2p_control;	/* L2pControlField */
+	u32 l2p_duration;	/* L2pDurationField */
+	u32 l2p_mhvalid;	/* L2pMhValidBits */
+	u32 l2p_addr_match;	/* L2pAddrMatchStat */
+	u32 lmpm_pmg_sel;	/* indicate which clocks are turned on
+				 * (LMPM_PMG_SEL) */
+	u32 u_timestamp;	/* indicate when the date and time of the
+				 * compilation */
+	u32 flow_handler;	/* FH read/write pointers, RX credit */
+} __packed /* LOG_ERROR_TABLE_API_S_VER_2 */;
 
 /*
  * UMAC error struct - relevant starting from family 8000 chip.
@@ -396,11 +444,11 @@ struct iwl_umac_error_event_table {
 	u32 data1;		/* error-specific data */
 	u32 data2;		/* error-specific data */
 	u32 data3;		/* error-specific data */
-	u32 umac_fw_ver;	/* UMAC version */
-	u32 umac_fw_api_ver;	/* UMAC FW API ver */
+	u32 umac_major;
+	u32 umac_minor;
 	u32 frame_pointer;	/* core register 27*/
 	u32 stack_pointer;	/* core register 28 */
-	u32 cmd_header;	/* latest host cmd sent to UMAC */
+	u32 cmd_header;		/* latest host cmd sent to UMAC */
 	u32 nic_isr_pref;	/* ISR status register */
 } __packed;
 
@@ -441,18 +489,18 @@ static void iwl_mvm_dump_umac_error_log(struct iwl_mvm *mvm)
 	IWL_ERR(mvm, "0x%08X | umac data1\n", table.data1);
 	IWL_ERR(mvm, "0x%08X | umac data2\n", table.data2);
 	IWL_ERR(mvm, "0x%08X | umac data3\n", table.data3);
-	IWL_ERR(mvm, "0x%08X | umac version\n", table.umac_fw_ver);
-	IWL_ERR(mvm, "0x%08X | umac api version\n", table.umac_fw_api_ver);
+	IWL_ERR(mvm, "0x%08X | umac major\n", table.umac_major);
+	IWL_ERR(mvm, "0x%08X | umac minor\n", table.umac_minor);
 	IWL_ERR(mvm, "0x%08X | frame pointer\n", table.frame_pointer);
 	IWL_ERR(mvm, "0x%08X | stack pointer\n", table.stack_pointer);
 	IWL_ERR(mvm, "0x%08X | last host cmd\n", table.cmd_header);
 	IWL_ERR(mvm, "0x%08X | isr status reg\n", table.nic_isr_pref);
 }
 
-void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
+static void iwl_mvm_dump_nic_error_log_old(struct iwl_mvm *mvm)
 {
 	struct iwl_trans *trans = mvm->trans;
-	struct iwl_error_event_table table;
+	struct iwl_error_event_table_v1 table;
 	u32 base;
 
 	base = mvm->error_event_table;
@@ -489,7 +537,7 @@ void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
 				      table.data1, table.data2, table.data3,
 				      table.blink1, table.blink2, table.ilink1,
 				      table.ilink2, table.bcon_time, table.gp1,
-				      table.gp2, table.gp3, table.ucode_ver,
+				      table.gp2, table.gp3, table.ucode_ver, 0,
 				      table.hw_ver, table.brd_ver);
 	IWL_ERR(mvm, "0x%08X | %-28s\n", table.error_id,
 		desc_lookup(table.error_id));
@@ -530,6 +578,92 @@ void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
 		iwl_mvm_dump_umac_error_log(mvm);
 }
 
+void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
+{
+	struct iwl_trans *trans = mvm->trans;
+	struct iwl_error_event_table table;
+	u32 base;
+
+	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_NEW_VERSION)) {
+		iwl_mvm_dump_nic_error_log_old(mvm);
+		return;
+	}
+
+	base = mvm->error_event_table;
+	if (mvm->cur_ucode == IWL_UCODE_INIT) {
+		if (!base)
+			base = mvm->fw->init_errlog_ptr;
+	} else {
+		if (!base)
+			base = mvm->fw->inst_errlog_ptr;
+	}
+
+	if (base < 0x800000) {
+		IWL_ERR(mvm,
+			"Not valid error log pointer 0x%08X for %s uCode\n",
+			base,
+			(mvm->cur_ucode == IWL_UCODE_INIT)
+					? "Init" : "RT");
+		return;
+	}
+
+	iwl_trans_read_mem_bytes(trans, base, &table, sizeof(table));
+
+	if (ERROR_START_OFFSET <= table.valid * ERROR_ELEM_SIZE) {
+		IWL_ERR(trans, "Start IWL Error Log Dump:\n");
+		IWL_ERR(trans, "Status: 0x%08lX, count: %d\n",
+			mvm->status, table.valid);
+	}
+
+	/* Do not change this output - scripts rely on it */
+
+	IWL_ERR(mvm, "Loaded firmware version: %s\n", mvm->fw->fw_version);
+
+	trace_iwlwifi_dev_ucode_error(trans->dev, table.error_id, table.tsf_low,
+				      table.data1, table.data2, table.data3,
+				      table.blink1, table.blink2, table.ilink1,
+				      table.ilink2, table.bcon_time, table.gp1,
+				      table.gp2, table.gp3, table.major,
+				      table.minor, table.hw_ver, table.brd_ver);
+	IWL_ERR(mvm, "0x%08X | %-28s\n", table.error_id,
+		desc_lookup(table.error_id));
+	IWL_ERR(mvm, "0x%08X | uPc\n", table.pc);
+	IWL_ERR(mvm, "0x%08X | branchlink1\n", table.blink1);
+	IWL_ERR(mvm, "0x%08X | branchlink2\n", table.blink2);
+	IWL_ERR(mvm, "0x%08X | interruptlink1\n", table.ilink1);
+	IWL_ERR(mvm, "0x%08X | interruptlink2\n", table.ilink2);
+	IWL_ERR(mvm, "0x%08X | data1\n", table.data1);
+	IWL_ERR(mvm, "0x%08X | data2\n", table.data2);
+	IWL_ERR(mvm, "0x%08X | data3\n", table.data3);
+	IWL_ERR(mvm, "0x%08X | beacon time\n", table.bcon_time);
+	IWL_ERR(mvm, "0x%08X | tsf low\n", table.tsf_low);
+	IWL_ERR(mvm, "0x%08X | tsf hi\n", table.tsf_hi);
+	IWL_ERR(mvm, "0x%08X | time gp1\n", table.gp1);
+	IWL_ERR(mvm, "0x%08X | time gp2\n", table.gp2);
+	IWL_ERR(mvm, "0x%08X | time gp3\n", table.gp3);
+	IWL_ERR(mvm, "0x%08X | uCode version major\n", table.major);
+	IWL_ERR(mvm, "0x%08X | uCode version minor\n", table.minor);
+	IWL_ERR(mvm, "0x%08X | hw version\n", table.hw_ver);
+	IWL_ERR(mvm, "0x%08X | board version\n", table.brd_ver);
+	IWL_ERR(mvm, "0x%08X | hcmd\n", table.hcmd);
+	IWL_ERR(mvm, "0x%08X | isr0\n", table.isr0);
+	IWL_ERR(mvm, "0x%08X | isr1\n", table.isr1);
+	IWL_ERR(mvm, "0x%08X | isr2\n", table.isr2);
+	IWL_ERR(mvm, "0x%08X | isr3\n", table.isr3);
+	IWL_ERR(mvm, "0x%08X | isr4\n", table.isr4);
+	IWL_ERR(mvm, "0x%08X | isr_pref\n", table.isr_pref);
+	IWL_ERR(mvm, "0x%08X | wait_event\n", table.wait_event);
+	IWL_ERR(mvm, "0x%08X | l2p_control\n", table.l2p_control);
+	IWL_ERR(mvm, "0x%08X | l2p_duration\n", table.l2p_duration);
+	IWL_ERR(mvm, "0x%08X | l2p_mhvalid\n", table.l2p_mhvalid);
+	IWL_ERR(mvm, "0x%08X | l2p_addr_match\n", table.l2p_addr_match);
+	IWL_ERR(mvm, "0x%08X | lmpm_pmg_sel\n", table.lmpm_pmg_sel);
+	IWL_ERR(mvm, "0x%08X | timestamp\n", table.u_timestamp);
+	IWL_ERR(mvm, "0x%08X | flow_handler\n", table.flow_handler);
+
+	if (mvm->support_umac_log)
+		iwl_mvm_dump_umac_error_log(mvm);
+}
 void iwl_mvm_enable_txq(struct iwl_mvm *mvm, int queue, u16 ssn,
 			const struct iwl_trans_txq_scd_cfg *cfg,
 			unsigned int wdg_timeout)
@@ -603,7 +737,7 @@ int iwl_mvm_send_lq_cmd(struct iwl_mvm *mvm, struct iwl_lq_cmd *lq, bool init)
 }
 
 /**
- * iwl_mvm_update_smps - Get a requst to change the SMPS mode
+ * iwl_mvm_update_smps - Get a request to change the SMPS mode
  * @req_type: The part of the driver who call for a change.
  * @smps_requests: The request to change the SMPS mode.
  *
@@ -643,6 +777,40 @@ void iwl_mvm_update_smps(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	ieee80211_request_smps(vif, smps_mode);
 }
 
+int iwl_mvm_request_statistics(struct iwl_mvm *mvm, bool clear)
+{
+	struct iwl_statistics_cmd scmd = {
+		.flags = clear ? cpu_to_le32(IWL_STATISTICS_FLG_CLEAR) : 0,
+	};
+	struct iwl_host_cmd cmd = {
+		.id = STATISTICS_CMD,
+		.len[0] = sizeof(scmd),
+		.data[0] = &scmd,
+		.flags = CMD_WANT_SKB,
+	};
+	int ret;
+
+	ret = iwl_mvm_send_cmd(mvm, &cmd);
+	if (ret)
+		return ret;
+
+	iwl_mvm_handle_rx_statistics(mvm, cmd.resp_pkt);
+	iwl_free_resp(&cmd);
+
+	if (clear)
+		iwl_mvm_accu_radio_stats(mvm);
+
+	return 0;
+}
+
+void iwl_mvm_accu_radio_stats(struct iwl_mvm *mvm)
+{
+	mvm->accu_radio_stats.rx_time += mvm->radio_stats.rx_time;
+	mvm->accu_radio_stats.tx_time += mvm->radio_stats.tx_time;
+	mvm->accu_radio_stats.on_time_rf += mvm->radio_stats.on_time_rf;
+	mvm->accu_radio_stats.on_time_scan += mvm->radio_stats.on_time_scan;
+}
+
 static void iwl_mvm_diversity_iter(void *_data, u8 *mac,
 				   struct ieee80211_vif *vif)
 {
@@ -689,7 +857,7 @@ int iwl_mvm_update_low_latency(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	mvmvif->low_latency = value;
 
-	res = iwl_mvm_update_quotas(mvm, NULL);
+	res = iwl_mvm_update_quotas(mvm, false, NULL);
 	if (res)
 		return res;
 
@@ -717,25 +885,6 @@ bool iwl_mvm_low_latency(struct iwl_mvm *mvm)
 	return result;
 }
 
-static void iwl_mvm_idle_iter(void *_data, u8 *mac, struct ieee80211_vif *vif)
-{
-	bool *idle = _data;
-
-	if (!vif->bss_conf.idle)
-		*idle = false;
-}
-
-bool iwl_mvm_is_idle(struct iwl_mvm *mvm)
-{
-	bool idle = true;
-
-	ieee80211_iterate_active_interfaces_atomic(
-			mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
-			iwl_mvm_idle_iter, &idle);
-
-	return idle;
-}
-
 struct iwl_bss_iter_data {
 	struct ieee80211_vif *vif;
 	bool error;
@@ -772,3 +921,71 @@ struct ieee80211_vif *iwl_mvm_get_bss_vif(struct iwl_mvm *mvm)
 
 	return bss_iter_data.vif;
 }
+
+unsigned int iwl_mvm_get_wd_timeout(struct iwl_mvm *mvm,
+				    struct ieee80211_vif *vif,
+				    bool tdls, bool cmd_q)
+{
+	struct iwl_fw_dbg_trigger_tlv *trigger;
+	struct iwl_fw_dbg_trigger_txq_timer *txq_timer;
+	unsigned int default_timeout =
+		cmd_q ? IWL_DEF_WD_TIMEOUT : mvm->cfg->base_params->wd_timeout;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_TXQ_TIMERS))
+		return iwlmvm_mod_params.tfd_q_hang_detect ?
+			default_timeout : IWL_WATCHDOG_DISABLED;
+
+	trigger = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_TXQ_TIMERS);
+	txq_timer = (void *)trigger->data;
+
+	if (tdls)
+		return le32_to_cpu(txq_timer->tdls);
+
+	if (cmd_q)
+		return le32_to_cpu(txq_timer->command_queue);
+
+	if (WARN_ON(!vif))
+		return default_timeout;
+
+	switch (ieee80211_vif_type_p2p(vif)) {
+	case NL80211_IFTYPE_ADHOC:
+		return le32_to_cpu(txq_timer->ibss);
+	case NL80211_IFTYPE_STATION:
+		return le32_to_cpu(txq_timer->bss);
+	case NL80211_IFTYPE_AP:
+		return le32_to_cpu(txq_timer->softap);
+	case NL80211_IFTYPE_P2P_CLIENT:
+		return le32_to_cpu(txq_timer->p2p_client);
+	case NL80211_IFTYPE_P2P_GO:
+		return le32_to_cpu(txq_timer->p2p_go);
+	case NL80211_IFTYPE_P2P_DEVICE:
+		return le32_to_cpu(txq_timer->p2p_device);
+	default:
+		WARN_ON(1);
+		return mvm->cfg->base_params->wd_timeout;
+	}
+}
+
+void iwl_mvm_connection_loss(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			     const char *errmsg)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_mlme *trig_mlme;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_MLME))
+		goto out;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_MLME);
+	trig_mlme = (void *)trig->data;
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trig))
+		goto out;
+
+	if (trig_mlme->stop_connection_loss &&
+	    --trig_mlme->stop_connection_loss)
+		goto out;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trig, "%s", errmsg);
+
+out:
+	ieee80211_connection_loss(vif);
+}
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index dbd6bcf52205..b18569734922 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)},
@@ -413,10 +415,35 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 
 /* 8000 Series */
 	{IWL_PCI_DEVICE(0x24F3, 0x0010, iwl8260_2ac_cfg)},
-	{IWL_PCI_DEVICE(0x24F3, 0x0004, iwl8260_2n_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x1010, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0110, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x1110, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0050, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0250, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x1050, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0150, iwl8260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x24F4, 0x0030, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F4, 0x1130, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F4, 0x1030, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0xC010, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0xD010, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F4, 0xC030, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F4, 0xD030, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0xC050, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0xD050, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x8010, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x9010, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F4, 0x8030, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F4, 0x9030, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x8050, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x9050, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0004, iwl8260_2n_cfg)},
 	{IWL_PCI_DEVICE(0x24F5, 0x0010, iwl4165_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x24F6, 0x0030, iwl4165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0810, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0910, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0850, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0950, iwl8260_2ac_cfg)},
 #endif /* CONFIG_IWLMVM */
 
 	{0}
diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index cae0eb8835ce..01996c9d98a7 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -217,6 +217,8 @@ struct iwl_pcie_txq_scratch_buf {
  * @active: stores if queue is active
  * @ampdu: true if this queue is an ampdu queue for an specific RA/TID
  * @wd_timeout: queue watchdog timeout (jiffies) - per queue
+ * @frozen: tx stuck queue timer is frozen
+ * @frozen_expiry_remainder: remember how long until the timer fires
  *
  * A Tx queue consists of circular buffer of BDs (a.k.a. TFDs, transmit frame
  * descriptors) and required locking structures.
@@ -228,9 +230,11 @@ struct iwl_txq {
 	dma_addr_t scratchbufs_dma;
 	struct iwl_pcie_txq_entry *entries;
 	spinlock_t lock;
+	unsigned long frozen_expiry_remainder;
 	struct timer_list stuck_timer;
 	struct iwl_trans_pcie *trans_pcie;
 	bool need_update;
+	bool frozen;
 	u8 active;
 	bool ampdu;
 	unsigned long wd_timeout;
diff --git a/drivers/net/wireless/iwlwifi/pcie/rx.c b/drivers/net/wireless/iwlwifi/pcie/rx.c
index 7b7e2f223fb2..7ff69c642103 100644
--- a/drivers/net/wireless/iwlwifi/pcie/rx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/rx.c
@@ -600,9 +600,11 @@ static void iwl_pcie_rx_handle_rb(struct iwl_trans *trans,
 		if (pkt->len_n_flags == cpu_to_le32(FH_RSCSR_FRAME_INVALID))
 			break;
 
-		IWL_DEBUG_RX(trans, "cmd at offset %d: %s (0x%.2x)\n",
-			rxcb._offset, get_cmd_string(trans_pcie, pkt->hdr.cmd),
-			pkt->hdr.cmd);
+		IWL_DEBUG_RX(trans,
+			     "cmd at offset %d: %s (0x%.2x, seq 0x%x)\n",
+			     rxcb._offset,
+			     get_cmd_string(trans_pcie, pkt->hdr.cmd),
+			     pkt->hdr.cmd, le16_to_cpu(pkt->hdr.sequence));
 
 		len = iwl_rx_packet_len(pkt);
 		len += sizeof(u32); /* account for status word */
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 69935aa5a1b3..2de8fbfe4edf 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -682,10 +682,51 @@ static int iwl_pcie_load_section(struct iwl_trans *trans, u8 section_num,
 	return ret;
 }
 
-static int iwl_pcie_load_cpu_sections_8000b(struct iwl_trans *trans,
-					    const struct fw_img *image,
-					    int cpu,
-					    int *first_ucode_section)
+/*
+ * Driver Takes the ownership on secure machine before FW load
+ * and prevent race with the BT load.
+ * W/A for ROM bug. (should be remove in the next Si step)
+ */
+static int iwl_pcie_rsa_race_bug_wa(struct iwl_trans *trans)
+{
+	u32 val, loop = 1000;
+
+	/*
+	 * Check the RSA semaphore is accessible.
+	 * If the HW isn't locked and the rsa semaphore isn't accessible,
+	 * we are in trouble.
+	 */
+	val = iwl_read_prph(trans, PREG_AUX_BUS_WPROT_0);
+	if (val & (BIT(1) | BIT(17))) {
+		IWL_INFO(trans,
+			 "can't access the RSA semaphore it is write protected\n");
+		return 0;
+	}
+
+	/* take ownership on the AUX IF */
+	iwl_write_prph(trans, WFPM_CTRL_REG, WFPM_AUX_CTL_AUX_IF_MAC_OWNER_MSK);
+	iwl_write_prph(trans, AUX_MISC_MASTER1_EN, AUX_MISC_MASTER1_EN_SBE_MSK);
+
+	do {
+		iwl_write_prph(trans, AUX_MISC_MASTER1_SMPHR_STATUS, 0x1);
+		val = iwl_read_prph(trans, AUX_MISC_MASTER1_SMPHR_STATUS);
+		if (val == 0x1) {
+			iwl_write_prph(trans, RSA_ENABLE, 0);
+			return 0;
+		}
+
+		udelay(10);
+		loop--;
+	} while (loop > 0);
+
+	IWL_ERR(trans, "Failed to take ownership on secure machine\n");
+	return -EIO;
+}
+
+static int iwl_pcie_load_cpu_sections_8000(struct iwl_trans *trans,
+					   const struct fw_img *image,
+					   int cpu,
+					   int *first_ucode_section)
 {
 	int shift_param;
 	int i, ret = 0, sec_num = 0x1;
@@ -880,64 +921,50 @@ static int iwl_pcie_load_given_ucode(struct iwl_trans *trans,
 	}
 
 	/* release CPU reset */
-	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
-		iwl_write_prph(trans, RELEASE_CPU_RESET, RELEASE_CPU_RESET_BIT);
-	else
-		iwl_write32(trans, CSR_RESET, 0);
+	iwl_write32(trans, CSR_RESET, 0);
 
 	return 0;
 }
 
-static int iwl_pcie_load_given_ucode_8000b(struct iwl_trans *trans,
-					   const struct fw_img *image)
+static int iwl_pcie_load_given_ucode_8000(struct iwl_trans *trans,
+					  const struct fw_img *image)
 {
 	int ret = 0;
 	int first_ucode_section;
-	u32 reg;
 
 	IWL_DEBUG_FW(trans, "working with %s CPU\n",
 		     image->is_dual_cpus ? "Dual" : "Single");
 
+	if (trans->dbg_dest_tlv)
+		iwl_pcie_apply_destination(trans);
+
+	/* TODO: remove in the next Si step */
+	ret = iwl_pcie_rsa_race_bug_wa(trans);
+	if (ret)
+		return ret;
+
 	/* configure the ucode to be ready to get the secured image */
 	/* release CPU reset */
 	iwl_write_prph(trans, RELEASE_CPU_RESET, RELEASE_CPU_RESET_BIT);
 
 	/* load to FW the binary Secured sections of CPU1 */
-	ret = iwl_pcie_load_cpu_sections_8000b(trans, image, 1,
-					       &first_ucode_section);
+	ret = iwl_pcie_load_cpu_sections_8000(trans, image, 1,
+					      &first_ucode_section);
 	if (ret)
 		return ret;
 
 	/* load to FW the binary sections of CPU2 */
-	ret = iwl_pcie_load_cpu_sections_8000b(trans, image, 2,
-					       &first_ucode_section);
+	ret = iwl_pcie_load_cpu_sections_8000(trans, image, 2,
+					      &first_ucode_section);
 	if (ret)
 		return ret;
 
-	if (trans->dbg_dest_tlv)
-		iwl_pcie_apply_destination(trans);
-
-	/* wait for image verification to complete  */
-	ret = iwl_poll_prph_bit(trans, LMPM_SECURE_BOOT_CPU1_STATUS_ADDR_B0,
-				LMPM_SECURE_BOOT_STATUS_SUCCESS,
-				LMPM_SECURE_BOOT_STATUS_SUCCESS,
-				LMPM_SECURE_TIME_OUT);
-	if (ret < 0) {
-		reg = iwl_read_prph(trans,
-				    LMPM_SECURE_BOOT_CPU1_STATUS_ADDR_B0);
-
-		IWL_ERR(trans, "Timeout on secure boot process, reg = %x\n",
-			reg);
-		return ret;
-	}
-
 	return 0;
 }
 
 static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 				   const struct fw_img *fw, bool run_in_rfkill)
 {
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int ret;
 	bool hw_rfkill;
 
@@ -967,9 +994,6 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 		return ret;
 	}
 
-	/* init ref_count to 1 (should be cleared when ucode is loaded) */
-	trans_pcie->ref_count = 1;
-
 	/* make sure rfkill handshake bits are cleared */
 	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
 	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR,
@@ -984,9 +1008,8 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
 
 	/* Load the given image to the HW */
-	if ((trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) &&
-	    (CSR_HW_REV_STEP(trans->hw_rev) != SILICON_A_STEP))
-		return iwl_pcie_load_given_ucode_8000b(trans, fw);
+	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		return iwl_pcie_load_given_ucode_8000(trans, fw);
 	else
 		return iwl_pcie_load_given_ucode(trans, fw);
 }
@@ -1288,6 +1311,9 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 	trans_pcie->bc_table_dword = trans_cfg->bc_table_dword;
 	trans_pcie->scd_set_active = trans_cfg->scd_set_active;
 
+	/* init ref_count to 1 (should be cleared when ucode is loaded) */
+	trans_pcie->ref_count = 1;
+
 	/* Initialize NAPI here - it should be before registering to mac80211
 	 * in the opmode but after the HW struct is allocated.
 	 * As this function may be called again in some corner cases don't
@@ -1462,6 +1488,60 @@ static int iwl_trans_pcie_write_mem(struct iwl_trans *trans, u32 addr,
 	return ret;
 }
 
+static void iwl_trans_pcie_freeze_txq_timer(struct iwl_trans *trans,
+					    unsigned long txqs,
+					    bool freeze)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	int queue;
+
+	for_each_set_bit(queue, &txqs, BITS_PER_LONG) {
+		struct iwl_txq *txq = &trans_pcie->txq[queue];
+		unsigned long now;
+
+		spin_lock_bh(&txq->lock);
+
+		now = jiffies;
+
+		if (txq->frozen == freeze)
+			goto next_queue;
+
+		IWL_DEBUG_TX_QUEUES(trans, "%s TXQ %d\n",
+				    freeze ? "Freezing" : "Waking", queue);
+
+		txq->frozen = freeze;
+
+		if (txq->q.read_ptr == txq->q.write_ptr)
+			goto next_queue;
+
+		if (freeze) {
+			if (unlikely(time_after(now,
+						txq->stuck_timer.expires))) {
+				/*
+				 * The timer should have fired, maybe it is
+				 * spinning right now on the lock.
+				 */
+				goto next_queue;
+			}
+			/* remember how long until the timer fires */
+			txq->frozen_expiry_remainder =
+				txq->stuck_timer.expires - now;
+			del_timer(&txq->stuck_timer);
+			goto next_queue;
+		}
+
+		/*
+		 * Wake a non-empty queue -> arm timer with the
+		 * remainder before it froze
+		 */
+		mod_timer(&txq->stuck_timer,
+			  now + txq->frozen_expiry_remainder);
+
+next_queue:
+		spin_unlock_bh(&txq->lock);
+	}
+}
+
 #define IWL_FLUSH_WAIT_MS	2000
 
 static int iwl_trans_pcie_wait_txq_empty(struct iwl_trans *trans, u32 txq_bm)
@@ -1713,7 +1793,7 @@ static ssize_t iwl_dbgfs_tx_queue_read(struct file *file,
 	int ret;
 	size_t bufsz;
 
-	bufsz = sizeof(char) * 64 * trans->cfg->base_params->num_of_queues;
+	bufsz = sizeof(char) * 75 * trans->cfg->base_params->num_of_queues;
 
 	if (!trans_pcie->txq)
 		return -EAGAIN;
@@ -1726,11 +1806,11 @@ static ssize_t iwl_dbgfs_tx_queue_read(struct file *file,
 		txq = &trans_pcie->txq[cnt];
 		q = &txq->q;
 		pos += scnprintf(buf + pos, bufsz - pos,
-				"hwq %.2d: read=%u write=%u use=%d stop=%d need_update=%d%s\n",
+				"hwq %.2d: read=%u write=%u use=%d stop=%d need_update=%d frozen=%d%s\n",
 				cnt, q->read_ptr, q->write_ptr,
 				!!test_bit(cnt, trans_pcie->queue_used),
 				 !!test_bit(cnt, trans_pcie->queue_stopped),
-				 txq->need_update,
+				 txq->need_update, txq->frozen,
 				 (cnt == trans_pcie->cmd_queue ? " HCMD" : ""));
 	}
 	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
@@ -1961,24 +2041,25 @@ static const struct {
 	{ .start = 0x00a01c7c, .end = 0x00a01c7c },
 	{ .start = 0x00a01c28, .end = 0x00a01c54 },
 	{ .start = 0x00a01c5c, .end = 0x00a01c5c },
-	{ .start = 0x00a01c84, .end = 0x00a01c84 },
+	{ .start = 0x00a01c60, .end = 0x00a01cdc },
 	{ .start = 0x00a01ce0, .end = 0x00a01d0c },
 	{ .start = 0x00a01d18, .end = 0x00a01d20 },
 	{ .start = 0x00a01d2c, .end = 0x00a01d30 },
 	{ .start = 0x00a01d40, .end = 0x00a01d5c },
 	{ .start = 0x00a01d80, .end = 0x00a01d80 },
-	{ .start = 0x00a01d98, .end = 0x00a01d98 },
+	{ .start = 0x00a01d98, .end = 0x00a01d9c },
+	{ .start = 0x00a01da8, .end = 0x00a01da8 },
+	{ .start = 0x00a01db8, .end = 0x00a01df4 },
 	{ .start = 0x00a01dc0, .end = 0x00a01dfc },
 	{ .start = 0x00a01e00, .end = 0x00a01e2c },
 	{ .start = 0x00a01e40, .end = 0x00a01e60 },
+	{ .start = 0x00a01e68, .end = 0x00a01e6c },
+	{ .start = 0x00a01e74, .end = 0x00a01e74 },
 	{ .start = 0x00a01e84, .end = 0x00a01e90 },
 	{ .start = 0x00a01e9c, .end = 0x00a01ec4 },
-	{ .start = 0x00a01ed0, .end = 0x00a01ed0 },
-	{ .start = 0x00a01f00, .end = 0x00a01f14 },
-	{ .start = 0x00a01f44, .end = 0x00a01f58 },
-	{ .start = 0x00a01f80, .end = 0x00a01fa8 },
-	{ .start = 0x00a01fb0, .end = 0x00a01fbc },
-	{ .start = 0x00a01ff8, .end = 0x00a01ffc },
+	{ .start = 0x00a01ed0, .end = 0x00a01ee0 },
+	{ .start = 0x00a01f00, .end = 0x00a01f1c },
+	{ .start = 0x00a01f44, .end = 0x00a01ffc },
 	{ .start = 0x00a02000, .end = 0x00a02048 },
 	{ .start = 0x00a02068, .end = 0x00a020f0 },
 	{ .start = 0x00a02100, .end = 0x00a02118 },
@@ -2305,6 +2386,7 @@ static const struct iwl_trans_ops trans_ops_pcie = {
 	.dbgfs_register = iwl_trans_pcie_dbgfs_register,
 
 	.wait_tx_queue_empty = iwl_trans_pcie_wait_txq_empty,
+	.freeze_txq_timer = iwl_trans_pcie_freeze_txq_timer,
 
 	.write8 = iwl_trans_pcie_write8,
 	.write32 = iwl_trans_pcie_write32,
@@ -2423,10 +2505,45 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	 * "dash" value). To keep hw_rev backwards compatible - we'll store it
 	 * in the old format.
 	 */
-	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
+		unsigned long flags;
+		int ret;
+
 		trans->hw_rev = (trans->hw_rev & 0xfff0) |
 				(CSR_HW_REV_STEP(trans->hw_rev << 2) << 2);
 
+		/*
+		 * in-order to recognize C step driver should read chip version
+		 * id located at the AUX bus MISC address space.
+		 */
+		iwl_set_bit(trans, CSR_GP_CNTRL,
+			    CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+		udelay(2);
+
+		ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
+				   CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+				   CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+				   25000);
+		if (ret < 0) {
+			IWL_DEBUG_INFO(trans, "Failed to wake up the nic\n");
+			goto out_pci_disable_msi;
+		}
+
+		if (iwl_trans_grab_nic_access(trans, false, &flags)) {
+			u32 hw_step;
+
+			hw_step = __iwl_read_prph(trans, WFPM_CTRL_REG);
+			hw_step |= ENABLE_WFPM;
+			__iwl_write_prph(trans, WFPM_CTRL_REG, hw_step);
+			hw_step = __iwl_read_prph(trans, AUX_MISC_REG);
+			hw_step = (hw_step >> HW_STEP_LOCATION_BITS) & 0xF;
+			if (hw_step == 0x3)
+				trans->hw_rev = (trans->hw_rev & 0xFFFFFFF3) |
+						(SILICON_C_STEP << 2);
+			iwl_trans_release_nic_access(trans, &flags);
+		}
+	}
+
 	trans->hw_id = (pdev->device << 16) + pdev->subsystem_device;
 	snprintf(trans->hw_id_str, sizeof(trans->hw_id_str),
 		 "PCI ID: 0x%04X:0x%04X", pdev->device, pdev->subsystem_device);
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index af0bce736358..06952aadfd7b 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -725,33 +725,50 @@ void iwl_trans_pcie_tx_reset(struct iwl_trans *trans)
 	iwl_pcie_tx_start(trans, 0);
 }
 
+static void iwl_pcie_tx_stop_fh(struct iwl_trans *trans)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	unsigned long flags;
+	int ch, ret;
+	u32 mask = 0;
+
+	spin_lock(&trans_pcie->irq_lock);
+
+	if (!iwl_trans_grab_nic_access(trans, false, &flags))
+		goto out;
+
+	/* Stop each Tx DMA channel */
+	for (ch = 0; ch < FH_TCSR_CHNL_NUM; ch++) {
+		iwl_write32(trans, FH_TCSR_CHNL_TX_CONFIG_REG(ch), 0x0);
+		mask |= FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(ch);
+	}
+
+	/* Wait for DMA channels to be idle */
+	ret = iwl_poll_bit(trans, FH_TSSR_TX_STATUS_REG, mask, mask, 5000);
+	if (ret < 0)
+		IWL_ERR(trans,
+			"Failing on timeout while stopping DMA channel %d [0x%08x]\n",
+			ch, iwl_read32(trans, FH_TSSR_TX_STATUS_REG));
+
+	iwl_trans_release_nic_access(trans, &flags);
+
+out:
+	spin_unlock(&trans_pcie->irq_lock);
+}
+
 /*
  * iwl_pcie_tx_stop - Stop all Tx DMA channels
  */
 int iwl_pcie_tx_stop(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	int ch, txq_id, ret;
+	int txq_id;
 
 	/* Turn off all Tx DMA fifos */
-	spin_lock(&trans_pcie->irq_lock);
-
 	iwl_scd_deactivate_fifos(trans);
 
-	/* Stop each Tx DMA channel, and wait for it to be idle */
-	for (ch = 0; ch < FH_TCSR_CHNL_NUM; ch++) {
-		iwl_write_direct32(trans,
-				   FH_TCSR_CHNL_TX_CONFIG_REG(ch), 0x0);
-		ret = iwl_poll_direct_bit(trans, FH_TSSR_TX_STATUS_REG,
-			FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(ch), 1000);
-		if (ret < 0)
-			IWL_ERR(trans,
-				"Failing on timeout while stopping DMA channel %d [0x%08x]\n",
-				ch,
-				iwl_read_direct32(trans,
-						  FH_TSSR_TX_STATUS_REG));
-	}
-	spin_unlock(&trans_pcie->irq_lock);
+	/* Turn off all Tx DMA channels */
+	iwl_pcie_tx_stop_fh(trans);
 
 	/*
 	 * This function can be called before the op_mode disabled the
@@ -912,10 +929,19 @@ error:
 
 static inline void iwl_pcie_txq_progress(struct iwl_txq *txq)
 {
+	lockdep_assert_held(&txq->lock);
+
 	if (!txq->wd_timeout)
 		return;
 
 	/*
+	 * station is asleep and we send data - that must
+	 * be uAPSD or PS-Poll. Don't rearm the timer.
+	 */
+	if (txq->frozen)
+		return;
+
+	/*
 	 * if empty delete timer, otherwise move timer forward
 	 * since we're making progress on this queue
 	 */
@@ -1248,6 +1274,9 @@ void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int txq_id,
 			SCD_TX_STTS_QUEUE_OFFSET(txq_id);
 	static const u32 zero_val[4] = {};
 
+	trans_pcie->txq[txq_id].frozen_expiry_remainder = 0;
+	trans_pcie->txq[txq_id].frozen = false;
+
 	/*
 	 * Upon HW Rfkill - we stop the device, and then stop the queues
 	 * in the op_mode. Just for the sake of the simplicity of the op_mode,
diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/libertas/cfg.c
index a92985a6ea21..1a4d558022d8 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/libertas/cfg.c
@@ -1356,8 +1356,8 @@ static int lbs_cfg_connect(struct wiphy *wiphy, struct net_device *dev,
 
 	/* Find the BSS we want using available scan results */
 	bss = cfg80211_get_bss(wiphy, sme->channel, sme->bssid,
-		sme->ssid, sme->ssid_len,
-		WLAN_CAPABILITY_ESS, WLAN_CAPABILITY_ESS);
+		sme->ssid, sme->ssid_len, IEEE80211_BSS_TYPE_ESS,
+		IEEE80211_PRIVACY_ANY);
 	if (!bss) {
 		wiphy_err(wiphy, "assoc: bss %pM not in scan results\n",
 			  sme->bssid);
@@ -2000,7 +2000,7 @@ static int lbs_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	 * bss list is populated already */
 	bss = cfg80211_get_bss(wiphy, params->chandef.chan, params->bssid,
 		params->ssid, params->ssid_len,
-		WLAN_CAPABILITY_IBSS, WLAN_CAPABILITY_IBSS);
+		IEEE80211_BSS_TYPE_IBSS, IEEE80211_PRIVACY_ANY);
 
 	if (bss) {
 		ret = lbs_ibss_join_existing(priv, params, bss);
diff --git a/drivers/net/wireless/libertas/debugfs.c b/drivers/net/wireless/libertas/debugfs.c
index cc6a0a586f0b..26cbf1dcc662 100644
--- a/drivers/net/wireless/libertas/debugfs.c
+++ b/drivers/net/wireless/libertas/debugfs.c
@@ -742,8 +742,7 @@ void lbs_debugfs_init(void)
 
 void lbs_debugfs_remove(void)
 {
-	if (lbs_dir)
-		 debugfs_remove(lbs_dir);
+	debugfs_remove(lbs_dir);
 }
 
 void lbs_debugfs_init_one(struct lbs_private *priv, struct net_device *dev)
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 569b64ecc607..8079560f4965 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -667,7 +667,7 @@ static int lbs_setup_firmware(struct lbs_private *priv)
 	lbs_deb_enter(LBS_DEB_FW);
 
 	/* Read MAC address from firmware */
-	memset(priv->current_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(priv->current_addr);
 	ret = lbs_update_hw_spec(priv);
 	if (ret)
 		goto done;
@@ -871,7 +871,7 @@ static int lbs_init_adapter(struct lbs_private *priv)
 
 	lbs_deb_enter(LBS_DEB_MAIN);
 
-	memset(priv->current_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(priv->current_addr);
 
 	priv->connect_status = LBS_DISCONNECTED;
 	priv->channel = DEFAULT_AD_HOC_CHANNEL;
diff --git a/drivers/net/wireless/libertas_tf/if_usb.c b/drivers/net/wireless/libertas_tf/if_usb.c
index d576dd6665d3..1a20cee5febe 100644
--- a/drivers/net/wireless/libertas_tf/if_usb.c
+++ b/drivers/net/wireless/libertas_tf/if_usb.c
@@ -365,7 +365,6 @@ static int if_usb_reset_device(struct if_usb_card *cardp)
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(if_usb_reset_device);
 
 /**
  *  usb_tx_block - transfer data to the device
@@ -907,7 +906,6 @@ restart:
 	lbtf_deb_leave_args(LBTF_DEB_USB, "ret %d", ret);
 	return ret;
 }
-EXPORT_SYMBOL_GPL(if_usb_prog_firmware);
 
 
 #define if_usb_suspend NULL
diff --git a/drivers/net/wireless/libertas_tf/main.c b/drivers/net/wireless/libertas_tf/main.c
index 25c5acc78bd1..ed02e4bf2c26 100644
--- a/drivers/net/wireless/libertas_tf/main.c
+++ b/drivers/net/wireless/libertas_tf/main.c
@@ -152,7 +152,7 @@ static int lbtf_setup_firmware(struct lbtf_private *priv)
 	/*
 	 * Read priv address from HW
 	 */
-	memset(priv->current_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(priv->current_addr);
 	ret = lbtf_update_hw_spec(priv);
 	if (ret) {
 		ret = -1;
@@ -199,7 +199,7 @@ out:
 static int lbtf_init_adapter(struct lbtf_private *priv)
 {
 	lbtf_deb_enter(LBTF_DEB_MAIN);
-	memset(priv->current_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(priv->current_addr);
 	mutex_init(&priv->lock);
 
 	priv->vif = NULL;
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 8908be6dbc48..d5c0a1af08b9 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -330,6 +330,83 @@ static const struct ieee80211_rate hwsim_rates[] = {
 	{ .bitrate = 540 }
 };
 
+#define OUI_QCA 0x001374
+#define QCA_NL80211_SUBCMD_TEST 1
+enum qca_nl80211_vendor_subcmds {
+	QCA_WLAN_VENDOR_ATTR_TEST = 8,
+	QCA_WLAN_VENDOR_ATTR_MAX = QCA_WLAN_VENDOR_ATTR_TEST
+};
+
+static const struct nla_policy
+hwsim_vendor_test_policy[QCA_WLAN_VENDOR_ATTR_MAX + 1] = {
+	[QCA_WLAN_VENDOR_ATTR_MAX] = { .type = NLA_U32 },
+};
+
+static int mac80211_hwsim_vendor_cmd_test(struct wiphy *wiphy,
+					  struct wireless_dev *wdev,
+					  const void *data, int data_len)
+{
+	struct sk_buff *skb;
+	struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_MAX + 1];
+	int err;
+	u32 val;
+
+	err = nla_parse(tb, QCA_WLAN_VENDOR_ATTR_MAX, data, data_len,
+			hwsim_vendor_test_policy);
+	if (err)
+		return err;
+	if (!tb[QCA_WLAN_VENDOR_ATTR_TEST])
+		return -EINVAL;
+	val = nla_get_u32(tb[QCA_WLAN_VENDOR_ATTR_TEST]);
+	wiphy_debug(wiphy, "%s: test=%u\n", __func__, val);
+
+	/* Send a vendor event as a test. Note that this would not normally be
+	 * done within a command handler, but rather, based on some other
+	 * trigger. For simplicity, this command is used to trigger the event
+	 * here.
+	 *
+	 * event_idx = 0 (index in mac80211_hwsim_vendor_commands)
+	 */
+	skb = cfg80211_vendor_event_alloc(wiphy, wdev, 100, 0, GFP_KERNEL);
+	if (skb) {
+		/* skb_put() or nla_put() will fill up data within
+		 * NL80211_ATTR_VENDOR_DATA.
+		 */
+
+		/* Add vendor data */
+		nla_put_u32(skb, QCA_WLAN_VENDOR_ATTR_TEST, val + 1);
+
+		/* Send the event - this will call nla_nest_end() */
+		cfg80211_vendor_event(skb, GFP_KERNEL);
+	}
+
+	/* Send a response to the command */
+	skb = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, 10);
+	if (!skb)
+		return -ENOMEM;
+
+	/* skb_put() or nla_put() will fill up data within
+	 * NL80211_ATTR_VENDOR_DATA
+	 */
+	nla_put_u32(skb, QCA_WLAN_VENDOR_ATTR_TEST, val + 2);
+
+	return cfg80211_vendor_cmd_reply(skb);
+}
+
+static struct wiphy_vendor_command mac80211_hwsim_vendor_commands[] = {
+	{
+		.info = { .vendor_id = OUI_QCA,
+			  .subcmd = QCA_NL80211_SUBCMD_TEST },
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV,
+		.doit = mac80211_hwsim_vendor_cmd_test,
+	}
+};
+
+/* Advertise support vendor specific events */
+static const struct nl80211_vendor_cmd_info mac80211_hwsim_vendor_events[] = {
+	{ .vendor_id = OUI_QCA, .subcmd = 1 },
+};
+
 static const struct ieee80211_iface_limit hwsim_if_limits[] = {
 	{ .max = 1, .types = BIT(NL80211_IFTYPE_ADHOC) },
 	{ .max = 2048,  .types = BIT(NL80211_IFTYPE_STATION) |
@@ -906,8 +983,7 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 		goto nla_put_failure;
 	}
 
-	if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER,
-		    ETH_ALEN, data->addresses[1].addr))
+	if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER, ETH_ALEN, hdr->addr2))
 		goto nla_put_failure;
 
 	/* We get the skb->data */
@@ -1522,21 +1598,16 @@ static void mac80211_hwsim_bss_info_changed(struct ieee80211_hw *hw,
 		vp->aid = info->aid;
 	}
 
-	if (changed & BSS_CHANGED_BEACON_INT) {
-		wiphy_debug(hw->wiphy, "  BCNINT: %d\n", info->beacon_int);
-		data->beacon_int = info->beacon_int * 1024;
-	}
-
 	if (changed & BSS_CHANGED_BEACON_ENABLED) {
-		wiphy_debug(hw->wiphy, "  BCN EN: %d\n", info->enable_beacon);
+		wiphy_debug(hw->wiphy, "  BCN EN: %d (BI=%u)\n",
+			    info->enable_beacon, info->beacon_int);
 		vp->bcn_en = info->enable_beacon;
 		if (data->started &&
 		    !hrtimer_is_queued(&data->beacon_timer.timer) &&
 		    info->enable_beacon) {
 			u64 tsf, until_tbtt;
 			u32 bcn_int;
-			if (WARN_ON(!data->beacon_int))
-				data->beacon_int = 1000 * 1024;
+			data->beacon_int = info->beacon_int * 1024;
 			tsf = mac80211_hwsim_get_tsf(hw, vif);
 			bcn_int = data->beacon_int;
 			until_tbtt = bcn_int - do_div(tsf, bcn_int);
@@ -1550,8 +1621,10 @@ static void mac80211_hwsim_bss_info_changed(struct ieee80211_hw *hw,
 				mac80211_hwsim_bcn_en_iter, &count);
 			wiphy_debug(hw->wiphy, "  beaconing vifs remaining: %u",
 				    count);
-			if (count == 0)
+			if (count == 0) {
 				tasklet_hrtimer_cancel(&data->beacon_timer);
+				data->beacon_int = 0;
+			}
 		}
 	}
 
@@ -1911,7 +1984,7 @@ static void mac80211_hwsim_sw_scan_complete(struct ieee80211_hw *hw,
 
 	printk(KERN_DEBUG "hwsim sw_scan_complete\n");
 	hwsim->scanning = false;
-	memset(hwsim->scan_addr, 0, ETH_ALEN);
+	eth_zero_addr(hwsim->scan_addr);
 
 	mutex_unlock(&hwsim->mutex);
 }
@@ -2267,7 +2340,7 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 	skb_queue_head_init(&data->pending);
 
 	SET_IEEE80211_DEV(hw, data->dev);
-	memset(addr, 0, ETH_ALEN);
+	eth_zero_addr(addr);
 	addr[0] = 0x02;
 	addr[3] = idx >> 8;
 	addr[4] = idx;
@@ -2420,6 +2493,12 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 	hw->max_rates = 4;
 	hw->max_rate_tries = 11;
 
+	hw->wiphy->vendor_commands = mac80211_hwsim_vendor_commands;
+	hw->wiphy->n_vendor_commands =
+		ARRAY_SIZE(mac80211_hwsim_vendor_commands);
+	hw->wiphy->vendor_events = mac80211_hwsim_vendor_events;
+	hw->wiphy->n_vendor_events = ARRAY_SIZE(mac80211_hwsim_vendor_events);
+
 	if (param->reg_strict)
 		hw->wiphy->regulatory_flags |= REGULATORY_STRICT_REG;
 	if (param->regd) {
@@ -2600,7 +2679,7 @@ static void hwsim_mon_setup(struct net_device *dev)
 	ether_setup(dev);
 	dev->tx_queue_len = 0;
 	dev->type = ARPHRD_IEEE80211_RADIOTAP;
-	memset(dev->dev_addr, 0, ETH_ALEN);
+	eth_zero_addr(dev->dev_addr);
 	dev->dev_addr[0] = 0x12;
 }
 
@@ -2611,7 +2690,7 @@ static struct mac80211_hwsim_data *get_hwsim_data_ref_from_addr(const u8 *addr)
 
 	spin_lock_bh(&hwsim_radio_lock);
 	list_for_each_entry(data, &hwsim_radios, list) {
-		if (memcmp(data->addresses[1].addr, addr, ETH_ALEN) == 0) {
+		if (mac80211_hwsim_addr_match(data, addr)) {
 			_found = true;
 			break;
 		}
diff --git a/drivers/net/wireless/mwifiex/11n.c b/drivers/net/wireless/mwifiex/11n.c
index 543148d27b01..433bd6837c79 100644
--- a/drivers/net/wireless/mwifiex/11n.c
+++ b/drivers/net/wireless/mwifiex/11n.c
@@ -159,6 +159,7 @@ int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 	int tid;
 	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp = &resp->params.add_ba_rsp;
 	struct mwifiex_tx_ba_stream_tbl *tx_ba_tbl;
+	struct mwifiex_ra_list_tbl *ra_list;
 	u16 block_ack_param_set = le16_to_cpu(add_ba_rsp->block_ack_param_set);
 
 	add_ba_rsp->ssn = cpu_to_le16((le16_to_cpu(add_ba_rsp->ssn))
@@ -166,7 +167,13 @@ int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 
 	tid = (block_ack_param_set & IEEE80211_ADDBA_PARAM_TID_MASK)
 	       >> BLOCKACKPARAM_TID_POS;
+	ra_list = mwifiex_wmm_get_ralist_node(priv, tid, add_ba_rsp->
+		peer_mac_addr);
 	if (le16_to_cpu(add_ba_rsp->status_code) != BA_RESULT_SUCCESS) {
+		if (ra_list) {
+			ra_list->ba_status = BA_SETUP_NONE;
+			ra_list->amsdu_in_ampdu = false;
+		}
 		mwifiex_del_ba_tbl(priv, tid, add_ba_rsp->peer_mac_addr,
 				   TYPE_DELBA_SENT, true);
 		if (add_ba_rsp->add_rsp_result != BA_RESULT_TIMEOUT)
@@ -185,6 +192,10 @@ int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 			tx_ba_tbl->amsdu = true;
 		else
 			tx_ba_tbl->amsdu = false;
+		if (ra_list) {
+			ra_list->amsdu_in_ampdu = tx_ba_tbl->amsdu;
+			ra_list->ba_status = BA_SETUP_COMPLETE;
+		}
 	} else {
 		dev_err(priv->adapter->dev, "BA stream not created\n");
 	}
@@ -515,6 +526,7 @@ void mwifiex_create_ba_tbl(struct mwifiex_private *priv, u8 *ra, int tid,
 			   enum mwifiex_ba_status ba_status)
 {
 	struct mwifiex_tx_ba_stream_tbl *new_node;
+	struct mwifiex_ra_list_tbl *ra_list;
 	unsigned long flags;
 
 	if (!mwifiex_get_ba_tbl(priv, tid, ra)) {
@@ -522,7 +534,11 @@ void mwifiex_create_ba_tbl(struct mwifiex_private *priv, u8 *ra, int tid,
 				   GFP_ATOMIC);
 		if (!new_node)
 			return;
-
+		ra_list = mwifiex_wmm_get_ralist_node(priv, tid, ra);
+		if (ra_list) {
+			ra_list->ba_status = ba_status;
+			ra_list->amsdu_in_ampdu = false;
+		}
 		INIT_LIST_HEAD(&new_node->list);
 
 		new_node->tid = tid;
diff --git a/drivers/net/wireless/mwifiex/11n.h b/drivers/net/wireless/mwifiex/11n.h
index 8e2e39422ad8..afdd58aa90de 100644
--- a/drivers/net/wireless/mwifiex/11n.h
+++ b/drivers/net/wireless/mwifiex/11n.h
@@ -77,22 +77,6 @@ mwifiex_is_station_ampdu_allowed(struct mwifiex_private *priv,
 	return (node->ampdu_sta[tid] != BA_STREAM_NOT_ALLOWED) ? true : false;
 }
 
-/* This function checks whether AMSDU is allowed for BA stream. */
-static inline u8
-mwifiex_is_amsdu_in_ampdu_allowed(struct mwifiex_private *priv,
-				  struct mwifiex_ra_list_tbl *ptr, int tid)
-{
-	struct mwifiex_tx_ba_stream_tbl *tx_tbl;
-
-	if (is_broadcast_ether_addr(ptr->ra))
-		return false;
-	tx_tbl = mwifiex_get_ba_tbl(priv, tid, ptr->ra);
-	if (tx_tbl)
-		return tx_tbl->amsdu;
-
-	return false;
-}
-
 /* This function checks whether AMPDU is allowed or not for a particular TID. */
 static inline u8
 mwifiex_is_ampdu_allowed(struct mwifiex_private *priv,
@@ -182,22 +166,6 @@ mwifiex_find_stream_to_delete(struct mwifiex_private *priv, int ptr_tid,
 }
 
 /*
- * This function checks whether BA stream is set up or not.
- */
-static inline int
-mwifiex_is_ba_stream_setup(struct mwifiex_private *priv,
-			   struct mwifiex_ra_list_tbl *ptr, int tid)
-{
-	struct mwifiex_tx_ba_stream_tbl *tx_tbl;
-
-	tx_tbl = mwifiex_get_ba_tbl(priv, tid, ptr->ra);
-	if (tx_tbl && IS_BASTREAM_SETUP(tx_tbl))
-		return true;
-
-	return false;
-}
-
-/*
  * This function checks whether associated station is 11n enabled
  */
 static inline int mwifiex_is_sta_11n_enabled(struct mwifiex_private *priv,
diff --git a/drivers/net/wireless/mwifiex/11n_aggr.c b/drivers/net/wireless/mwifiex/11n_aggr.c
index 9b983b5cebbd..6183e255e62a 100644
--- a/drivers/net/wireless/mwifiex/11n_aggr.c
+++ b/drivers/net/wireless/mwifiex/11n_aggr.c
@@ -170,7 +170,7 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct sk_buff *skb_aggr, *skb_src;
 	struct mwifiex_txinfo *tx_info_aggr, *tx_info_src;
-	int pad = 0, ret;
+	int pad = 0, aggr_num = 0, ret;
 	struct mwifiex_tx_param tx_param;
 	struct txpd *ptx_pd = NULL;
 	struct timeval tv;
@@ -184,7 +184,8 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 	}
 
 	tx_info_src = MWIFIEX_SKB_TXCB(skb_src);
-	skb_aggr = dev_alloc_skb(adapter->tx_buf_size);
+	skb_aggr = mwifiex_alloc_dma_align_buf(adapter->tx_buf_size,
+					       GFP_ATOMIC | GFP_DMA);
 	if (!skb_aggr) {
 		dev_err(adapter->dev, "%s: alloc skb_aggr\n", __func__);
 		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
@@ -200,6 +201,7 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 
 	if (tx_info_src->flags & MWIFIEX_BUF_FLAG_TDLS_PKT)
 		tx_info_aggr->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
+	tx_info_aggr->flags |= MWIFIEX_BUF_FLAG_AGGR_PKT;
 	skb_aggr->priority = skb_src->priority;
 
 	do_gettimeofday(&tv);
@@ -211,11 +213,9 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 			break;
 
 		skb_src = skb_dequeue(&pra_list->skb_head);
-
 		pra_list->total_pkt_count--;
-
 		atomic_dec(&priv->wmm.tx_pkts_queued);
-
+		aggr_num++;
 		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
 				       ra_list_flags);
 		mwifiex_11n_form_amsdu_pkt(skb_aggr, skb_src, &pad);
@@ -251,6 +251,12 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 		ptx_pd = (struct txpd *)skb_aggr->data;
 
 	skb_push(skb_aggr, headroom);
+	tx_info_aggr->aggr_num = aggr_num * 2;
+	if (adapter->data_sent || adapter->tx_lock_flag) {
+		atomic_add(aggr_num * 2, &adapter->tx_queued);
+		skb_queue_tail(&adapter->tx_data_q, skb_aggr);
+		return 0;
+	}
 
 	if (adapter->iface_type == MWIFIEX_USB) {
 		adapter->data_sent = true;
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index a2e8817b56d8..f75f8acfaca0 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -659,6 +659,7 @@ mwifiex_del_ba_tbl(struct mwifiex_private *priv, int tid, u8 *peer_mac,
 {
 	struct mwifiex_rx_reorder_tbl *tbl;
 	struct mwifiex_tx_ba_stream_tbl *ptx_tbl;
+	struct mwifiex_ra_list_tbl *ra_list;
 	u8 cleanup_rx_reorder_tbl;
 	unsigned long flags;
 
@@ -686,7 +687,11 @@ mwifiex_del_ba_tbl(struct mwifiex_private *priv, int tid, u8 *peer_mac,
 				"event: TID, RA not found in table\n");
 			return;
 		}
-
+		ra_list = mwifiex_wmm_get_ralist_node(priv, tid, peer_mac);
+		if (ra_list) {
+			ra_list->amsdu_in_ampdu = false;
+			ra_list->ba_status = BA_SETUP_NONE;
+		}
 		spin_lock_irqsave(&priv->tx_ba_stream_tbl_lock, flags);
 		mwifiex_11n_delete_tx_ba_stream_tbl_entry(priv, ptx_tbl);
 		spin_unlock_irqrestore(&priv->tx_ba_stream_tbl_lock, flags);
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 41c8e25df954..bf9020ff2d33 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -717,6 +717,9 @@ mwifiex_cfg80211_init_p2p_go(struct mwifiex_private *priv)
 
 static int mwifiex_deinit_priv_params(struct mwifiex_private *priv)
 {
+	struct mwifiex_adapter *adapter = priv->adapter;
+	unsigned long flags;
+
 	priv->mgmt_frame_mask = 0;
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_MGMT_FRAME_REG,
 			     HostCmd_ACT_GEN_SET, 0,
@@ -727,6 +730,25 @@ static int mwifiex_deinit_priv_params(struct mwifiex_private *priv)
 	}
 
 	mwifiex_deauthenticate(priv, NULL);
+
+	spin_lock_irqsave(&adapter->main_proc_lock, flags);
+	adapter->main_locked = true;
+	if (adapter->mwifiex_processing) {
+		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
+		flush_workqueue(adapter->workqueue);
+	} else {
+		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
+	}
+
+	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
+	adapter->rx_locked = true;
+	if (adapter->rx_processing) {
+		spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+		flush_workqueue(adapter->rx_workqueue);
+	} else {
+	spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+	}
+
 	mwifiex_free_priv(priv);
 	priv->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
 	priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
@@ -740,6 +762,9 @@ mwifiex_init_new_priv_params(struct mwifiex_private *priv,
 			     struct net_device *dev,
 			     enum nl80211_iftype type)
 {
+	struct mwifiex_adapter *adapter = priv->adapter;
+	unsigned long flags;
+
 	mwifiex_init_priv(priv);
 
 	priv->bss_mode = type;
@@ -770,6 +795,14 @@ mwifiex_init_new_priv_params(struct mwifiex_private *priv,
 		return -EOPNOTSUPP;
 	}
 
+	spin_lock_irqsave(&adapter->main_proc_lock, flags);
+	adapter->main_locked = false;
+	spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
+
+	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
+	adapter->rx_locked = false;
+	spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+
 	return 0;
 }
 
@@ -1563,7 +1596,7 @@ mwifiex_cfg80211_del_station(struct wiphy *wiphy, struct net_device *dev,
 
 	wiphy_dbg(wiphy, "%s: mac address %pM\n", __func__, params->mac);
 
-	memset(deauth_mac, 0, ETH_ALEN);
+	eth_zero_addr(deauth_mac);
 
 	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
 	sta_node = mwifiex_get_sta_entry(priv, params->mac);
@@ -1786,7 +1819,7 @@ mwifiex_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
 	wiphy_dbg(wiphy, "info: successfully disconnected from %pM:"
 		" reason code %d\n", priv->cfg_bssid, reason_code);
 
-	memset(priv->cfg_bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->cfg_bssid);
 	priv->hs2_enabled = false;
 
 	return 0;
@@ -1954,13 +1987,13 @@ done:
 		if (mode == NL80211_IFTYPE_ADHOC)
 			bss = cfg80211_get_bss(priv->wdev.wiphy, channel,
 					       bssid, ssid, ssid_len,
-					       WLAN_CAPABILITY_IBSS,
-					       WLAN_CAPABILITY_IBSS);
+					       IEEE80211_BSS_TYPE_IBSS,
+					       IEEE80211_PRIVACY_ANY);
 		else
 			bss = cfg80211_get_bss(priv->wdev.wiphy, channel,
 					       bssid, ssid, ssid_len,
-					       WLAN_CAPABILITY_ESS,
-					       WLAN_CAPABILITY_ESS);
+					       IEEE80211_BSS_TYPE_ESS,
+					       IEEE80211_PRIVACY_ANY);
 
 		if (!bss) {
 			if (is_scanning_required) {
@@ -2046,7 +2079,7 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 		dev_dbg(priv->adapter->dev,
 			"info: association to bssid %pM failed\n",
 			priv->cfg_bssid);
-		memset(priv->cfg_bssid, 0, ETH_ALEN);
+		eth_zero_addr(priv->cfg_bssid);
 
 		if (ret > 0)
 			cfg80211_connect_result(priv->netdev, priv->cfg_bssid,
@@ -2194,7 +2227,7 @@ mwifiex_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 	if (mwifiex_deauthenticate(priv, NULL))
 		return -EFAULT;
 
-	memset(priv->cfg_bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->cfg_bssid);
 
 	return 0;
 }
@@ -2397,12 +2430,12 @@ mwifiex_setup_ht_caps(struct ieee80211_sta_ht_cap *ht_info,
 	ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
 }
 
-#define MWIFIEX_MAX_WQ_LEN  30
 /*
- *  create a new virtual interface with the given name
+ *  create a new virtual interface with the given name and name assign type
  */
 struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 					      const char *name,
+					      unsigned char name_assign_type,
 					      enum nl80211_iftype type,
 					      u32 *flags,
 					      struct vif_params *params)
@@ -2411,7 +2444,6 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	struct mwifiex_private *priv;
 	struct net_device *dev;
 	void *mdev_priv;
-	char dfs_cac_str[MWIFIEX_MAX_WQ_LEN], dfs_chsw_str[MWIFIEX_MAX_WQ_LEN];
 
 	if (!adapter)
 		return ERR_PTR(-EFAULT);
@@ -2523,7 +2555,7 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	}
 
 	dev = alloc_netdev_mqs(sizeof(struct mwifiex_private *), name,
-			       NET_NAME_UNKNOWN, ether_setup,
+			       name_assign_type, ether_setup,
 			       IEEE80211_NUM_ACS, 1);
 	if (!dev) {
 		wiphy_err(wiphy, "no memory available for netdevice\n");
@@ -2576,12 +2608,10 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		return ERR_PTR(-EFAULT);
 	}
 
-	strcpy(dfs_cac_str, "MWIFIEX_DFS_CAC");
-	strcat(dfs_cac_str, name);
-	priv->dfs_cac_workqueue = alloc_workqueue(dfs_cac_str,
+	priv->dfs_cac_workqueue = alloc_workqueue("MWIFIEX_DFS_CAC%s",
 						  WQ_HIGHPRI |
 						  WQ_MEM_RECLAIM |
-						  WQ_UNBOUND, 1);
+						  WQ_UNBOUND, 1, name);
 	if (!priv->dfs_cac_workqueue) {
 		wiphy_err(wiphy, "cannot register virtual network device\n");
 		free_netdev(dev);
@@ -2594,11 +2624,9 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 	INIT_DELAYED_WORK(&priv->dfs_cac_work, mwifiex_dfs_cac_work_queue);
 
-	strcpy(dfs_chsw_str, "MWIFIEX_DFS_CHSW");
-	strcat(dfs_chsw_str, name);
-	priv->dfs_chan_sw_workqueue = alloc_workqueue(dfs_chsw_str,
+	priv->dfs_chan_sw_workqueue = alloc_workqueue("MWIFIEX_DFS_CHSW%s",
 						      WQ_HIGHPRI | WQ_UNBOUND |
-						      WQ_MEM_RECLAIM, 1);
+						      WQ_MEM_RECLAIM, 1, name);
 	if (!priv->dfs_chan_sw_workqueue) {
 		wiphy_err(wiphy, "cannot register virtual network device\n");
 		free_netdev(dev);
@@ -2738,24 +2766,71 @@ mwifiex_is_pattern_supported(struct cfg80211_pkt_pattern *pat, s8 *byte_seq,
 }
 
 #ifdef CONFIG_PM
-static int mwifiex_set_mef_filter(struct mwifiex_private *priv,
-				  struct cfg80211_wowlan *wowlan)
+static void mwifiex_set_auto_arp_mef_entry(struct mwifiex_private *priv,
+					   struct mwifiex_mef_entry *mef_entry)
+{
+	int i, filt_num = 0, num_ipv4 = 0;
+	struct in_device *in_dev;
+	struct in_ifaddr *ifa;
+	__be32 ips[MWIFIEX_MAX_SUPPORTED_IPADDR];
+	struct mwifiex_adapter *adapter = priv->adapter;
+
+	mef_entry->mode = MEF_MODE_HOST_SLEEP;
+	mef_entry->action = MEF_ACTION_AUTO_ARP;
+
+	/* Enable ARP offload feature */
+	memset(ips, 0, sizeof(ips));
+	for (i = 0; i < MWIFIEX_MAX_BSS_NUM; i++) {
+		if (adapter->priv[i]->netdev) {
+			in_dev = __in_dev_get_rtnl(adapter->priv[i]->netdev);
+			if (!in_dev)
+				continue;
+			ifa = in_dev->ifa_list;
+			if (!ifa || !ifa->ifa_local)
+				continue;
+			ips[i] = ifa->ifa_local;
+			num_ipv4++;
+		}
+	}
+
+	for (i = 0; i < num_ipv4; i++) {
+		if (!ips[i])
+			continue;
+		mef_entry->filter[filt_num].repeat = 1;
+		memcpy(mef_entry->filter[filt_num].byte_seq,
+		       (u8 *)&ips[i], sizeof(ips[i]));
+		mef_entry->filter[filt_num].
+			byte_seq[MWIFIEX_MEF_MAX_BYTESEQ] =
+			sizeof(ips[i]);
+		mef_entry->filter[filt_num].offset = 46;
+		mef_entry->filter[filt_num].filt_type = TYPE_EQ;
+		if (filt_num) {
+			mef_entry->filter[filt_num].filt_action =
+				TYPE_OR;
+		}
+		filt_num++;
+	}
+
+	mef_entry->filter[filt_num].repeat = 1;
+	mef_entry->filter[filt_num].byte_seq[0] = 0x08;
+	mef_entry->filter[filt_num].byte_seq[1] = 0x06;
+	mef_entry->filter[filt_num].byte_seq[MWIFIEX_MEF_MAX_BYTESEQ] = 2;
+	mef_entry->filter[filt_num].offset = 20;
+	mef_entry->filter[filt_num].filt_type = TYPE_EQ;
+	mef_entry->filter[filt_num].filt_action = TYPE_AND;
+}
+
+static int mwifiex_set_wowlan_mef_entry(struct mwifiex_private *priv,
+					struct mwifiex_ds_mef_cfg *mef_cfg,
+					struct mwifiex_mef_entry *mef_entry,
+					struct cfg80211_wowlan *wowlan)
 {
 	int i, filt_num = 0, ret = 0;
 	bool first_pat = true;
 	u8 byte_seq[MWIFIEX_MEF_MAX_BYTESEQ + 1];
 	const u8 ipv4_mc_mac[] = {0x33, 0x33};
 	const u8 ipv6_mc_mac[] = {0x01, 0x00, 0x5e};
-	struct mwifiex_ds_mef_cfg mef_cfg;
-	struct mwifiex_mef_entry *mef_entry;
 
-	mef_entry = kzalloc(sizeof(*mef_entry), GFP_KERNEL);
-	if (!mef_entry)
-		return -ENOMEM;
-
-	memset(&mef_cfg, 0, sizeof(mef_cfg));
-	mef_cfg.num_entries = 1;
-	mef_cfg.mef_entry = mef_entry;
 	mef_entry->mode = MEF_MODE_HOST_SLEEP;
 	mef_entry->action = MEF_ACTION_ALLOW_AND_WAKEUP_HOST;
 
@@ -2772,20 +2847,19 @@ static int mwifiex_set_mef_filter(struct mwifiex_private *priv,
 		if (!wowlan->patterns[i].pkt_offset) {
 			if (!(byte_seq[0] & 0x01) &&
 			    (byte_seq[MWIFIEX_MEF_MAX_BYTESEQ] == 1)) {
-				mef_cfg.criteria |= MWIFIEX_CRITERIA_UNICAST;
+				mef_cfg->criteria |= MWIFIEX_CRITERIA_UNICAST;
 				continue;
 			} else if (is_broadcast_ether_addr(byte_seq)) {
-				mef_cfg.criteria |= MWIFIEX_CRITERIA_BROADCAST;
+				mef_cfg->criteria |= MWIFIEX_CRITERIA_BROADCAST;
 				continue;
 			} else if ((!memcmp(byte_seq, ipv4_mc_mac, 2) &&
 				    (byte_seq[MWIFIEX_MEF_MAX_BYTESEQ] == 2)) ||
 				   (!memcmp(byte_seq, ipv6_mc_mac, 3) &&
 				    (byte_seq[MWIFIEX_MEF_MAX_BYTESEQ] == 3))) {
-				mef_cfg.criteria |= MWIFIEX_CRITERIA_MULTICAST;
+				mef_cfg->criteria |= MWIFIEX_CRITERIA_MULTICAST;
 				continue;
 			}
 		}
-
 		mef_entry->filter[filt_num].repeat = 1;
 		mef_entry->filter[filt_num].offset =
 			wowlan->patterns[i].pkt_offset;
@@ -2802,7 +2876,7 @@ static int mwifiex_set_mef_filter(struct mwifiex_private *priv,
 	}
 
 	if (wowlan->magic_pkt) {
-		mef_cfg.criteria |= MWIFIEX_CRITERIA_UNICAST;
+		mef_cfg->criteria |= MWIFIEX_CRITERIA_UNICAST;
 		mef_entry->filter[filt_num].repeat = 16;
 		memcpy(mef_entry->filter[filt_num].byte_seq, priv->curr_addr,
 				ETH_ALEN);
@@ -2823,6 +2897,34 @@ static int mwifiex_set_mef_filter(struct mwifiex_private *priv,
 		mef_entry->filter[filt_num].filt_type = TYPE_EQ;
 		mef_entry->filter[filt_num].filt_action = TYPE_OR;
 	}
+	return ret;
+}
+
+static int mwifiex_set_mef_filter(struct mwifiex_private *priv,
+				  struct cfg80211_wowlan *wowlan)
+{
+	int ret = 0, num_entries = 1;
+	struct mwifiex_ds_mef_cfg mef_cfg;
+	struct mwifiex_mef_entry *mef_entry;
+
+	if (wowlan->n_patterns || wowlan->magic_pkt)
+		num_entries++;
+
+	mef_entry = kcalloc(num_entries, sizeof(*mef_entry), GFP_KERNEL);
+	if (!mef_entry)
+		return -ENOMEM;
+
+	memset(&mef_cfg, 0, sizeof(mef_cfg));
+	mef_cfg.criteria |= MWIFIEX_CRITERIA_BROADCAST |
+		MWIFIEX_CRITERIA_UNICAST;
+	mef_cfg.num_entries = num_entries;
+	mef_cfg.mef_entry = mef_entry;
+
+	mwifiex_set_auto_arp_mef_entry(priv, &mef_entry[0]);
+
+	if (wowlan->n_patterns || wowlan->magic_pkt)
+		ret = mwifiex_set_wowlan_mef_entry(priv, &mef_cfg,
+						   &mef_entry[1], wowlan);
 
 	if (!mef_cfg.criteria)
 		mef_cfg.criteria = MWIFIEX_CRITERIA_BROADCAST |
@@ -2830,8 +2932,8 @@ static int mwifiex_set_mef_filter(struct mwifiex_private *priv,
 			MWIFIEX_CRITERIA_MULTICAST;
 
 	ret = mwifiex_send_cmd(priv, HostCmd_CMD_MEF_CFG,
-			HostCmd_ACT_GEN_SET, 0, &mef_cfg, true);
-
+			HostCmd_ACT_GEN_SET, 0,
+			&mef_cfg, true);
 	kfree(mef_entry);
 	return ret;
 }
@@ -2841,27 +2943,33 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 {
 	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
 	struct mwifiex_ds_hs_cfg hs_cfg;
-	int ret = 0;
-	struct mwifiex_private *priv =
-			mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
+	int i, ret = 0;
+	struct mwifiex_private *priv;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		priv = adapter->priv[i];
+		mwifiex_abort_cac(priv);
+	}
+
+	mwifiex_cancel_all_pending_cmd(adapter);
 
 	if (!wowlan) {
 		dev_warn(adapter->dev, "None of the WOWLAN triggers enabled\n");
 		return 0;
 	}
 
+	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
+
 	if (!priv->media_connected) {
 		dev_warn(adapter->dev,
 			 "Can not configure WOWLAN in disconnected state\n");
 		return 0;
 	}
 
-	if (wowlan->n_patterns || wowlan->magic_pkt) {
-		ret = mwifiex_set_mef_filter(priv, wowlan);
-		if (ret) {
-			dev_err(adapter->dev, "Failed to set MEF filter\n");
-			return ret;
-		}
+	ret = mwifiex_set_mef_filter(priv, wowlan);
+	if (ret) {
+		dev_err(adapter->dev, "Failed to set MEF filter\n");
+		return ret;
 	}
 
 	if (wowlan->disconnect) {
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index 88d0eade6bb1..38f24e0427d2 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -33,6 +33,7 @@
 #define MWIFIEX_MAX_BSS_NUM         (3)
 
 #define MWIFIEX_DMA_ALIGN_SZ	    64
+#define MWIFIEX_RX_HEADROOM	    64
 #define MAX_TXPD_SZ		    32
 #define INTF_HDR_ALIGN		     4
 
@@ -82,6 +83,7 @@
 #define MWIFIEX_BUF_FLAG_TDLS_PKT	   BIT(2)
 #define MWIFIEX_BUF_FLAG_EAPOL_TX_STATUS   BIT(3)
 #define MWIFIEX_BUF_FLAG_ACTION_TX_STATUS  BIT(4)
+#define MWIFIEX_BUF_FLAG_AGGR_PKT          BIT(5)
 
 #define MWIFIEX_BRIDGED_PKTS_THR_HIGH      1024
 #define MWIFIEX_BRIDGED_PKTS_THR_LOW        128
@@ -110,6 +112,11 @@
 
 #define MWIFIEX_A_BAND_START_FREQ	5000
 
+/* SDIO Aggr data packet special info */
+#define SDIO_MAX_AGGR_BUF_SIZE		(256 * 255)
+#define BLOCK_NUMBER_OFFSET		15
+#define SDIO_HEADER_OFFSET		28
+
 enum mwifiex_bss_type {
 	MWIFIEX_BSS_TYPE_STA = 0,
 	MWIFIEX_BSS_TYPE_UAP = 1,
@@ -167,10 +174,11 @@ struct mwifiex_wait_queue {
 };
 
 struct mwifiex_rxinfo {
+	struct sk_buff *parent;
 	u8 bss_num;
 	u8 bss_type;
-	struct sk_buff *parent;
 	u8 use_count;
+	u8 buf_type;
 };
 
 struct mwifiex_txinfo {
@@ -178,6 +186,7 @@ struct mwifiex_txinfo {
 	u8 flags;
 	u8 bss_num;
 	u8 bss_type;
+	u8 aggr_num;
 	u32 pkt_len;
 	u8 ack_frame_id;
 	u64 cookie;
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index df553e86a0ad..59d8964dd0dc 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -197,6 +197,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define ISSUPP_11NENABLED(FwCapInfo) (FwCapInfo & BIT(11))
 #define ISSUPP_TDLS_ENABLED(FwCapInfo) (FwCapInfo & BIT(14))
+#define ISSUPP_SDIO_SPA_ENABLED(FwCapInfo) (FwCapInfo & BIT(16))
 
 #define MWIFIEX_DEF_HT_CAP	(IEEE80211_HT_CAP_DSSSCCK40 | \
 				 (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT) | \
@@ -353,6 +354,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_REMAIN_ON_CHAN                    0x010d
 #define HostCmd_CMD_11AC_CFG			      0x0112
 #define HostCmd_CMD_TDLS_OPER                         0x0122
+#define HostCmd_CMD_SDIO_SP_RX_AGGR_CFG               0x0223
 
 #define PROTOCOL_NO_SECURITY        0x01
 #define PROTOCOL_STATIC_WEP         0x02
@@ -523,9 +525,11 @@ enum P2P_MODES {
 #define TYPE_OR				(MAX_OPERAND+5)
 #define MEF_MODE_HOST_SLEEP			1
 #define MEF_ACTION_ALLOW_AND_WAKEUP_HOST	3
+#define MEF_ACTION_AUTO_ARP                    0x10
 #define MWIFIEX_CRITERIA_BROADCAST	BIT(0)
 #define MWIFIEX_CRITERIA_UNICAST	BIT(1)
 #define MWIFIEX_CRITERIA_MULTICAST	BIT(3)
+#define MWIFIEX_MAX_SUPPORTED_IPADDR              4
 
 #define ACT_TDLS_DELETE            0x00
 #define ACT_TDLS_CREATE            0x01
@@ -1240,6 +1244,12 @@ struct host_cmd_ds_chan_rpt_event {
 	u8 tlvbuf[0];
 } __packed;
 
+struct host_cmd_sdio_sp_rx_aggr_cfg {
+	u8 action;
+	u8 enable;
+	__le16 block_size;
+} __packed;
+
 struct mwifiex_fixed_bcn_param {
 	__le64 timestamp;
 	__le16 beacon_period;
@@ -1962,6 +1972,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_coalesce_cfg coalesce_cfg;
 		struct host_cmd_ds_tdls_oper tdls_oper;
 		struct host_cmd_ds_chan_rpt_req chan_rpt_req;
+		struct host_cmd_sdio_sp_rx_aggr_cfg sdio_rx_aggr_cfg;
 	} params;
 } __packed;
 
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index b77ba743e1c4..e12192f5cfad 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -76,7 +76,7 @@ int mwifiex_init_priv(struct mwifiex_private *priv)
 	u32 i;
 
 	priv->media_connected = false;
-	memset(priv->curr_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(priv->curr_addr);
 
 	priv->pkt_tx_ctrl = 0;
 	priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
@@ -266,18 +266,15 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 
 	mwifiex_wmm_init(adapter);
 
-	if (adapter->sleep_cfm) {
-		sleep_cfm_buf = (struct mwifiex_opt_sleep_confirm *)
-						adapter->sleep_cfm->data;
-		memset(sleep_cfm_buf, 0, adapter->sleep_cfm->len);
-		sleep_cfm_buf->command =
-				cpu_to_le16(HostCmd_CMD_802_11_PS_MODE_ENH);
-		sleep_cfm_buf->size =
-				cpu_to_le16(adapter->sleep_cfm->len);
-		sleep_cfm_buf->result = 0;
-		sleep_cfm_buf->action = cpu_to_le16(SLEEP_CONFIRM);
-		sleep_cfm_buf->resp_ctrl = cpu_to_le16(RESP_NEEDED);
-	}
+	sleep_cfm_buf = (struct mwifiex_opt_sleep_confirm *)
+					adapter->sleep_cfm->data;
+	memset(sleep_cfm_buf, 0, adapter->sleep_cfm->len);
+	sleep_cfm_buf->command = cpu_to_le16(HostCmd_CMD_802_11_PS_MODE_ENH);
+	sleep_cfm_buf->size = cpu_to_le16(adapter->sleep_cfm->len);
+	sleep_cfm_buf->result = 0;
+	sleep_cfm_buf->action = cpu_to_le16(SLEEP_CONFIRM);
+	sleep_cfm_buf->resp_ctrl = cpu_to_le16(RESP_NEEDED);
+
 	memset(&adapter->sleep_params, 0, sizeof(adapter->sleep_params));
 	memset(&adapter->sleep_period, 0, sizeof(adapter->sleep_period));
 	adapter->tx_lock_flag = false;
@@ -296,10 +293,9 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	memset(&adapter->arp_filter, 0, sizeof(adapter->arp_filter));
 	adapter->arp_filter_size = 0;
 	adapter->max_mgmt_ie_index = MAX_MGMT_IE_INDEX;
-	adapter->ext_scan = false;
 	adapter->key_api_major_ver = 0;
 	adapter->key_api_minor_ver = 0;
-	memset(adapter->perm_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(adapter->perm_addr);
 	adapter->iface_limit.sta_intf = MWIFIEX_MAX_STA_NUM;
 	adapter->iface_limit.uap_intf = MWIFIEX_MAX_UAP_NUM;
 	adapter->iface_limit.p2p_intf = MWIFIEX_MAX_P2P_NUM;
@@ -482,6 +478,7 @@ int mwifiex_init_lock_list(struct mwifiex_adapter *adapter)
 	spin_lock_init(&adapter->rx_proc_lock);
 
 	skb_queue_head_init(&adapter->rx_data_q);
+	skb_queue_head_init(&adapter->tx_data_q);
 
 	for (i = 0; i < adapter->priv_num; ++i) {
 		INIT_LIST_HEAD(&adapter->bss_prio_tbl[i].bss_prio_head);
@@ -689,6 +686,10 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 		}
 	}
 
+	atomic_set(&adapter->tx_queued, 0);
+	while ((skb = skb_dequeue(&adapter->tx_data_q)))
+		mwifiex_write_data_complete(adapter, skb, 0, 0);
+
 	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
 
 	while ((skb = skb_dequeue(&adapter->rx_data_q))) {
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 7e74b4fccddd..03a95c7d34bf 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -131,10 +131,39 @@ static int mwifiex_unregister(struct mwifiex_adapter *adapter)
 	return 0;
 }
 
+void mwifiex_queue_main_work(struct mwifiex_adapter *adapter)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&adapter->main_proc_lock, flags);
+	if (adapter->mwifiex_processing) {
+		adapter->more_task_flag = true;
+		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
+	} else {
+		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
+		queue_work(adapter->workqueue, &adapter->main_work);
+	}
+}
+EXPORT_SYMBOL_GPL(mwifiex_queue_main_work);
+
+static void mwifiex_queue_rx_work(struct mwifiex_adapter *adapter)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
+	if (adapter->rx_processing) {
+		spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+	} else {
+		spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+		queue_work(adapter->rx_workqueue, &adapter->rx_work);
+	}
+}
+
 static int mwifiex_process_rx(struct mwifiex_adapter *adapter)
 {
 	unsigned long flags;
 	struct sk_buff *skb;
+	struct mwifiex_rxinfo *rx_info;
 
 	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
 	if (adapter->rx_processing || adapter->rx_locked) {
@@ -154,9 +183,16 @@ static int mwifiex_process_rx(struct mwifiex_adapter *adapter)
 			if (adapter->if_ops.submit_rem_rx_urbs)
 				adapter->if_ops.submit_rem_rx_urbs(adapter);
 			adapter->delay_main_work = false;
-			queue_work(adapter->workqueue, &adapter->main_work);
+			mwifiex_queue_main_work(adapter);
+		}
+		rx_info = MWIFIEX_SKB_RXCB(skb);
+		if (rx_info->buf_type == MWIFIEX_TYPE_AGGR_DATA) {
+			if (adapter->if_ops.deaggr_pkt)
+				adapter->if_ops.deaggr_pkt(adapter, skb);
+			dev_kfree_skb_any(skb);
+		} else {
+			mwifiex_handle_rx_packet(adapter, skb);
 		}
-		mwifiex_handle_rx_packet(adapter, skb);
 	}
 	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
 	adapter->rx_processing = false;
@@ -189,15 +225,17 @@ int mwifiex_main_process(struct mwifiex_adapter *adapter)
 	spin_lock_irqsave(&adapter->main_proc_lock, flags);
 
 	/* Check if already processing */
-	if (adapter->mwifiex_processing) {
+	if (adapter->mwifiex_processing || adapter->main_locked) {
+		adapter->more_task_flag = true;
 		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 		goto exit_main_proc;
 	} else {
 		adapter->mwifiex_processing = true;
-		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 	}
 process_start:
 	do {
+		adapter->more_task_flag = false;
+		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 		if ((adapter->hw_status == MWIFIEX_HW_STATUS_CLOSING) ||
 		    (adapter->hw_status == MWIFIEX_HW_STATUS_NOT_READY))
 			break;
@@ -212,9 +250,7 @@ process_start:
 		if (atomic_read(&adapter->rx_pending) >= HIGH_RX_PENDING &&
 		    adapter->iface_type != MWIFIEX_USB) {
 			adapter->delay_main_work = true;
-			if (!adapter->rx_processing)
-				queue_work(adapter->rx_workqueue,
-					   &adapter->rx_work);
+			mwifiex_queue_rx_work(adapter);
 			break;
 		}
 
@@ -227,24 +263,26 @@ process_start:
 		}
 
 		if (adapter->rx_work_enabled && adapter->data_received)
-			queue_work(adapter->rx_workqueue, &adapter->rx_work);
+			mwifiex_queue_rx_work(adapter);
 
 		/* Need to wake up the card ? */
 		if ((adapter->ps_state == PS_STATE_SLEEP) &&
 		    (adapter->pm_wakeup_card_req &&
 		     !adapter->pm_wakeup_fw_try) &&
 		    (is_command_pending(adapter) ||
+		     !skb_queue_empty(&adapter->tx_data_q) ||
 		     !mwifiex_wmm_lists_empty(adapter))) {
 			adapter->pm_wakeup_fw_try = true;
 			mod_timer(&adapter->wakeup_timer, jiffies + (HZ*3));
 			adapter->if_ops.wakeup(adapter);
+			spin_lock_irqsave(&adapter->main_proc_lock, flags);
 			continue;
 		}
 
 		if (IS_CARD_RX_RCVD(adapter)) {
 			adapter->data_received = false;
 			adapter->pm_wakeup_fw_try = false;
-			del_timer_sync(&adapter->wakeup_timer);
+			del_timer(&adapter->wakeup_timer);
 			if (adapter->ps_state == PS_STATE_SLEEP)
 				adapter->ps_state = PS_STATE_AWAKE;
 		} else {
@@ -257,7 +295,8 @@ process_start:
 
 			if ((!adapter->scan_chan_gap_enabled &&
 			     adapter->scan_processing) || adapter->data_sent ||
-			    mwifiex_wmm_lists_empty(adapter)) {
+			    (mwifiex_wmm_lists_empty(adapter) &&
+			     skb_queue_empty(&adapter->tx_data_q))) {
 				if (adapter->cmd_sent || adapter->curr_cmd ||
 				    (!is_command_pending(adapter)))
 					break;
@@ -295,8 +334,10 @@ process_start:
 		if ((adapter->ps_state == PS_STATE_SLEEP) ||
 		    (adapter->ps_state == PS_STATE_PRE_SLEEP) ||
 		    (adapter->ps_state == PS_STATE_SLEEP_CFM) ||
-		    adapter->tx_lock_flag)
+		    adapter->tx_lock_flag){
+			spin_lock_irqsave(&adapter->main_proc_lock, flags);
 			continue;
+		}
 
 		if (!adapter->cmd_sent && !adapter->curr_cmd) {
 			if (mwifiex_exec_next_cmd(adapter) == -1) {
@@ -307,6 +348,20 @@ process_start:
 
 		if ((adapter->scan_chan_gap_enabled ||
 		     !adapter->scan_processing) &&
+		    !adapter->data_sent &&
+		    !skb_queue_empty(&adapter->tx_data_q)) {
+			mwifiex_process_tx_queue(adapter);
+			if (adapter->hs_activated) {
+				adapter->is_hs_configured = false;
+				mwifiex_hs_activated_event
+					(mwifiex_get_priv
+					(adapter, MWIFIEX_BSS_ROLE_ANY),
+					false);
+			}
+		}
+
+		if ((adapter->scan_chan_gap_enabled ||
+		     !adapter->scan_processing) &&
 		    !adapter->data_sent && !mwifiex_wmm_lists_empty(adapter)) {
 			mwifiex_wmm_process_tx(adapter);
 			if (adapter->hs_activated) {
@@ -320,7 +375,8 @@ process_start:
 
 		if (adapter->delay_null_pkt && !adapter->cmd_sent &&
 		    !adapter->curr_cmd && !is_command_pending(adapter) &&
-		    mwifiex_wmm_lists_empty(adapter)) {
+		    (mwifiex_wmm_lists_empty(adapter) &&
+		     skb_queue_empty(&adapter->tx_data_q))) {
 			if (!mwifiex_send_null_packet
 			    (mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA),
 			     MWIFIEX_TxPD_POWER_MGMT_NULL_PACKET |
@@ -330,15 +386,12 @@ process_start:
 			}
 			break;
 		}
+		spin_lock_irqsave(&adapter->main_proc_lock, flags);
 	} while (true);
 
 	spin_lock_irqsave(&adapter->main_proc_lock, flags);
-	if (!adapter->delay_main_work &&
-	    (adapter->int_status || IS_CARD_RX_RCVD(adapter))) {
-		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
+	if (adapter->more_task_flag)
 		goto process_start;
-	}
-
 	adapter->mwifiex_processing = false;
 	spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 
@@ -466,7 +519,7 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 
 	rtnl_lock();
 	/* Create station interface by default */
-	wdev = mwifiex_add_virtual_intf(adapter->wiphy, "mlan%d",
+	wdev = mwifiex_add_virtual_intf(adapter->wiphy, "mlan%d", NET_NAME_ENUM,
 					NL80211_IFTYPE_STATION, NULL, NULL);
 	if (IS_ERR(wdev)) {
 		dev_err(adapter->dev, "cannot create default STA interface\n");
@@ -475,7 +528,7 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	}
 
 	if (driver_mode & MWIFIEX_DRIVER_MODE_UAP) {
-		wdev = mwifiex_add_virtual_intf(adapter->wiphy, "uap%d",
+		wdev = mwifiex_add_virtual_intf(adapter->wiphy, "uap%d", NET_NAME_ENUM,
 						NL80211_IFTYPE_AP, NULL, NULL);
 		if (IS_ERR(wdev)) {
 			dev_err(adapter->dev, "cannot create AP interface\n");
@@ -485,7 +538,7 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	}
 
 	if (driver_mode & MWIFIEX_DRIVER_MODE_P2P) {
-		wdev = mwifiex_add_virtual_intf(adapter->wiphy, "p2p%d",
+		wdev = mwifiex_add_virtual_intf(adapter->wiphy, "p2p%d", NET_NAME_ENUM,
 						NL80211_IFTYPE_P2P_CLIENT, NULL,
 						NULL);
 		if (IS_ERR(wdev)) {
@@ -604,7 +657,7 @@ int mwifiex_queue_tx_pkt(struct mwifiex_private *priv, struct sk_buff *skb)
 	atomic_inc(&priv->adapter->tx_pending);
 	mwifiex_wmm_add_buf_txqueue(priv, skb);
 
-	queue_work(priv->adapter->workqueue, &priv->adapter->main_work);
+	mwifiex_queue_main_work(priv->adapter);
 
 	return 0;
 }
@@ -1096,9 +1149,6 @@ mwifiex_add_card(void *card, struct semaphore *sem,
 		INIT_WORK(&adapter->rx_work, mwifiex_rx_work_queue);
 	}
 
-	if (adapter->if_ops.iface_work)
-		INIT_WORK(&adapter->iface_work, adapter->if_ops.iface_work);
-
 	/* Register the device. Fill up the private data structure with relevant
 	   information from the card. */
 	if (adapter->if_ops.register_dev(adapter)) {
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index f0a6af179af0..fe1256044a6c 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -35,6 +35,7 @@
 #include <linux/ctype.h>
 #include <linux/of.h>
 #include <linux/idr.h>
+#include <linux/inetdevice.h>
 
 #include "decl.h"
 #include "ioctl.h"
@@ -58,6 +59,8 @@ enum {
 
 #define MWIFIEX_MAX_AP				64
 
+#define MWIFIEX_MAX_PKTS_TXQ			16
+
 #define MWIFIEX_DEFAULT_WATCHDOG_TIMEOUT	(5 * HZ)
 
 #define MWIFIEX_TIMER_10S			10000
@@ -118,6 +121,7 @@ enum {
 
 #define MWIFIEX_TYPE_CMD				1
 #define MWIFIEX_TYPE_DATA				0
+#define MWIFIEX_TYPE_AGGR_DATA				10
 #define MWIFIEX_TYPE_EVENT				3
 
 #define MAX_BITMAP_RATES_SIZE			18
@@ -140,6 +144,9 @@ enum {
 
 #define MWIFIEX_DRV_INFO_SIZE_MAX 0x40000
 
+/* Address alignment */
+#define MWIFIEX_ALIGN_ADDR(p, a) (((long)(p) + (a) - 1) & ~((a) - 1))
+
 struct mwifiex_dbg {
 	u32 num_cmd_host_to_card_failure;
 	u32 num_cmd_sleep_cfm_host_to_card_failure;
@@ -207,6 +214,12 @@ struct mwifiex_tx_aggr {
 	u8 amsdu;
 };
 
+enum mwifiex_ba_status {
+	BA_SETUP_NONE = 0,
+	BA_SETUP_INPROGRESS,
+	BA_SETUP_COMPLETE
+};
+
 struct mwifiex_ra_list_tbl {
 	struct list_head list;
 	struct sk_buff_head skb_head;
@@ -215,6 +228,8 @@ struct mwifiex_ra_list_tbl {
 	u16 max_amsdu;
 	u16 ba_pkt_count;
 	u8 ba_packet_thr;
+	enum mwifiex_ba_status ba_status;
+	u8 amsdu_in_ampdu;
 	u16 total_pkt_count;
 	bool tdls_link;
 };
@@ -598,11 +613,6 @@ struct mwifiex_private {
 	struct mwifiex_11h_intf_state state_11h;
 };
 
-enum mwifiex_ba_status {
-	BA_SETUP_NONE = 0,
-	BA_SETUP_INPROGRESS,
-	BA_SETUP_COMPLETE
-};
 
 struct mwifiex_tx_ba_stream_tbl {
 	struct list_head list;
@@ -735,6 +745,7 @@ struct mwifiex_if_ops {
 	int (*clean_pcie_ring) (struct mwifiex_adapter *adapter);
 	void (*iface_work)(struct work_struct *work);
 	void (*submit_rem_rx_urbs)(struct mwifiex_adapter *adapter);
+	void (*deaggr_pkt)(struct mwifiex_adapter *, struct sk_buff *);
 };
 
 struct mwifiex_adapter {
@@ -768,14 +779,18 @@ struct mwifiex_adapter {
 	bool rx_processing;
 	bool delay_main_work;
 	bool rx_locked;
+	bool main_locked;
 	struct mwifiex_bss_prio_tbl bss_prio_tbl[MWIFIEX_MAX_BSS_NUM];
 	/* spin lock for init/shutdown */
 	spinlock_t mwifiex_lock;
 	/* spin lock for main process */
 	spinlock_t main_proc_lock;
 	u32 mwifiex_processing;
+	u8 more_task_flag;
 	u16 tx_buf_size;
 	u16 curr_tx_buf_size;
+	bool sdio_rx_aggr_enable;
+	u16 sdio_rx_block_size;
 	u32 ioport;
 	enum MWIFIEX_HARDWARE_STATUS hw_status;
 	u16 number_of_antenna;
@@ -810,6 +825,8 @@ struct mwifiex_adapter {
 	spinlock_t scan_pending_q_lock;
 	/* spin lock for RX processing routine */
 	spinlock_t rx_proc_lock;
+	struct sk_buff_head tx_data_q;
+	atomic_t tx_queued;
 	u32 scan_processing;
 	u16 region_code;
 	struct mwifiex_802_11d_domain_reg domain_reg;
@@ -881,8 +898,6 @@ struct mwifiex_adapter {
 	bool ext_scan;
 	u8 fw_api_ver;
 	u8 key_api_major_ver, key_api_minor_ver;
-	struct work_struct iface_work;
-	unsigned long iface_work_flags;
 	struct memory_type_mapping *mem_type_mapping_tbl;
 	u8 num_mem_types;
 	u8 curr_mem_idx;
@@ -896,6 +911,8 @@ struct mwifiex_adapter {
 	bool auto_tdls;
 };
 
+void mwifiex_process_tx_queue(struct mwifiex_adapter *adapter);
+
 int mwifiex_init_lock_list(struct mwifiex_adapter *adapter);
 
 void mwifiex_set_trans_start(struct net_device *dev);
@@ -1318,6 +1335,7 @@ u8 mwifiex_chan_type_to_sec_chan_offset(enum nl80211_channel_type chan_type);
 
 struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 					      const char *name,
+					      unsigned char name_assign_type,
 					      enum nl80211_iftype type,
 					      u32 *flags,
 					      struct vif_params *params);
@@ -1417,6 +1435,8 @@ u8 mwifiex_adjust_data_rate(struct mwifiex_private *priv,
 			    u8 rx_rate, u8 ht_info);
 
 void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter);
+void *mwifiex_alloc_dma_align_buf(int rx_len, gfp_t flags);
+void mwifiex_queue_main_work(struct mwifiex_adapter *adapter);
 
 #ifdef CONFIG_DEBUG_FS
 void mwifiex_debugfs_init(void);
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/mwifiex/pcie.c
index a5828da59365..bcc7751d883c 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -203,7 +203,7 @@ static int mwifiex_pcie_probe(struct pci_dev *pdev,
 		card->pcie.reg = data->reg;
 		card->pcie.blksz_fw_dl = data->blksz_fw_dl;
 		card->pcie.tx_buf_size = data->tx_buf_size;
-		card->pcie.supports_fw_dump = data->supports_fw_dump;
+		card->pcie.can_dump_fw = data->can_dump_fw;
 		card->pcie.can_ext_scan = data->can_ext_scan;
 	}
 
@@ -234,8 +234,6 @@ static void mwifiex_pcie_remove(struct pci_dev *pdev)
 	if (!adapter || !adapter->priv_num)
 		return;
 
-	cancel_work_sync(&adapter->iface_work);
-
 	if (user_rmmod) {
 #ifdef CONFIG_PM_SLEEP
 		if (adapter->is_suspended)
@@ -498,7 +496,8 @@ static int mwifiex_init_rxq_ring(struct mwifiex_adapter *adapter)
 
 	for (i = 0; i < MWIFIEX_MAX_TXRX_BD; i++) {
 		/* Allocate skb here so that firmware can DMA data from it */
-		skb = dev_alloc_skb(MWIFIEX_RX_DATA_BUF_SIZE);
+		skb = mwifiex_alloc_dma_align_buf(MWIFIEX_RX_DATA_BUF_SIZE,
+						  GFP_KERNEL | GFP_DMA);
 		if (!skb) {
 			dev_err(adapter->dev,
 				"Unable to allocate skb for RX ring.\n");
@@ -1297,7 +1296,8 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 			}
 		}
 
-		skb_tmp = dev_alloc_skb(MWIFIEX_RX_DATA_BUF_SIZE);
+		skb_tmp = mwifiex_alloc_dma_align_buf(MWIFIEX_RX_DATA_BUF_SIZE,
+						      GFP_KERNEL | GFP_DMA);
 		if (!skb_tmp) {
 			dev_err(adapter->dev,
 				"Unable to allocate skb.\n");
@@ -2099,7 +2099,7 @@ static irqreturn_t mwifiex_pcie_interrupt(int irq, void *context)
 		goto exit;
 
 	mwifiex_interrupt_status(adapter);
-	queue_work(adapter->workqueue, &adapter->main_work);
+	mwifiex_queue_main_work(adapter);
 
 exit:
 	return IRQ_HANDLED;
@@ -2271,7 +2271,7 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 	int ret;
 	static char *env[] = { "DRIVER=mwifiex_pcie", "EVENT=fw_dump", NULL };
 
-	if (!card->pcie.supports_fw_dump)
+	if (!card->pcie.can_dump_fw)
 		return;
 
 	for (idx = 0; idx < ARRAY_SIZE(mem_type_mapping_tbl); idx++) {
@@ -2371,25 +2371,26 @@ done:
 	adapter->curr_mem_idx = 0;
 }
 
+static unsigned long iface_work_flags;
+static struct mwifiex_adapter *save_adapter;
 static void mwifiex_pcie_work(struct work_struct *work)
 {
-	struct mwifiex_adapter *adapter =
-			container_of(work, struct mwifiex_adapter, iface_work);
-
 	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_FW_DUMP,
-			       &adapter->iface_work_flags))
-		mwifiex_pcie_fw_dump_work(adapter);
+			       &iface_work_flags))
+		mwifiex_pcie_fw_dump_work(save_adapter);
 }
 
+static DECLARE_WORK(pcie_work, mwifiex_pcie_work);
 /* This function dumps FW information */
 static void mwifiex_pcie_fw_dump(struct mwifiex_adapter *adapter)
 {
-	if (test_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &adapter->iface_work_flags))
+	save_adapter = adapter;
+	if (test_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags))
 		return;
 
-	set_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &adapter->iface_work_flags);
+	set_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags);
 
-	schedule_work(&adapter->iface_work);
+	schedule_work(&pcie_work);
 }
 
 /*
@@ -2617,7 +2618,6 @@ static struct mwifiex_if_ops pcie_ops = {
 	.init_fw_port =			mwifiex_pcie_init_fw_port,
 	.clean_pcie_ring =		mwifiex_clean_pcie_ring_buf,
 	.fw_dump =			mwifiex_pcie_fw_dump,
-	.iface_work =			mwifiex_pcie_work,
 };
 
 /*
@@ -2663,6 +2663,7 @@ static void mwifiex_pcie_cleanup_module(void)
 	/* Set the flag as user is removing this module. */
 	user_rmmod = 1;
 
+	cancel_work_sync(&pcie_work);
 	pci_unregister_driver(&mwifiex_pcie);
 }
 
diff --git a/drivers/net/wireless/mwifiex/pcie.h b/drivers/net/wireless/mwifiex/pcie.h
index 666d40e9dbc3..0e7ee8b72358 100644
--- a/drivers/net/wireless/mwifiex/pcie.h
+++ b/drivers/net/wireless/mwifiex/pcie.h
@@ -205,7 +205,7 @@ struct mwifiex_pcie_device {
 	const struct mwifiex_pcie_card_reg *reg;
 	u16 blksz_fw_dl;
 	u16 tx_buf_size;
-	bool supports_fw_dump;
+	bool can_dump_fw;
 	bool can_ext_scan;
 };
 
@@ -214,7 +214,7 @@ static const struct mwifiex_pcie_device mwifiex_pcie8766 = {
 	.reg            = &mwifiex_reg_8766,
 	.blksz_fw_dl = MWIFIEX_PCIE_BLOCK_SIZE_FW_DNLD,
 	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
-	.supports_fw_dump = false,
+	.can_dump_fw = false,
 	.can_ext_scan = true,
 };
 
@@ -223,7 +223,7 @@ static const struct mwifiex_pcie_device mwifiex_pcie8897 = {
 	.reg            = &mwifiex_reg_8897,
 	.blksz_fw_dl = MWIFIEX_PCIE_BLOCK_SIZE_FW_DNLD,
 	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K,
-	.supports_fw_dump = true,
+	.can_dump_fw = true,
 	.can_ext_scan = true,
 };
 
diff --git a/drivers/net/wireless/mwifiex/sdio.c b/drivers/net/wireless/mwifiex/sdio.c
index 91e36cda9543..d10320f89bc1 100644
--- a/drivers/net/wireless/mwifiex/sdio.c
+++ b/drivers/net/wireless/mwifiex/sdio.c
@@ -47,6 +47,7 @@
 static u8 user_rmmod;
 
 static struct mwifiex_if_ops sdio_ops;
+static unsigned long iface_work_flags;
 
 static struct semaphore add_remove_card_sem;
 
@@ -105,8 +106,8 @@ mwifiex_sdio_probe(struct sdio_func *func, const struct sdio_device_id *id)
 		card->tx_buf_size = data->tx_buf_size;
 		card->mp_tx_agg_buf_size = data->mp_tx_agg_buf_size;
 		card->mp_rx_agg_buf_size = data->mp_rx_agg_buf_size;
-		card->supports_fw_dump = data->supports_fw_dump;
-		card->auto_tdls = data->auto_tdls;
+		card->can_dump_fw = data->can_dump_fw;
+		card->can_auto_tdls = data->can_auto_tdls;
 		card->can_ext_scan = data->can_ext_scan;
 	}
 
@@ -200,8 +201,6 @@ mwifiex_sdio_remove(struct sdio_func *func)
 	if (!adapter || !adapter->priv_num)
 		return;
 
-	cancel_work_sync(&adapter->iface_work);
-
 	if (user_rmmod) {
 		if (adapter->is_suspended)
 			mwifiex_sdio_resume(adapter->dev);
@@ -1043,6 +1042,59 @@ static int mwifiex_check_fw_status(struct mwifiex_adapter *adapter,
 }
 
 /*
+ * This function decode sdio aggreation pkt.
+ *
+ * Based on the the data block size and pkt_len,
+ * skb data will be decoded to few packets.
+ */
+static void mwifiex_deaggr_sdio_pkt(struct mwifiex_adapter *adapter,
+				    struct sk_buff *skb)
+{
+	u32 total_pkt_len, pkt_len;
+	struct sk_buff *skb_deaggr;
+	u32 pkt_type;
+	u16 blk_size;
+	u8 blk_num;
+	u8 *data;
+
+	data = skb->data;
+	total_pkt_len = skb->len;
+
+	while (total_pkt_len >= (SDIO_HEADER_OFFSET + INTF_HEADER_LEN)) {
+		if (total_pkt_len < adapter->sdio_rx_block_size)
+			break;
+		blk_num = *(data + BLOCK_NUMBER_OFFSET);
+		blk_size = adapter->sdio_rx_block_size * blk_num;
+		if (blk_size > total_pkt_len) {
+			dev_err(adapter->dev, "%s: error in pkt,\t"
+				"blk_num=%d, blk_size=%d, total_pkt_len=%d\n",
+				__func__, blk_num, blk_size, total_pkt_len);
+			break;
+		}
+		pkt_len = le16_to_cpu(*(__le16 *)(data + SDIO_HEADER_OFFSET));
+		pkt_type = le16_to_cpu(*(__le16 *)(data + SDIO_HEADER_OFFSET +
+					 2));
+		if ((pkt_len + SDIO_HEADER_OFFSET) > blk_size) {
+			dev_err(adapter->dev, "%s: error in pkt,\t"
+				"pkt_len=%d, blk_size=%d\n",
+				__func__, pkt_len, blk_size);
+			break;
+		}
+		skb_deaggr = mwifiex_alloc_dma_align_buf(pkt_len,
+							 GFP_KERNEL | GFP_DMA);
+		if (!skb_deaggr)
+			break;
+		skb_put(skb_deaggr, pkt_len);
+		memcpy(skb_deaggr->data, data + SDIO_HEADER_OFFSET, pkt_len);
+		skb_pull(skb_deaggr, INTF_HEADER_LEN);
+
+		mwifiex_handle_rx_packet(adapter, skb_deaggr);
+		data += blk_size;
+		total_pkt_len -= blk_size;
+	}
+}
+
+/*
  * This function decodes a received packet.
  *
  * Based on the type, the packet is treated as either a data, or
@@ -1055,11 +1107,28 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 	u8 *cmd_buf;
 	__le16 *curr_ptr = (__le16 *)skb->data;
 	u16 pkt_len = le16_to_cpu(*curr_ptr);
+	struct mwifiex_rxinfo *rx_info;
 
-	skb_trim(skb, pkt_len);
-	skb_pull(skb, INTF_HEADER_LEN);
+	if (upld_typ != MWIFIEX_TYPE_AGGR_DATA) {
+		skb_trim(skb, pkt_len);
+		skb_pull(skb, INTF_HEADER_LEN);
+	}
 
 	switch (upld_typ) {
+	case MWIFIEX_TYPE_AGGR_DATA:
+		dev_dbg(adapter->dev, "info: --- Rx: Aggr Data packet ---\n");
+		rx_info = MWIFIEX_SKB_RXCB(skb);
+		rx_info->buf_type = MWIFIEX_TYPE_AGGR_DATA;
+		if (adapter->rx_work_enabled) {
+			skb_queue_tail(&adapter->rx_data_q, skb);
+			atomic_inc(&adapter->rx_pending);
+			adapter->data_received = true;
+		} else {
+			mwifiex_deaggr_sdio_pkt(adapter, skb);
+			dev_kfree_skb_any(skb);
+		}
+		break;
+
 	case MWIFIEX_TYPE_DATA:
 		dev_dbg(adapter->dev, "info: --- Rx: Data packet ---\n");
 		if (adapter->rx_work_enabled) {
@@ -1127,17 +1196,17 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
  * provided there is space left, processed and finally uploaded.
  */
 static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
-					     struct sk_buff *skb, u8 port)
+					     u16 rx_len, u8 port)
 {
 	struct sdio_mmc_card *card = adapter->card;
 	s32 f_do_rx_aggr = 0;
 	s32 f_do_rx_cur = 0;
 	s32 f_aggr_cur = 0;
+	s32 f_post_aggr_cur = 0;
 	struct sk_buff *skb_deaggr;
-	u32 pind;
-	u32 pkt_len, pkt_type, mport;
+	struct sk_buff *skb = NULL;
+	u32 pkt_len, pkt_type, mport, pind;
 	u8 *curr_ptr;
-	u32 rx_len = skb->len;
 
 	if ((card->has_control_mask) && (port == CTRL_PORT)) {
 		/* Read the command Resp without aggr */
@@ -1164,12 +1233,12 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 		dev_dbg(adapter->dev, "info: %s: not last packet\n", __func__);
 
 		if (MP_RX_AGGR_IN_PROGRESS(card)) {
-			if (MP_RX_AGGR_BUF_HAS_ROOM(card, skb->len)) {
+			if (MP_RX_AGGR_BUF_HAS_ROOM(card, rx_len)) {
 				f_aggr_cur = 1;
 			} else {
 				/* No room in Aggr buf, do rx aggr now */
 				f_do_rx_aggr = 1;
-				f_do_rx_cur = 1;
+				f_post_aggr_cur = 1;
 			}
 		} else {
 			/* Rx aggr not in progress */
@@ -1182,7 +1251,7 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 
 		if (MP_RX_AGGR_IN_PROGRESS(card)) {
 			f_do_rx_aggr = 1;
-			if (MP_RX_AGGR_BUF_HAS_ROOM(card, skb->len))
+			if (MP_RX_AGGR_BUF_HAS_ROOM(card, rx_len))
 				f_aggr_cur = 1;
 			else
 				/* No room in Aggr buf, do rx aggr now */
@@ -1195,7 +1264,7 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 	if (f_aggr_cur) {
 		dev_dbg(adapter->dev, "info: current packet aggregation\n");
 		/* Curr pkt can be aggregated */
-		mp_rx_aggr_setup(card, skb, port);
+		mp_rx_aggr_setup(card, rx_len, port);
 
 		if (MP_RX_AGGR_PKT_LIMIT_REACHED(card) ||
 		    mp_rx_aggr_port_limit_reached(card)) {
@@ -1238,16 +1307,29 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 		curr_ptr = card->mpa_rx.buf;
 
 		for (pind = 0; pind < card->mpa_rx.pkt_cnt; pind++) {
+			u32 *len_arr = card->mpa_rx.len_arr;
 
 			/* get curr PKT len & type */
 			pkt_len = le16_to_cpu(*(__le16 *) &curr_ptr[0]);
 			pkt_type = le16_to_cpu(*(__le16 *) &curr_ptr[2]);
 
 			/* copy pkt to deaggr buf */
-			skb_deaggr = card->mpa_rx.skb_arr[pind];
+			skb_deaggr = mwifiex_alloc_dma_align_buf(len_arr[pind],
+								 GFP_KERNEL |
+								 GFP_DMA);
+			if (!skb_deaggr) {
+				dev_err(adapter->dev, "skb allocation failure drop pkt len=%d type=%d\n",
+					pkt_len, pkt_type);
+				curr_ptr += len_arr[pind];
+				continue;
+			}
 
-			if ((pkt_type == MWIFIEX_TYPE_DATA) && (pkt_len <=
-					 card->mpa_rx.len_arr[pind])) {
+			skb_put(skb_deaggr, len_arr[pind]);
+
+			if ((pkt_type == MWIFIEX_TYPE_DATA ||
+			     (pkt_type == MWIFIEX_TYPE_AGGR_DATA &&
+			      adapter->sdio_rx_aggr_enable)) &&
+			    (pkt_len <= len_arr[pind])) {
 
 				memcpy(skb_deaggr->data, curr_ptr, pkt_len);
 
@@ -1257,13 +1339,15 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 				mwifiex_decode_rx_packet(adapter, skb_deaggr,
 							 pkt_type);
 			} else {
-				dev_err(adapter->dev, "wrong aggr pkt:"
-					" type=%d len=%d max_len=%d\n",
+				dev_err(adapter->dev, " drop wrong aggr pkt:\t"
+					"sdio_single_port_rx_aggr=%d\t"
+					"type=%d len=%d max_len=%d\n",
+					adapter->sdio_rx_aggr_enable,
 					pkt_type, pkt_len,
-					card->mpa_rx.len_arr[pind]);
+					len_arr[pind]);
 				dev_kfree_skb_any(skb_deaggr);
 			}
-			curr_ptr += card->mpa_rx.len_arr[pind];
+			curr_ptr += len_arr[pind];
 		}
 		MP_RX_AGGR_BUF_RESET(card);
 	}
@@ -1273,28 +1357,46 @@ rx_curr_single:
 		dev_dbg(adapter->dev, "info: RX: port: %d, rx_len: %d\n",
 			port, rx_len);
 
+		skb = mwifiex_alloc_dma_align_buf(rx_len, GFP_KERNEL | GFP_DMA);
+		if (!skb) {
+			dev_err(adapter->dev, "single skb allocated fail,\t"
+				"drop pkt port=%d len=%d\n", port, rx_len);
+			if (mwifiex_sdio_card_to_host(adapter, &pkt_type,
+						      card->mpa_rx.buf, rx_len,
+						      adapter->ioport + port))
+				goto error;
+			return 0;
+		}
+
+		skb_put(skb, rx_len);
+
 		if (mwifiex_sdio_card_to_host(adapter, &pkt_type,
 					      skb->data, skb->len,
 					      adapter->ioport + port))
 			goto error;
+		if (!adapter->sdio_rx_aggr_enable &&
+		    pkt_type == MWIFIEX_TYPE_AGGR_DATA) {
+			dev_err(adapter->dev, "drop wrong pkt type %d\t"
+				"current SDIO RX Aggr not enabled\n",
+				pkt_type);
+			dev_kfree_skb_any(skb);
+			return 0;
+		}
 
 		mwifiex_decode_rx_packet(adapter, skb, pkt_type);
 	}
+	if (f_post_aggr_cur) {
+		dev_dbg(adapter->dev, "info: current packet aggregation\n");
+		/* Curr pkt can be aggregated */
+		mp_rx_aggr_setup(card, rx_len, port);
+	}
 
 	return 0;
-
 error:
-	if (MP_RX_AGGR_IN_PROGRESS(card)) {
-		/* Multiport-aggregation transfer failed - cleanup */
-		for (pind = 0; pind < card->mpa_rx.pkt_cnt; pind++) {
-			/* copy pkt to deaggr buf */
-			skb_deaggr = card->mpa_rx.skb_arr[pind];
-			dev_kfree_skb_any(skb_deaggr);
-		}
+	if (MP_RX_AGGR_IN_PROGRESS(card))
 		MP_RX_AGGR_BUF_RESET(card);
-	}
 
-	if (f_do_rx_cur)
+	if (f_do_rx_cur && skb)
 		/* Single transfer pending. Free curr buff also */
 		dev_kfree_skb_any(skb);
 
@@ -1356,8 +1458,9 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		     MWIFIEX_RX_DATA_BUF_SIZE)
 			return -1;
 		rx_len = (u16) (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE);
+		dev_dbg(adapter->dev, "info: rx_len = %d\n", rx_len);
 
-		skb = dev_alloc_skb(rx_len);
+		skb = mwifiex_alloc_dma_align_buf(rx_len, GFP_KERNEL | GFP_DMA);
 		if (!skb)
 			return -1;
 
@@ -1447,27 +1550,16 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 				 1) / MWIFIEX_SDIO_BLOCK_SIZE;
 			if (rx_len <= INTF_HEADER_LEN ||
 			    (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE) >
-			     MWIFIEX_RX_DATA_BUF_SIZE) {
+			     card->mpa_rx.buf_size) {
 				dev_err(adapter->dev, "invalid rx_len=%d\n",
 					rx_len);
 				return -1;
 			}
-			rx_len = (u16) (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE);
-
-			skb = dev_alloc_skb(rx_len);
-
-			if (!skb) {
-				dev_err(adapter->dev, "%s: failed to alloc skb",
-					__func__);
-				return -1;
-			}
 
-			skb_put(skb, rx_len);
-
-			dev_dbg(adapter->dev, "info: rx_len = %d skb->len = %d\n",
-				rx_len, skb->len);
+			rx_len = (u16) (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE);
+			dev_dbg(adapter->dev, "info: rx_len = %d\n", rx_len);
 
-			if (mwifiex_sdio_card_to_host_mp_aggr(adapter, skb,
+			if (mwifiex_sdio_card_to_host_mp_aggr(adapter, rx_len,
 							      port)) {
 				dev_err(adapter->dev, "card_to_host_mpa failed:"
 					" int status=%#x\n", sdio_ireg);
@@ -1735,6 +1827,7 @@ static int mwifiex_alloc_sdio_mpa_buffers(struct mwifiex_adapter *adapter,
 				   u32 mpa_tx_buf_size, u32 mpa_rx_buf_size)
 {
 	struct sdio_mmc_card *card = adapter->card;
+	u32 rx_buf_size;
 	int ret = 0;
 
 	card->mpa_tx.buf = kzalloc(mpa_tx_buf_size, GFP_KERNEL);
@@ -1745,13 +1838,15 @@ static int mwifiex_alloc_sdio_mpa_buffers(struct mwifiex_adapter *adapter,
 
 	card->mpa_tx.buf_size = mpa_tx_buf_size;
 
-	card->mpa_rx.buf = kzalloc(mpa_rx_buf_size, GFP_KERNEL);
+	rx_buf_size = max_t(u32, mpa_rx_buf_size,
+			    (u32)SDIO_MAX_AGGR_BUF_SIZE);
+	card->mpa_rx.buf = kzalloc(rx_buf_size, GFP_KERNEL);
 	if (!card->mpa_rx.buf) {
 		ret = -1;
 		goto error;
 	}
 
-	card->mpa_rx.buf_size = mpa_rx_buf_size;
+	card->mpa_rx.buf_size = rx_buf_size;
 
 error:
 	if (ret) {
@@ -1887,7 +1982,7 @@ static int mwifiex_init_sdio(struct mwifiex_adapter *adapter)
 		return -1;
 	}
 
-	adapter->auto_tdls = card->auto_tdls;
+	adapter->auto_tdls = card->can_auto_tdls;
 	adapter->ext_scan = card->can_ext_scan;
 	return ret;
 }
@@ -1950,6 +2045,7 @@ mwifiex_update_mp_end_port(struct mwifiex_adapter *adapter, u16 port)
 		port, card->mp_data_port_mask);
 }
 
+static struct mwifiex_adapter *save_adapter;
 static void mwifiex_sdio_card_reset_work(struct mwifiex_adapter *adapter)
 {
 	struct sdio_mmc_card *card = adapter->card;
@@ -2018,10 +2114,8 @@ rdwr_status mwifiex_sdio_rdwr_firmware(struct mwifiex_adapter *adapter,
 }
 
 /* This function dump firmware memory to file */
-static void mwifiex_sdio_fw_dump_work(struct work_struct *work)
+static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
 {
-	struct mwifiex_adapter *adapter =
-			container_of(work, struct mwifiex_adapter, iface_work);
 	struct sdio_mmc_card *card = adapter->card;
 	int ret = 0;
 	unsigned int reg, reg_start, reg_end;
@@ -2032,7 +2126,7 @@ static void mwifiex_sdio_fw_dump_work(struct work_struct *work)
 
 	mwifiex_dump_drv_info(adapter);
 
-	if (!card->supports_fw_dump)
+	if (!card->can_dump_fw)
 		return;
 
 	for (idx = 0; idx < ARRAY_SIZE(mem_type_mapping_tbl); idx++) {
@@ -2143,36 +2237,36 @@ done:
 
 static void mwifiex_sdio_work(struct work_struct *work)
 {
-	struct mwifiex_adapter *adapter =
-			container_of(work, struct mwifiex_adapter, iface_work);
-
-	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_CARD_RESET,
-			       &adapter->iface_work_flags))
-		mwifiex_sdio_card_reset_work(adapter);
 	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_FW_DUMP,
-			       &adapter->iface_work_flags))
-		mwifiex_sdio_fw_dump_work(work);
+			       &iface_work_flags))
+		mwifiex_sdio_fw_dump_work(save_adapter);
+	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_CARD_RESET,
+			       &iface_work_flags))
+		mwifiex_sdio_card_reset_work(save_adapter);
 }
 
+static DECLARE_WORK(sdio_work, mwifiex_sdio_work);
 /* This function resets the card */
 static void mwifiex_sdio_card_reset(struct mwifiex_adapter *adapter)
 {
-	if (test_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &adapter->iface_work_flags))
+	save_adapter = adapter;
+	if (test_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &iface_work_flags))
 		return;
 
-	set_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &adapter->iface_work_flags);
+	set_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &iface_work_flags);
 
-	schedule_work(&adapter->iface_work);
+	schedule_work(&sdio_work);
 }
 
 /* This function dumps FW information */
 static void mwifiex_sdio_fw_dump(struct mwifiex_adapter *adapter)
 {
-	if (test_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &adapter->iface_work_flags))
+	save_adapter = adapter;
+	if (test_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags))
 		return;
 
-	set_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &adapter->iface_work_flags);
-	schedule_work(&adapter->iface_work);
+	set_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags);
+	schedule_work(&sdio_work);
 }
 
 /* Function to dump SDIO function registers and SDIO scratch registers in case
@@ -2288,9 +2382,9 @@ static struct mwifiex_if_ops sdio_ops = {
 	.cmdrsp_complete = mwifiex_sdio_cmdrsp_complete,
 	.event_complete = mwifiex_sdio_event_complete,
 	.card_reset = mwifiex_sdio_card_reset,
-	.iface_work = mwifiex_sdio_work,
 	.fw_dump = mwifiex_sdio_fw_dump,
 	.reg_dump = mwifiex_sdio_reg_dump,
+	.deaggr_pkt = mwifiex_deaggr_sdio_pkt,
 };
 
 /*
@@ -2327,6 +2421,7 @@ mwifiex_sdio_cleanup_module(void)
 
 	/* Set the flag as user is removing this module. */
 	user_rmmod = 1;
+	cancel_work_sync(&sdio_work);
 
 	sdio_unregister_driver(&mwifiex_sdio);
 }
diff --git a/drivers/net/wireless/mwifiex/sdio.h b/drivers/net/wireless/mwifiex/sdio.h
index 957cca246618..6f645cf47369 100644
--- a/drivers/net/wireless/mwifiex/sdio.h
+++ b/drivers/net/wireless/mwifiex/sdio.h
@@ -67,6 +67,8 @@
 
 #define MWIFIEX_MP_AGGR_BUF_SIZE_16K	(16384)
 #define MWIFIEX_MP_AGGR_BUF_SIZE_32K	(32768)
+/* we leave one block of 256 bytes for DMA alignment*/
+#define MWIFIEX_MP_AGGR_BUF_SIZE_MAX    (65280)
 
 /* Misc. Config Register : Auto Re-enable interrupts */
 #define AUTO_RE_ENABLE_INT              BIT(4)
@@ -238,9 +240,6 @@ struct sdio_mmc_card {
 	const struct mwifiex_sdio_card_reg *reg;
 	u8 max_ports;
 	u8 mp_agg_pkt_limit;
-	bool supports_sdio_new_mode;
-	bool has_control_mask;
-	bool supports_fw_dump;
 	u16 tx_buf_size;
 	u32 mp_tx_agg_buf_size;
 	u32 mp_rx_agg_buf_size;
@@ -255,7 +254,10 @@ struct sdio_mmc_card {
 	u8 curr_wr_port;
 
 	u8 *mp_regs;
-	u8 auto_tdls;
+	bool supports_sdio_new_mode;
+	bool has_control_mask;
+	bool can_dump_fw;
+	bool can_auto_tdls;
 	bool can_ext_scan;
 
 	struct mwifiex_sdio_mpa_tx mpa_tx;
@@ -267,13 +269,13 @@ struct mwifiex_sdio_device {
 	const struct mwifiex_sdio_card_reg *reg;
 	u8 max_ports;
 	u8 mp_agg_pkt_limit;
-	bool supports_sdio_new_mode;
-	bool has_control_mask;
-	bool supports_fw_dump;
 	u16 tx_buf_size;
 	u32 mp_tx_agg_buf_size;
 	u32 mp_rx_agg_buf_size;
-	u8 auto_tdls;
+	bool supports_sdio_new_mode;
+	bool has_control_mask;
+	bool can_dump_fw;
+	bool can_auto_tdls;
 	bool can_ext_scan;
 };
 
@@ -412,13 +414,13 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8786 = {
 	.reg = &mwifiex_reg_sd87xx,
 	.max_ports = 16,
 	.mp_agg_pkt_limit = 8,
-	.supports_sdio_new_mode = false,
-	.has_control_mask = true,
 	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
 	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
-	.supports_fw_dump = false,
-	.auto_tdls = false,
+	.supports_sdio_new_mode = false,
+	.has_control_mask = true,
+	.can_dump_fw = false,
+	.can_auto_tdls = false,
 	.can_ext_scan = false,
 };
 
@@ -427,13 +429,13 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8787 = {
 	.reg = &mwifiex_reg_sd87xx,
 	.max_ports = 16,
 	.mp_agg_pkt_limit = 8,
-	.supports_sdio_new_mode = false,
-	.has_control_mask = true,
 	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
 	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
-	.supports_fw_dump = false,
-	.auto_tdls = false,
+	.supports_sdio_new_mode = false,
+	.has_control_mask = true,
+	.can_dump_fw = false,
+	.can_auto_tdls = false,
 	.can_ext_scan = true,
 };
 
@@ -442,13 +444,13 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8797 = {
 	.reg = &mwifiex_reg_sd87xx,
 	.max_ports = 16,
 	.mp_agg_pkt_limit = 8,
-	.supports_sdio_new_mode = false,
-	.has_control_mask = true,
 	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
 	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
-	.supports_fw_dump = false,
-	.auto_tdls = false,
+	.supports_sdio_new_mode = false,
+	.has_control_mask = true,
+	.can_dump_fw = false,
+	.can_auto_tdls = false,
 	.can_ext_scan = true,
 };
 
@@ -457,13 +459,13 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8897 = {
 	.reg = &mwifiex_reg_sd8897,
 	.max_ports = 32,
 	.mp_agg_pkt_limit = 16,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K,
+	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_MAX,
+	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_MAX,
 	.supports_sdio_new_mode = true,
 	.has_control_mask = false,
-	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K,
-	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_32K,
-	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_32K,
-	.supports_fw_dump = true,
-	.auto_tdls = false,
+	.can_dump_fw = true,
+	.can_auto_tdls = false,
 	.can_ext_scan = true,
 };
 
@@ -472,13 +474,13 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8887 = {
 	.reg = &mwifiex_reg_sd8887,
 	.max_ports = 32,
 	.mp_agg_pkt_limit = 16,
-	.supports_sdio_new_mode = true,
-	.has_control_mask = false,
-	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_32K,
 	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_32K,
-	.supports_fw_dump = false,
-	.auto_tdls = true,
+	.supports_sdio_new_mode = true,
+	.has_control_mask = false,
+	.can_dump_fw = false,
+	.can_auto_tdls = true,
 	.can_ext_scan = true,
 };
 
@@ -492,8 +494,8 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8801 = {
 	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
 	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_16K,
-	.supports_fw_dump = false,
-	.auto_tdls = false,
+	.can_dump_fw = false,
+	.can_auto_tdls = false,
 	.can_ext_scan = true,
 };
 
@@ -571,9 +573,9 @@ mp_tx_aggr_port_limit_reached(struct sdio_mmc_card *card)
 
 /* Prepare to copy current packet from card to SDIO Rx aggregation buffer */
 static inline void mp_rx_aggr_setup(struct sdio_mmc_card *card,
-				    struct sk_buff *skb, u8 port)
+				    u16 rx_len, u8 port)
 {
-	card->mpa_rx.buf_len += skb->len;
+	card->mpa_rx.buf_len += rx_len;
 
 	if (!card->mpa_rx.pkt_cnt)
 		card->mpa_rx.start_port = port;
@@ -586,8 +588,8 @@ static inline void mp_rx_aggr_setup(struct sdio_mmc_card *card,
 		else
 			card->mpa_rx.ports |= 1 << (card->mpa_rx.pkt_cnt + 1);
 	}
-	card->mpa_rx.skb_arr[card->mpa_rx.pkt_cnt] = skb;
-	card->mpa_rx.len_arr[card->mpa_rx.pkt_cnt] = skb->len;
+	card->mpa_rx.skb_arr[card->mpa_rx.pkt_cnt] = NULL;
+	card->mpa_rx.len_arr[card->mpa_rx.pkt_cnt] = rx_len;
 	card->mpa_rx.pkt_cnt++;
 }
 #endif /* _MWIFIEX_SDIO_H */
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index f7d204ffd6e9..49422f2a5380 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -1370,22 +1370,29 @@ mwifiex_cmd_mef_cfg(struct mwifiex_private *priv,
 		    struct mwifiex_ds_mef_cfg *mef)
 {
 	struct host_cmd_ds_mef_cfg *mef_cfg = &cmd->params.mef_cfg;
+	struct mwifiex_fw_mef_entry *mef_entry = NULL;
 	u8 *pos = (u8 *)mef_cfg;
+	u16 i;
 
 	cmd->command = cpu_to_le16(HostCmd_CMD_MEF_CFG);
 
 	mef_cfg->criteria = cpu_to_le32(mef->criteria);
 	mef_cfg->num_entries = cpu_to_le16(mef->num_entries);
 	pos += sizeof(*mef_cfg);
-	mef_cfg->mef_entry->mode = mef->mef_entry->mode;
-	mef_cfg->mef_entry->action = mef->mef_entry->action;
-	pos += sizeof(*(mef_cfg->mef_entry));
 
-	if (mwifiex_cmd_append_rpn_expression(priv, mef->mef_entry, &pos))
-		return -1;
+	for (i = 0; i < mef->num_entries; i++) {
+		mef_entry = (struct mwifiex_fw_mef_entry *)pos;
+		mef_entry->mode = mef->mef_entry[i].mode;
+		mef_entry->action = mef->mef_entry[i].action;
+		pos += sizeof(*mef_cfg->mef_entry);
+
+		if (mwifiex_cmd_append_rpn_expression(priv,
+						      &mef->mef_entry[i], &pos))
+			return -1;
 
-	mef_cfg->mef_entry->exprsize =
-			cpu_to_le16(pos - mef_cfg->mef_entry->expr);
+		mef_entry->exprsize =
+			cpu_to_le16(pos - mef_entry->expr);
+	}
 	cmd->size = cpu_to_le16((u16) (pos - (u8 *)mef_cfg) + S_DS_GEN);
 
 	return 0;
@@ -1664,6 +1671,25 @@ mwifiex_cmd_tdls_oper(struct mwifiex_private *priv,
 
 	return 0;
 }
+
+/* This function prepares command of sdio rx aggr info. */
+static int mwifiex_cmd_sdio_rx_aggr_cfg(struct host_cmd_ds_command *cmd,
+					u16 cmd_action, void *data_buf)
+{
+	struct host_cmd_sdio_sp_rx_aggr_cfg *cfg =
+					&cmd->params.sdio_rx_aggr_cfg;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_SDIO_SP_RX_AGGR_CFG);
+	cmd->size =
+		cpu_to_le16(sizeof(struct host_cmd_sdio_sp_rx_aggr_cfg) +
+			    S_DS_GEN);
+	cfg->action = cmd_action;
+	if (cmd_action == HostCmd_ACT_GEN_SET)
+		cfg->enable = *(u8 *)data_buf;
+
+	return 0;
+}
+
 /*
  * This function prepares the commands before sending them to the firmware.
  *
@@ -1901,6 +1927,10 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 		ret = mwifiex_cmd_issue_chan_report_request(priv, cmd_ptr,
 							    data_buf);
 		break;
+	case HostCmd_CMD_SDIO_SP_RX_AGGR_CFG:
+		ret = mwifiex_cmd_sdio_rx_aggr_cfg(cmd_ptr, cmd_action,
+						   data_buf);
+		break;
 	default:
 		dev_err(priv->adapter->dev,
 			"PREP_CMD: unknown cmd- %#x\n", cmd_no);
@@ -1940,6 +1970,7 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta, bool init)
 	struct mwifiex_ds_auto_ds auto_ds;
 	enum state_11d_t state_11d;
 	struct mwifiex_ds_11n_tx_cfg tx_cfg;
+	u8 sdio_sp_rx_aggr_enable;
 
 	if (first_sta) {
 		if (priv->adapter->iface_type == MWIFIEX_PCIE) {
@@ -1983,6 +2014,22 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta, bool init)
 		if (ret)
 			return -1;
 
+		/** Set SDIO Single Port RX Aggr Info */
+		if (priv->adapter->iface_type == MWIFIEX_SDIO &&
+		    ISSUPP_SDIO_SPA_ENABLED(priv->adapter->fw_cap_info)) {
+			sdio_sp_rx_aggr_enable = true;
+			ret = mwifiex_send_cmd(priv,
+					       HostCmd_CMD_SDIO_SP_RX_AGGR_CFG,
+					       HostCmd_ACT_GEN_SET, 0,
+					       &sdio_sp_rx_aggr_enable,
+					       true);
+			if (ret) {
+				dev_err(priv->adapter->dev,
+					"error while enabling SP aggregation..disable it");
+				adapter->sdio_rx_aggr_enable = false;
+			}
+		}
+
 		/* Reconfigure tx buf size */
 		ret = mwifiex_send_cmd(priv, HostCmd_CMD_RECONFIGURE_TX_BUFF,
 				       HostCmd_ACT_GEN_SET, 0,
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 5f8da5924666..88dc6b672ef4 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -90,6 +90,10 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
 	case HostCmd_CMD_MAC_CONTROL:
 		break;
 
+	case HostCmd_CMD_SDIO_SP_RX_AGGR_CFG:
+		dev_err(priv->adapter->dev, "SDIO RX single-port aggregation Not support\n");
+		break;
+
 	default:
 		break;
 	}
@@ -943,6 +947,20 @@ static int mwifiex_ret_cfg_data(struct mwifiex_private *priv,
 	return 0;
 }
 
+/** This Function handles the command response of sdio rx aggr */
+static int mwifiex_ret_sdio_rx_aggr_cfg(struct mwifiex_private *priv,
+					struct host_cmd_ds_command *resp)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct host_cmd_sdio_sp_rx_aggr_cfg *cfg =
+				&resp->params.sdio_rx_aggr_cfg;
+
+	adapter->sdio_rx_aggr_enable = cfg->enable;
+	adapter->sdio_rx_block_size = le16_to_cpu(cfg->block_size);
+
+	return 0;
+}
+
 /*
  * This function handles the command responses.
  *
@@ -1124,6 +1142,9 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		break;
 	case HostCmd_CMD_CHAN_REPORT_REQUEST:
 		break;
+	case HostCmd_CMD_SDIO_SP_RX_AGGR_CFG:
+		ret = mwifiex_ret_sdio_rx_aggr_cfg(priv, resp);
+		break;
 	default:
 		dev_err(adapter->dev, "CMD_RESP: unknown cmd response %#x\n",
 			resp->command);
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index 80ffe7412496..0dc7a1d3993d 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -135,7 +135,7 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 		cfg80211_disconnected(priv->netdev, reason_code, NULL, 0,
 				      GFP_KERNEL);
 	}
-	memset(priv->cfg_bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->cfg_bssid);
 
 	mwifiex_stop_net_dev_queue(priv->netdev, adapter);
 	if (netif_carrier_ok(priv->netdev))
@@ -312,7 +312,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 					adapter->ps_state = PS_STATE_AWAKE;
 					adapter->pm_wakeup_card_req = false;
 					adapter->pm_wakeup_fw_try = false;
-					del_timer_sync(&adapter->wakeup_timer);
+					del_timer(&adapter->wakeup_timer);
 					break;
 				}
 				if (!mwifiex_send_null_packet
@@ -327,7 +327,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		adapter->ps_state = PS_STATE_AWAKE;
 		adapter->pm_wakeup_card_req = false;
 		adapter->pm_wakeup_fw_try = false;
-		del_timer_sync(&adapter->wakeup_timer);
+		del_timer(&adapter->wakeup_timer);
 
 		break;
 
diff --git a/drivers/net/wireless/mwifiex/txrx.c b/drivers/net/wireless/mwifiex/txrx.c
index ac93557cbdc9..a245f444aeec 100644
--- a/drivers/net/wireless/mwifiex/txrx.c
+++ b/drivers/net/wireless/mwifiex/txrx.c
@@ -80,23 +80,29 @@ EXPORT_SYMBOL_GPL(mwifiex_handle_rx_packet);
 int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 		       struct mwifiex_tx_param *tx_param)
 {
-	int ret = -1;
+	int hroom, ret = -1;
 	struct mwifiex_adapter *adapter = priv->adapter;
 	u8 *head_ptr;
 	struct txpd *local_tx_pd = NULL;
 
+	hroom = (adapter->iface_type == MWIFIEX_USB) ? 0 : INTF_HEADER_LEN;
+
 	if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP)
 		head_ptr = mwifiex_process_uap_txpd(priv, skb);
 	else
 		head_ptr = mwifiex_process_sta_txpd(priv, skb);
 
+	if ((adapter->data_sent || adapter->tx_lock_flag) && head_ptr) {
+		skb_queue_tail(&adapter->tx_data_q, skb);
+		atomic_inc(&adapter->tx_queued);
+		return 0;
+	}
+
 	if (head_ptr) {
 		if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA)
-			local_tx_pd =
-				(struct txpd *) (head_ptr + INTF_HEADER_LEN);
+			local_tx_pd = (struct txpd *)(head_ptr + hroom);
 		if (adapter->iface_type == MWIFIEX_USB) {
 			adapter->data_sent = true;
-			skb_pull(skb, INTF_HEADER_LEN);
 			ret = adapter->if_ops.host_to_card(adapter,
 							   MWIFIEX_USB_EP_DATA,
 							   skb, NULL);
@@ -142,6 +148,123 @@ int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 	return ret;
 }
 
+static int mwifiex_host_to_card(struct mwifiex_adapter *adapter,
+				struct sk_buff *skb,
+				struct mwifiex_tx_param *tx_param)
+{
+	struct txpd *local_tx_pd = NULL;
+	u8 *head_ptr = skb->data;
+	int ret = 0;
+	struct mwifiex_private *priv;
+	struct mwifiex_txinfo *tx_info;
+
+	tx_info = MWIFIEX_SKB_TXCB(skb);
+	priv = mwifiex_get_priv_by_id(adapter, tx_info->bss_num,
+				      tx_info->bss_type);
+	if (!priv) {
+		dev_err(adapter->dev, "data: priv not found. Drop TX packet\n");
+		adapter->dbg.num_tx_host_to_card_failure++;
+		mwifiex_write_data_complete(adapter, skb, 0, 0);
+		return ret;
+	}
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) {
+		if (adapter->iface_type == MWIFIEX_USB)
+			local_tx_pd = (struct txpd *)head_ptr;
+		else
+			local_tx_pd = (struct txpd *) (head_ptr +
+				INTF_HEADER_LEN);
+	}
+
+	if (adapter->iface_type == MWIFIEX_USB) {
+		adapter->data_sent = true;
+		ret = adapter->if_ops.host_to_card(adapter,
+						   MWIFIEX_USB_EP_DATA,
+						   skb, NULL);
+	} else {
+		ret = adapter->if_ops.host_to_card(adapter,
+						   MWIFIEX_TYPE_DATA,
+						   skb, tx_param);
+	}
+	switch (ret) {
+	case -ENOSR:
+		dev_err(adapter->dev, "data: -ENOSR is returned\n");
+		break;
+	case -EBUSY:
+		if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
+		    (adapter->pps_uapsd_mode) &&
+		    (adapter->tx_lock_flag)) {
+			priv->adapter->tx_lock_flag = false;
+			if (local_tx_pd)
+				local_tx_pd->flags = 0;
+		}
+		skb_queue_head(&adapter->tx_data_q, skb);
+		if (tx_info->flags & MWIFIEX_BUF_FLAG_AGGR_PKT)
+			atomic_add(tx_info->aggr_num, &adapter->tx_queued);
+		else
+			atomic_inc(&adapter->tx_queued);
+		dev_dbg(adapter->dev, "data: -EBUSY is returned\n");
+		break;
+	case -1:
+		if (adapter->iface_type != MWIFIEX_PCIE)
+			adapter->data_sent = false;
+		dev_err(adapter->dev, "mwifiex_write_data_async failed: 0x%X\n",
+			ret);
+		adapter->dbg.num_tx_host_to_card_failure++;
+		mwifiex_write_data_complete(adapter, skb, 0, ret);
+		break;
+	case -EINPROGRESS:
+		if (adapter->iface_type != MWIFIEX_PCIE)
+			adapter->data_sent = false;
+		break;
+	case 0:
+		mwifiex_write_data_complete(adapter, skb, 0, ret);
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+static int
+mwifiex_dequeue_tx_queue(struct mwifiex_adapter *adapter)
+{
+	struct sk_buff *skb, *skb_next;
+	struct mwifiex_txinfo *tx_info;
+	struct mwifiex_tx_param tx_param;
+
+	skb = skb_dequeue(&adapter->tx_data_q);
+	if (!skb)
+		return -1;
+
+	tx_info = MWIFIEX_SKB_TXCB(skb);
+	if (tx_info->flags & MWIFIEX_BUF_FLAG_AGGR_PKT)
+		atomic_sub(tx_info->aggr_num, &adapter->tx_queued);
+	else
+		atomic_dec(&adapter->tx_queued);
+
+	if (!skb_queue_empty(&adapter->tx_data_q))
+		skb_next = skb_peek(&adapter->tx_data_q);
+	else
+		skb_next = NULL;
+	tx_param.next_pkt_len = ((skb_next) ? skb_next->len : 0);
+	if (!tx_param.next_pkt_len) {
+		if (!mwifiex_wmm_lists_empty(adapter))
+			tx_param.next_pkt_len = 1;
+	}
+	return mwifiex_host_to_card(adapter, skb, &tx_param);
+}
+
+void
+mwifiex_process_tx_queue(struct mwifiex_adapter *adapter)
+{
+	do {
+		if (adapter->data_sent || adapter->tx_lock_flag)
+			break;
+		if (mwifiex_dequeue_tx_queue(adapter))
+			break;
+	} while (!skb_queue_empty(&adapter->tx_data_q));
+}
+
 /*
  * Packet send completion callback handler.
  *
@@ -179,8 +302,11 @@ int mwifiex_write_data_complete(struct mwifiex_adapter *adapter,
 		priv->stats.tx_errors++;
 	}
 
-	if (tx_info->flags & MWIFIEX_BUF_FLAG_BRIDGED_PKT)
+	if (tx_info->flags & MWIFIEX_BUF_FLAG_BRIDGED_PKT) {
 		atomic_dec_return(&adapter->pending_bridged_pkts);
+		if (tx_info->flags & MWIFIEX_BUF_FLAG_AGGR_PKT)
+			goto done;
+	}
 
 	if (aggr)
 		/* For skb_aggr, do not wake up tx queue */
diff --git a/drivers/net/wireless/mwifiex/usb.c b/drivers/net/wireless/mwifiex/usb.c
index 223873022ffe..fd8027f200a0 100644
--- a/drivers/net/wireless/mwifiex/usb.c
+++ b/drivers/net/wireless/mwifiex/usb.c
@@ -193,7 +193,7 @@ static void mwifiex_usb_rx_complete(struct urb *urb)
 		dev_dbg(adapter->dev, "info: recv_length=%d, status=%d\n",
 			recv_length, status);
 		if (status == -EINPROGRESS) {
-			queue_work(adapter->workqueue, &adapter->main_work);
+			mwifiex_queue_main_work(adapter);
 
 			/* urb for data_ep is re-submitted now;
 			 * urb for cmd_ep will be re-submitted in callback
@@ -262,7 +262,7 @@ static void mwifiex_usb_tx_complete(struct urb *urb)
 					    urb->status ? -1 : 0);
 	}
 
-	queue_work(adapter->workqueue, &adapter->main_work);
+	mwifiex_queue_main_work(adapter);
 
 	return;
 }
@@ -1006,7 +1006,7 @@ static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
 {
 	/* Simulation of HS_AWAKE event */
 	adapter->pm_wakeup_fw_try = false;
-	del_timer_sync(&adapter->wakeup_timer);
+	del_timer(&adapter->wakeup_timer);
 	adapter->pm_wakeup_card_req = false;
 	adapter->ps_state = PS_STATE_AWAKE;
 
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/mwifiex/util.c
index 308550611f22..b8a45872354d 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -367,6 +367,13 @@ mwifiex_process_mgmt_packet(struct mwifiex_private *priv,
 	if (!skb)
 		return -1;
 
+	if (!priv->mgmt_frame_mask ||
+	    priv->wdev.iftype == NL80211_IFTYPE_UNSPECIFIED) {
+		dev_dbg(priv->adapter->dev,
+			"do not receive mgmt frames on uninitialized intf");
+		return -1;
+	}
+
 	rx_pd = (struct rxpd *)skb->data;
 
 	skb_pull(skb, le16_to_cpu(rx_pd->rx_pkt_offset));
@@ -624,3 +631,26 @@ void mwifiex_hist_data_reset(struct mwifiex_private *priv)
 	for (ix = 0; ix < MWIFIEX_MAX_SIG_STRENGTH; ix++)
 		atomic_set(&phist_data->sig_str[ix], 0);
 }
+
+void *mwifiex_alloc_dma_align_buf(int rx_len, gfp_t flags)
+{
+	struct sk_buff *skb;
+	int buf_len, pad;
+
+	buf_len = rx_len + MWIFIEX_RX_HEADROOM + MWIFIEX_DMA_ALIGN_SZ;
+
+	skb = __dev_alloc_skb(buf_len, flags);
+
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, MWIFIEX_RX_HEADROOM);
+
+	pad = MWIFIEX_ALIGN_ADDR(skb->data, MWIFIEX_DMA_ALIGN_SZ) -
+	      (long)skb->data;
+
+	skb_reserve(skb, pad);
+
+	return skb;
+}
+EXPORT_SYMBOL_GPL(mwifiex_alloc_dma_align_buf);
diff --git a/drivers/net/wireless/mwifiex/wmm.c b/drivers/net/wireless/mwifiex/wmm.c
index ef717acec8b7..b2e99569a0f8 100644
--- a/drivers/net/wireless/mwifiex/wmm.c
+++ b/drivers/net/wireless/mwifiex/wmm.c
@@ -157,6 +157,8 @@ void mwifiex_ralist_add(struct mwifiex_private *priv, const u8 *ra)
 
 		ra_list->is_11n_enabled = 0;
 		ra_list->tdls_link = false;
+		ra_list->ba_status = BA_SETUP_NONE;
+		ra_list->amsdu_in_ampdu = false;
 		if (!mwifiex_queuing_ra_based(priv)) {
 			if (mwifiex_get_tdls_link_status(priv, ra) ==
 			    TDLS_SETUP_COMPLETE) {
@@ -574,7 +576,7 @@ mwifiex_clean_txrx(struct mwifiex_private *priv)
  * This function retrieves a particular RA list node, matching with the
  * given TID and RA address.
  */
-static struct mwifiex_ra_list_tbl *
+struct mwifiex_ra_list_tbl *
 mwifiex_wmm_get_ralist_node(struct mwifiex_private *priv, u8 tid,
 			    const u8 *ra_addr)
 {
@@ -730,7 +732,7 @@ mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 	} else {
 		memcpy(ra, skb->data, ETH_ALEN);
 		if (ra[0] & 0x01 || mwifiex_is_skb_mgmt_frame(skb))
-			memset(ra, 0xff, ETH_ALEN);
+			eth_broadcast_addr(ra);
 		ra_list = mwifiex_wmm_get_queue_raptr(priv, tid_down, ra);
 	}
 
@@ -942,14 +944,11 @@ mwifiex_wmm_get_highest_priolist_ptr(struct mwifiex_adapter *adapter,
 	struct mwifiex_ra_list_tbl *ptr;
 	struct mwifiex_tid_tbl *tid_ptr;
 	atomic_t *hqp;
-	unsigned long flags_bss, flags_ra;
+	unsigned long flags_ra;
 	int i, j;
 
 	/* check the BSS with highest priority first */
 	for (j = adapter->priv_num - 1; j >= 0; --j) {
-		spin_lock_irqsave(&adapter->bss_prio_tbl[j].bss_prio_lock,
-				  flags_bss);
-
 		/* iterate over BSS with the equal priority */
 		list_for_each_entry(adapter->bss_prio_tbl[j].bss_prio_cur,
 				    &adapter->bss_prio_tbl[j].bss_prio_head,
@@ -985,19 +984,15 @@ mwifiex_wmm_get_highest_priolist_ptr(struct mwifiex_adapter *adapter,
 			}
 		}
 
-		spin_unlock_irqrestore(&adapter->bss_prio_tbl[j].bss_prio_lock,
-				       flags_bss);
 	}
 
 	return NULL;
 
 found:
-	/* holds bss_prio_lock / ra_list_spinlock */
+	/* holds ra_list_spinlock */
 	if (atomic_read(hqp) > i)
 		atomic_set(hqp, i);
 	spin_unlock_irqrestore(&priv_tmp->wmm.ra_list_spinlock, flags_ra);
-	spin_unlock_irqrestore(&adapter->bss_prio_tbl[j].bss_prio_lock,
-			       flags_bss);
 
 	*priv = priv_tmp;
 	*tid = tos_to_tid[i];
@@ -1179,6 +1174,14 @@ mwifiex_send_processed_packet(struct mwifiex_private *priv,
 
 	skb = skb_dequeue(&ptr->skb_head);
 
+	if (adapter->data_sent || adapter->tx_lock_flag) {
+		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
+				       ra_list_flags);
+		skb_queue_tail(&adapter->tx_data_q, skb);
+		atomic_inc(&adapter->tx_queued);
+		return;
+	}
+
 	if (!skb_queue_empty(&ptr->skb_head))
 		skb_next = skb_peek(&ptr->skb_head);
 	else
@@ -1276,13 +1279,13 @@ mwifiex_dequeue_tx_packet(struct mwifiex_adapter *adapter)
 	}
 
 	if (!ptr->is_11n_enabled ||
-	    mwifiex_is_ba_stream_setup(priv, ptr, tid) ||
-	    priv->wps.session_enable) {
+		ptr->ba_status ||
+		priv->wps.session_enable) {
 		if (ptr->is_11n_enabled &&
-		    mwifiex_is_ba_stream_setup(priv, ptr, tid) &&
-		    mwifiex_is_amsdu_in_ampdu_allowed(priv, ptr, tid) &&
-		    mwifiex_is_amsdu_allowed(priv, tid) &&
-		    mwifiex_is_11n_aggragation_possible(priv, ptr,
+			ptr->ba_status &&
+			ptr->amsdu_in_ampdu &&
+			mwifiex_is_amsdu_allowed(priv, tid) &&
+			mwifiex_is_11n_aggragation_possible(priv, ptr,
 							adapter->tx_buf_size))
 			mwifiex_11n_aggregate_pkt(priv, ptr, ptr_index, flags);
 			/* ra_list_spinlock has been freed in
@@ -1329,11 +1332,16 @@ void
 mwifiex_wmm_process_tx(struct mwifiex_adapter *adapter)
 {
 	do {
-		/* Check if busy */
-		if (adapter->data_sent || adapter->tx_lock_flag)
-			break;
-
 		if (mwifiex_dequeue_tx_packet(adapter))
 			break;
+		if (adapter->iface_type != MWIFIEX_SDIO) {
+			if (adapter->data_sent ||
+			    adapter->tx_lock_flag)
+				break;
+		} else {
+			if (atomic_read(&adapter->tx_queued) >=
+			    MWIFIEX_MAX_PKTS_TXQ)
+				break;
+		}
 	} while (!mwifiex_wmm_lists_empty(adapter));
 }
diff --git a/drivers/net/wireless/mwifiex/wmm.h b/drivers/net/wireless/mwifiex/wmm.h
index 569bd73f33c5..48ece0b35591 100644
--- a/drivers/net/wireless/mwifiex/wmm.h
+++ b/drivers/net/wireless/mwifiex/wmm.h
@@ -127,4 +127,6 @@ mwifiex_wmm_get_queue_raptr(struct mwifiex_private *priv, u8 tid,
 			    const u8 *ra_addr);
 u8 mwifiex_wmm_downgrade_tid(struct mwifiex_private *priv, u32 tid);
 
+struct mwifiex_ra_list_tbl *mwifiex_wmm_get_ralist_node(struct mwifiex_private
+					*priv, u8 tid, const u8 *ra_addr);
 #endif /* !_MWIFIEX_WMM_H_ */
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index f9b1218c761a..95921167b53f 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -1277,7 +1277,7 @@ static inline void mwl8k_save_beacon(struct ieee80211_hw *hw,
 	struct mwl8k_priv *priv = hw->priv;
 
 	priv->capture_beacon = false;
-	memset(priv->capture_bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->capture_bssid);
 
 	/*
 	 * Use GFP_ATOMIC as rxq_process is called from
diff --git a/drivers/net/wireless/orinoco/Kconfig b/drivers/net/wireless/orinoco/Kconfig
index 6d831d4d1b5f..f6fa3f4e294f 100644
--- a/drivers/net/wireless/orinoco/Kconfig
+++ b/drivers/net/wireless/orinoco/Kconfig
@@ -2,7 +2,7 @@ config HERMES
 	tristate "Hermes chipset 802.11b support (Orinoco/Prism2/Symbol)"
 	depends on (PPC_PMAC || PCI || PCMCIA)
 	depends on CFG80211
-	select CFG80211_WEXT
+	select CFG80211_WEXT_EXPORT
 	select WIRELESS_EXT
 	select WEXT_SPY
 	select WEXT_PRIV
diff --git a/drivers/net/wireless/orinoco/airport.c b/drivers/net/wireless/orinoco/airport.c
index 0ca8b1455cd9..77e6c53040a3 100644
--- a/drivers/net/wireless/orinoco/airport.c
+++ b/drivers/net/wireless/orinoco/airport.c
@@ -228,7 +228,7 @@ MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
 MODULE_DESCRIPTION("Driver for the Apple Airport wireless card.");
 MODULE_LICENSE("Dual MPL/GPL");
 
-static struct of_device_id airport_match[] = {
+static const struct of_device_id airport_match[] = {
 	{
 	.name		= "radio",
 	},
diff --git a/drivers/net/wireless/orinoco/wext.c b/drivers/net/wireless/orinoco/wext.c
index 6abdaf0aa052..1d4dae422106 100644
--- a/drivers/net/wireless/orinoco/wext.c
+++ b/drivers/net/wireless/orinoco/wext.c
@@ -168,7 +168,7 @@ static int orinoco_ioctl_setwap(struct net_device *dev,
 	if (is_zero_ether_addr(ap_addr->sa_data) ||
 	    is_broadcast_ether_addr(ap_addr->sa_data)) {
 		priv->bssid_fixed = 0;
-		memset(priv->desired_bssid, 0, ETH_ALEN);
+		eth_zero_addr(priv->desired_bssid);
 
 		/* "off" means keep existing connection */
 		if (ap_addr->sa_data[0] == 0) {
diff --git a/drivers/net/wireless/p54/fwio.c b/drivers/net/wireless/p54/fwio.c
index 5367d510b22d..275408eaf95e 100644
--- a/drivers/net/wireless/p54/fwio.c
+++ b/drivers/net/wireless/p54/fwio.c
@@ -671,7 +671,7 @@ int p54_upload_key(struct p54_common *priv, u8 algo, int slot, u8 idx, u8 len,
 	if (addr)
 		memcpy(rxkey->mac, addr, ETH_ALEN);
 	else
-		memset(rxkey->mac, ~0, ETH_ALEN);
+		eth_broadcast_addr(rxkey->mac);
 
 	switch (algo) {
 	case P54_CRYPTO_WEP:
diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/p54/main.c
index b9250d75d253..e79674f73dc5 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/p54/main.c
@@ -182,7 +182,7 @@ static int p54_start(struct ieee80211_hw *dev)
 	if (err)
 		goto out;
 
-	memset(priv->bssid, ~0, ETH_ALEN);
+	eth_broadcast_addr(priv->bssid);
 	priv->mode = NL80211_IFTYPE_MONITOR;
 	err = p54_setup_mac(priv);
 	if (err) {
@@ -274,8 +274,8 @@ static void p54_remove_interface(struct ieee80211_hw *dev,
 		wait_for_completion_interruptible_timeout(&priv->beacon_comp, HZ);
 	}
 	priv->mode = NL80211_IFTYPE_MONITOR;
-	memset(priv->mac_addr, 0, ETH_ALEN);
-	memset(priv->bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->mac_addr);
+	eth_zero_addr(priv->bssid);
 	p54_setup_mac(priv);
 	mutex_unlock(&priv->conf_mutex);
 }
@@ -794,7 +794,7 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
 	init_completion(&priv->beacon_comp);
 	INIT_DELAYED_WORK(&priv->work, p54_work);
 
-	memset(&priv->mc_maclist[0], ~0, ETH_ALEN);
+	eth_broadcast_addr(priv->mc_maclist[0]);
 	priv->curchan = NULL;
 	p54_reset_stats(priv);
 	return dev;
diff --git a/drivers/net/wireless/ray_cs.c b/drivers/net/wireless/ray_cs.c
index 8330fa33e50b..477f86354dc5 100644
--- a/drivers/net/wireless/ray_cs.c
+++ b/drivers/net/wireless/ray_cs.c
@@ -808,7 +808,7 @@ static int ray_dev_init(struct net_device *dev)
 
 	/* copy mac and broadcast addresses to linux device */
 	memcpy(dev->dev_addr, &local->sparm.b4.a_mac_addr, ADDRLEN);
-	memset(dev->broadcast, 0xff, ETH_ALEN);
+	eth_broadcast_addr(dev->broadcast);
 
 	dev_dbg(&link->dev, "ray_dev_init ending\n");
 	return 0;
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 60d44ce9c017..d72ff8e7125d 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -199,13 +199,13 @@ enum ndis_80211_pmkid_cand_list_flag_bits {
 
 struct ndis_80211_auth_request {
 	__le32 length;
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	u8 padding[2];
 	__le32 flags;
 } __packed;
 
 struct ndis_80211_pmkid_candidate {
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	u8 padding[2];
 	__le32 flags;
 } __packed;
@@ -248,7 +248,7 @@ struct ndis_80211_conf {
 
 struct ndis_80211_bssid_ex {
 	__le32 length;
-	u8 mac[6];
+	u8 mac[ETH_ALEN];
 	u8 padding[2];
 	struct ndis_80211_ssid ssid;
 	__le32 privacy;
@@ -283,7 +283,7 @@ struct ndis_80211_key {
 	__le32 size;
 	__le32 index;
 	__le32 length;
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	u8 padding[6];
 	u8 rsc[8];
 	u8 material[32];
@@ -292,7 +292,7 @@ struct ndis_80211_key {
 struct ndis_80211_remove_key {
 	__le32 size;
 	__le32 index;
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	u8 padding[2];
 } __packed;
 
@@ -310,7 +310,7 @@ struct ndis_80211_assoc_info {
 	struct req_ie {
 		__le16 capa;
 		__le16 listen_interval;
-		u8 cur_ap_address[6];
+		u8 cur_ap_address[ETH_ALEN];
 	} req_ie;
 	__le32 req_ie_length;
 	__le32 offset_req_ies;
@@ -338,7 +338,7 @@ struct ndis_80211_capability {
 } __packed;
 
 struct ndis_80211_bssid_info {
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN];
 	u8 pmkid[16];
 } __packed;
 
@@ -1037,7 +1037,7 @@ static int get_bssid(struct usbnet *usbdev, u8 bssid[ETH_ALEN])
 			      bssid, &len);
 
 	if (ret != 0)
-		memset(bssid, 0, ETH_ALEN);
+		eth_zero_addr(bssid);
 
 	return ret;
 }
@@ -1391,7 +1391,7 @@ static int add_wep_key(struct usbnet *usbdev, const u8 *key, int key_len,
 	priv->encr_keys[index].len = key_len;
 	priv->encr_keys[index].cipher = cipher;
 	memcpy(&priv->encr_keys[index].material, key, key_len);
-	memset(&priv->encr_keys[index].bssid, 0xff, ETH_ALEN);
+	eth_broadcast_addr(priv->encr_keys[index].bssid);
 
 	return 0;
 }
@@ -1466,7 +1466,7 @@ static int add_wpa_key(struct usbnet *usbdev, const u8 *key, int key_len,
 	} else {
 		/* group key */
 		if (priv->infra_mode == NDIS_80211_INFRA_ADHOC)
-			memset(ndis_key.bssid, 0xff, ETH_ALEN);
+			eth_broadcast_addr(ndis_key.bssid);
 		else
 			get_bssid(usbdev, ndis_key.bssid);
 	}
@@ -1486,7 +1486,7 @@ static int add_wpa_key(struct usbnet *usbdev, const u8 *key, int key_len,
 	if (flags & NDIS_80211_ADDKEY_PAIRWISE_KEY)
 		memcpy(&priv->encr_keys[index].bssid, ndis_key.bssid, ETH_ALEN);
 	else
-		memset(&priv->encr_keys[index].bssid, 0xff, ETH_ALEN);
+		eth_broadcast_addr(priv->encr_keys[index].bssid);
 
 	if (flags & NDIS_80211_ADDKEY_TRANSMIT_KEY)
 		priv->encr_tx_key_index = index;
@@ -2280,7 +2280,7 @@ static int rndis_disconnect(struct wiphy *wiphy, struct net_device *dev,
 	netdev_dbg(usbdev->net, "cfg80211.disconnect(%d)\n", reason_code);
 
 	priv->connected = false;
-	memset(priv->bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->bssid);
 
 	return deauthenticate(usbdev);
 }
@@ -2392,7 +2392,7 @@ static int rndis_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 	netdev_dbg(usbdev->net, "cfg80211.leave_ibss()\n");
 
 	priv->connected = false;
-	memset(priv->bssid, 0, ETH_ALEN);
+	eth_zero_addr(priv->bssid);
 
 	return deauthenticate(usbdev);
 }
@@ -2857,7 +2857,7 @@ static void rndis_wlan_do_link_down_work(struct usbnet *usbdev)
 
 	if (priv->connected) {
 		priv->connected = false;
-		memset(priv->bssid, 0, ETH_ALEN);
+		eth_zero_addr(priv->bssid);
 
 		deauthenticate(usbdev);
 
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 8444313eabe2..6ec2466b52b6 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -233,6 +233,7 @@ static int rt2800usb_autorun_detect(struct rt2x00_dev *rt2x00dev)
 {
 	__le32 *reg;
 	u32 fw_mode;
+	int ret;
 
 	reg = kmalloc(sizeof(*reg), GFP_KERNEL);
 	if (reg == NULL)
@@ -242,11 +243,14 @@ static int rt2800usb_autorun_detect(struct rt2x00_dev *rt2x00dev)
 	 * magic value USB_MODE_AUTORUN (0x11) to the device, thus the
 	 * returned value would be invalid.
 	 */
-	rt2x00usb_vendor_request(rt2x00dev, USB_DEVICE_MODE,
-				 USB_VENDOR_REQUEST_IN, 0, USB_MODE_AUTORUN,
-				 reg, sizeof(*reg), REGISTER_TIMEOUT_FIRMWARE);
+	ret = rt2x00usb_vendor_request(rt2x00dev, USB_DEVICE_MODE,
+				       USB_VENDOR_REQUEST_IN, 0,
+				       USB_MODE_AUTORUN, reg, sizeof(*reg),
+				       REGISTER_TIMEOUT_FIRMWARE);
 	fw_mode = le32_to_cpu(*reg);
 	kfree(reg);
+	if (ret < 0)
+		return ret;
 
 	if ((fw_mode & 0x00000003) == 2)
 		return 1;
@@ -289,6 +293,7 @@ static int rt2800usb_write_firmware(struct rt2x00_dev *rt2x00dev,
 	if (retval) {
 		rt2x00_info(rt2x00dev,
 			    "Firmware loading not required - NIC in AutoRun mode\n");
+		__clear_bit(REQUIRE_FIRMWARE, &rt2x00dev->cap_flags);
 	} else {
 		rt2x00usb_register_multiwrite(rt2x00dev, FIRMWARE_IMAGE_BASE,
 					      data + offset, length);
@@ -374,7 +379,6 @@ static int rt2800usb_enable_radio(struct rt2x00_dev *rt2x00dev)
 static void rt2800usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
 	rt2800_disable_radio(rt2x00dev);
-	rt2x00usb_disable_radio(rt2x00dev);
 }
 
 static int rt2800usb_set_state(struct rt2x00_dev *rt2x00dev,
@@ -1040,6 +1044,7 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x07d1, 0x3c17) },
 	{ USB_DEVICE(0x2001, 0x3317) },
 	{ USB_DEVICE(0x2001, 0x3c1b) },
+	{ USB_DEVICE(0x2001, 0x3c25) },
 	/* Draytek */
 	{ USB_DEVICE(0x07fa, 0x7712) },
 	/* DVICO */
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.h b/drivers/net/wireless/rt2x00/rt2x00usb.h
index 8f85fbd5f237..569363da00a2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.h
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.h
@@ -199,7 +199,7 @@ static inline void rt2x00usb_register_read(struct rt2x00_dev *rt2x00dev,
 					   const unsigned int offset,
 					   u32 *value)
 {
-	__le32 reg;
+	__le32 reg = 0;
 	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
 				      USB_VENDOR_REQUEST_IN, offset,
 				      &reg, sizeof(reg));
@@ -219,7 +219,7 @@ static inline void rt2x00usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
 						const unsigned int offset,
 						u32 *value)
 {
-	__le32 reg;
+	__le32 reg = 0;
 	rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_READ,
 				       USB_VENDOR_REQUEST_IN, offset,
 				       &reg, sizeof(reg), REGISTER_TIMEOUT);
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index 1d4677460711..01f56c7df8b5 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -1315,7 +1315,8 @@ static void setup_arp_tx(struct rtl_priv *rtlpriv, struct rtl_ps_ctl *ppsc)
 }
 
 /*should call before software enc*/
-u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
+u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
+		       bool is_enc)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
@@ -1344,7 +1345,9 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 		break;
 	}
 
-	offset = mac_hdr_len + SNAP_SIZE + encrypt_header_len;
+	offset = mac_hdr_len + SNAP_SIZE;
+	if (is_enc)
+		offset += encrypt_header_len;
 	ether_type = be16_to_cpup((__be16 *)(skb->data + offset));
 
 	if (ETH_P_IP == ether_type) {
@@ -1386,8 +1389,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/base.h b/drivers/net/wireless/rtlwifi/base.h
index c6cb49c3ee32..74233d601a90 100644
--- a/drivers/net/wireless/rtlwifi/base.h
+++ b/drivers/net/wireless/rtlwifi/base.h
@@ -45,9 +45,6 @@ enum ap_peer {
 #define RTL_TX_DESC_SIZE	32
 #define RTL_TX_HEADER_SIZE	(RTL_TX_DESC_SIZE + RTL_TX_DUMMY_SIZE)
 
-#define HT_AMSDU_SIZE_4K	3839
-#define HT_AMSDU_SIZE_8K	7935
-
 #define MAX_BIT_RATE_40MHZ_MCS15	300	/* Mbps */
 #define MAX_BIT_RATE_40MHZ_MCS7		150	/* Mbps */
 
@@ -61,9 +58,6 @@ enum ap_peer {
 #define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS9	390	/* Mbps */
 #define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS7	293	/* Mbps */
 
-#define RTL_RATE_COUNT_LEGACY		12
-#define RTL_CHANNEL_COUNT		14
-
 #define FRAME_OFFSET_FRAME_CONTROL	0
 #define FRAME_OFFSET_DURATION		2
 #define FRAME_OFFSET_ADDRESS1		4
@@ -126,10 +120,10 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
 int rtlwifi_rate_mapping(struct ieee80211_hw *hw, bool isht,
 			 bool isvht, u8 desc_rate);
 bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
-u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
+u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
+		       bool is_enc);
 
 void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
-void rtl_watch_dog_timer_callback(unsigned long data);
 int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	struct ieee80211_sta *sta, u16 tid, u16 *ssn);
 int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
diff --git a/drivers/net/wireless/rtlwifi/cam.h b/drivers/net/wireless/rtlwifi/cam.h
index 35508087c0c5..e2e647d511c1 100644
--- a/drivers/net/wireless/rtlwifi/cam.h
+++ b/drivers/net/wireless/rtlwifi/cam.h
@@ -28,13 +28,11 @@
 
 #define CAM_CONTENT_COUNT				8
 
-#define CFG_DEFAULT_KEY					BIT(5)
 #define CFG_VALID					BIT(15)
 
 #define PAIRWISE_KEYIDX					0
 #define CAM_PAIRWISE_KEY_POSITION			4
 
-#define	CAM_CONFIG_USEDK				1
 #define	CAM_CONFIG_NO_USEDK				0
 
 void rtl_cam_reset_all_entry(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/core.c b/drivers/net/wireless/rtlwifi/core.c
index a31a12775f1a..3b3a88b53b11 100644
--- a/drivers/net/wireless/rtlwifi/core.c
+++ b/drivers/net/wireless/rtlwifi/core.c
@@ -195,7 +195,7 @@ static void rtl_op_stop(struct ieee80211_hw *hw)
 	if (!(support_remote_wakeup &&
 	      rtlhal->enter_pnp_sleep)) {
 		mac->link_state = MAC80211_NOLINK;
-		memset(mac->bssid, 0, 6);
+		eth_zero_addr(mac->bssid);
 		mac->vendor = PEER_UNKNOWN;
 
 		/* reset sec info */
@@ -357,7 +357,7 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 	mac->p2p = 0;
 	mac->vif = NULL;
 	mac->link_state = MAC80211_NOLINK;
-	memset(mac->bssid, 0, ETH_ALEN);
+	eth_zero_addr(mac->bssid);
 	mac->vendor = PEER_UNKNOWN;
 	mac->opmode = NL80211_IFTYPE_UNSPECIFIED;
 	rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
@@ -1157,7 +1157,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 			if (ppsc->p2p_ps_info.p2p_ps_mode > P2P_PS_NONE)
 				rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 			mac->link_state = MAC80211_NOLINK;
-			memset(mac->bssid, 0, ETH_ALEN);
+			eth_zero_addr(mac->bssid);
 			mac->vendor = PEER_UNKNOWN;
 			mac->mode = 0;
 
diff --git a/drivers/net/wireless/rtlwifi/core.h b/drivers/net/wireless/rtlwifi/core.h
index 7b64e34f421e..82733c6b8c46 100644
--- a/drivers/net/wireless/rtlwifi/core.h
+++ b/drivers/net/wireless/rtlwifi/core.h
@@ -33,8 +33,6 @@
 	FIF_FCSFAIL | \
 	FIF_BCN_PRBRESP_PROMISC)
 
-#define RTL_SUPPORTED_CTRL_FILTER	0xFF
-
 #define DM_DIG_THRESH_HIGH		40
 #define DM_DIG_THRESH_LOW		35
 #define DM_FALSEALARM_THRESH_LOW	400
diff --git a/drivers/net/wireless/rtlwifi/efuse.h b/drivers/net/wireless/rtlwifi/efuse.h
index fdab8240a5d7..be02e7894c61 100644
--- a/drivers/net/wireless/rtlwifi/efuse.h
+++ b/drivers/net/wireless/rtlwifi/efuse.h
@@ -40,12 +40,6 @@
 #define PG_STATE_WORD_3			0x10
 #define PG_STATE_DATA			0x20
 
-#define PG_SWBYTE_H			0x01
-#define PG_SWBYTE_L			0x02
-
-#define _POWERON_DELAY_
-#define _PRE_EXECUTE_READ_CMD_
-
 #define EFUSE_REPEAT_THRESHOLD_		3
 #define EFUSE_ERROE_HANDLE		1
 
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index a62170ea0481..f46c9d7f6528 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -887,7 +887,7 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 				unicast = true;
 				rtlpriv->stats.rxbytesunicast += skb->len;
 			}
-			rtl_is_special_data(hw, skb, false);
+			rtl_is_special_data(hw, skb, false, true);
 
 			if (ieee80211_is_data(fc)) {
 				rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
@@ -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/wireless/rtlwifi/rc.c b/drivers/net/wireless/rtlwifi/rc.c
index 7863bd278b22..74c14ce28238 100644
--- a/drivers/net/wireless/rtlwifi/rc.c
+++ b/drivers/net/wireless/rtlwifi/rc.c
@@ -56,7 +56,8 @@ static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
 		wireless_mode = sta_entry->wireless_mode;
 	}
 
-	if (rtl_is_special_data(rtlpriv->mac80211.hw, skb, true) || not_data) {
+	if (rtl_is_special_data(rtlpriv->mac80211.hw, skb, true, false) ||
+	    not_data) {
 		return 0;
 	} else {
 		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
@@ -201,7 +202,7 @@ static void rtl_tx_status(void *ppriv,
 	if (!priv_sta || !ieee80211_is_data(fc))
 		return;
 
-	if (rtl_is_special_data(mac->hw, skb, true))
+	if (rtl_is_special_data(mac->hw, skb, true, true))
 		return;
 
 	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/def.h b/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
index d9ea9d0c79a5..0532b9852444 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
@@ -26,53 +26,12 @@
 #ifndef __RTL92C_DEF_H__
 #define __RTL92C_DEF_H__
 
-#define HAL_RETRY_LIMIT_INFRA				48
-#define HAL_RETRY_LIMIT_AP_ADHOC			7
-
-#define RESET_DELAY_8185				20
-
-#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
-#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
-
-#define NUM_OF_FIRMWARE_QUEUE				10
-#define NUM_OF_PAGES_IN_FW				0x100
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT			0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH			0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
-
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM			0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM			0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM			0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM			0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM			0x00
-
-#define MAX_LINES_HWCONFIG_TXT				1000
-#define MAX_BYTES_LINE_HWCONFIG_TXT			256
-
-#define SW_THREE_WIRE					0
-#define HW_THREE_WIRE					2
-
-#define BT_DEMO_BOARD					0
-#define BT_QA_BOARD					1
-#define BT_FPGA						2
-
 #define HAL_PRIME_CHNL_OFFSET_DONT_CARE			0
 #define HAL_PRIME_CHNL_OFFSET_LOWER			1
 #define HAL_PRIME_CHNL_OFFSET_UPPER			2
 
-#define MAX_H2C_QUEUE_NUM				10
-
 #define RX_MPDU_QUEUE					0
 #define RX_CMD_QUEUE					1
-#define RX_MAX_QUEUE					2
-#define AC2QUEUEID(_AC)					(_AC)
 
 #define	C2H_RX_CMD_HDR_LEN				8
 #define	GET_C2H_CMD_CMD_LEN(__prxhdr)			\
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
index f2b9713c456e..86ce5b1930e6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
@@ -30,6 +30,7 @@
 #include "../cam.h"
 #include "../ps.h"
 #include "../pci.h"
+#include "../pwrseqcmd.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -566,7 +567,7 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				acm_ctrl &= (~ACMHW_VIQEN);
 				break;
 			case AC3_VO:
-				acm_ctrl &= (~ACMHW_BEQEN);
+				acm_ctrl &= (~ACMHW_VOQEN);
 				break;
 			default:
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -885,7 +886,7 @@ static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
 
 	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
 	rtl_write_byte(rtlpriv, REG_CR+1, 0x06);
-	rtl_write_byte(rtlpriv, REG_CR+2, 0x00);
+	rtl_write_byte(rtlpriv, MSR, 0x00);
 
 	if (!rtlhal->mac_func_enable) {
 		if (_rtl88ee_llt_table_init(hw) == false) {
@@ -1277,7 +1278,7 @@ static int _rtl88ee_set_media_status(struct ieee80211_hw *hw,
 			 mode);
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
+	rtl_write_byte(rtlpriv, MSR, bt_msr | mode);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
 	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
index 3f6c59cdeaba..a2bb02c7b837 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
@@ -452,9 +452,10 @@ static void handle_branch1(struct ieee80211_hw *hw, u16 arraylen,
 				READ_NEXT_PAIR(v1, v2, i);
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2)
+				       v2 != 0xCDCD && i < arraylen - 2) {
 					_rtl8188e_config_bb_reg(hw, v1, v2);
 					READ_NEXT_PAIR(v1, v2, i);
+				}
 
 				while (v2 != 0xDEAD && i < arraylen - 2)
 					READ_NEXT_PAIR(v1, v2, i);
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
index 5c1472d88fd4..0eca030e3238 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
@@ -27,7 +27,6 @@
 #define __RTL92C_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
 
 void rtl88e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
 				     u8 bandwidth);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
index 9b660df6fd71..690a7a1675e2 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
@@ -30,59 +30,18 @@
 #ifndef __RTL92C_DEF_H__
 #define __RTL92C_DEF_H__
 
-#define HAL_RETRY_LIMIT_INFRA				48
-#define HAL_RETRY_LIMIT_AP_ADHOC			7
-
 #define	PHY_RSSI_SLID_WIN_MAX				100
 #define	PHY_LINKQUALITY_SLID_WIN_MAX			20
 #define	PHY_BEACON_RSSI_SLID_WIN_MAX			10
 
-#define RESET_DELAY_8185				20
-
-#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
-#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
-
-#define NUM_OF_FIRMWARE_QUEUE				10
-#define NUM_OF_PAGES_IN_FW				0x100
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT			0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH			0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
-
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM			0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM			0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM			0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM			0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM			0x00
-
-#define MAX_LINES_HWCONFIG_TXT				1000
-#define MAX_BYTES_LINE_HWCONFIG_TXT			256
-
-#define SW_THREE_WIRE					0
-#define HW_THREE_WIRE					2
-
-#define BT_DEMO_BOARD					0
-#define BT_QA_BOARD					1
-#define BT_FPGA						2
-
 #define RX_SMOOTH_FACTOR				20
 
 #define HAL_PRIME_CHNL_OFFSET_DONT_CARE			0
 #define HAL_PRIME_CHNL_OFFSET_LOWER			1
 #define HAL_PRIME_CHNL_OFFSET_UPPER			2
 
-#define MAX_H2C_QUEUE_NUM				10
-
 #define RX_MPDU_QUEUE					0
 #define RX_CMD_QUEUE					1
-#define RX_MAX_QUEUE					2
-#define AC2QUEUEID(_AC)					(_AC)
 
 #define	C2H_RX_CMD_HDR_LEN				8
 #define	GET_C2H_CMD_CMD_LEN(__prxhdr)		\
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index 303b299376c9..04eb5c3f8464 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -363,7 +363,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 					acm_ctrl &= (~AcmHw_ViqEn);
 					break;
 				case AC3_VO:
-					acm_ctrl &= (~AcmHw_BeqEn);
+					acm_ctrl &= (~AcmHw_VoqEn);
 					break;
 				default:
 					RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h b/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h
index d8fe68b389d2..ebd72cae10b6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h
@@ -31,7 +31,6 @@
 #define __RTL92C_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
 #define RF6052_MAX_PATH			2
 
 void rtl92ce_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index fe4b699a12f5..d310d55d800e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -1364,7 +1364,7 @@ static int _rtl92cu_set_media_status(struct ieee80211_hw *hw,
 			 "Network type %d not supported!\n", type);
 		goto error_out;
 	}
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtl_write_byte(rtlpriv, MSR, bt_msr);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
 	if ((bt_msr & MSR_MASK) == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
@@ -1471,8 +1471,7 @@ static void _InitBeaconParameters(struct ieee80211_hw *hw)
 		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x66FF);
 }
 
-static void _beacon_function_enable(struct ieee80211_hw *hw, bool Enable,
-				    bool Linked)
+static void _beacon_function_enable(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -1517,7 +1516,7 @@ void rtl92cu_set_beacon_related_registers(struct ieee80211_hw *hw)
 		rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x50);
 		rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x50);
 	}
-	_beacon_function_enable(hw, true, true);
+	_beacon_function_enable(hw);
 }
 
 void rtl92cu_set_beacon_interval(struct ieee80211_hw *hw)
@@ -1589,6 +1588,8 @@ void rtl92cu_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_DATA_FILTER:
 		*((u16 *) (val)) = rtl_read_word(rtlpriv, REG_RXFLTMAP2);
 		break;
+	case HAL_DEF_WOWLAN:
+		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case not processed\n");
@@ -1871,7 +1872,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 					acm_ctrl &= (~AcmHw_ViqEn);
 					break;
 				case AC3_VO:
-					acm_ctrl &= (~AcmHw_BeqEn);
+					acm_ctrl &= (~AcmHw_VoqEn);
 					break;
 				default:
 					RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.h b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.h
index c1e33b0228c0..67588083e6cc 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.h
@@ -32,8 +32,6 @@
 
 #define H2C_RA_MASK	6
 
-#define LLT_POLLING_LLT_THRESHOLD		20
-#define LLT_POLLING_READY_TIMEOUT_COUNT		100
 #define LLT_LAST_ENTRY_OF_TX_PKT_BUFFER		255
 
 #define RX_PAGE_SIZE_REG_VALUE			PBP_128
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
index 133e395b7401..adb810794eef 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
@@ -497,7 +497,7 @@ int rtl92c_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
 			 "Network type %d not supported!\n", type);
 		return -EOPNOTSUPP;
 	}
-	rtl_write_byte(rtlpriv, (REG_CR + 2), value);
+	rtl_write_byte(rtlpriv, MSR, value);
 	return 0;
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/rf.h b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.h
index 11b439d6b671..6f987de5b441 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.h
@@ -31,7 +31,6 @@
 #define __RTL92CU_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
 #define RF6052_MAX_PATH			2
 
 void rtl92cu_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
index 90a714c189a8..23806c243a53 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
@@ -321,6 +321,7 @@ static struct usb_device_id rtl8192c_usb_ids[] = {
 	{RTL_USB_DEVICE(0x07b8, 0x8188, rtl92cu_hal_cfg)}, /*Abocom - Abocom*/
 	{RTL_USB_DEVICE(0x07b8, 0x8189, rtl92cu_hal_cfg)}, /*Funai - Abocom*/
 	{RTL_USB_DEVICE(0x0846, 0x9041, rtl92cu_hal_cfg)}, /*NetGear WNA1000M*/
+	{RTL_USB_DEVICE(0x0b05, 0x17ba, rtl92cu_hal_cfg)}, /*ASUS-Edimax*/
 	{RTL_USB_DEVICE(0x0bda, 0x5088, rtl92cu_hal_cfg)}, /*Thinkware-CC&C*/
 	{RTL_USB_DEVICE(0x0df6, 0x0052, rtl92cu_hal_cfg)}, /*Sitecom - Edimax*/
 	{RTL_USB_DEVICE(0x0df6, 0x005c, rtl92cu_hal_cfg)}, /*Sitecom - Edimax*/
@@ -377,6 +378,7 @@ static struct usb_device_id rtl8192c_usb_ids[] = {
 	{RTL_USB_DEVICE(0x2001, 0x3307, rtl92cu_hal_cfg)}, /*D-Link-Cameo*/
 	{RTL_USB_DEVICE(0x2001, 0x3309, rtl92cu_hal_cfg)}, /*D-Link-Alpha*/
 	{RTL_USB_DEVICE(0x2001, 0x330a, rtl92cu_hal_cfg)}, /*D-Link-Alpha*/
+	{RTL_USB_DEVICE(0x2001, 0x330d, rtl92cu_hal_cfg)}, /*D-Link DWA-131 */
 	{RTL_USB_DEVICE(0x2019, 0xab2b, rtl92cu_hal_cfg)}, /*Planex -Abocom*/
 	{RTL_USB_DEVICE(0x20f4, 0x624d, rtl92cu_hal_cfg)}, /*TRENDNet*/
 	{RTL_USB_DEVICE(0x2357, 0x0100, rtl92cu_hal_cfg)}, /*TP-Link WN8200ND*/
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/def.h b/drivers/net/wireless/rtlwifi/rtl8192de/def.h
index 939c905f547f..0a443ed17cf4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/def.h
@@ -35,61 +35,22 @@
 #define	MAX_MSS_DENSITY_1T				0x0A
 
 #define RF6052_MAX_TX_PWR				0x3F
-#define RF6052_MAX_REG					0x3F
 #define RF6052_MAX_PATH					2
 
-#define HAL_RETRY_LIMIT_INFRA				48
-#define HAL_RETRY_LIMIT_AP_ADHOC			7
-
 #define	PHY_RSSI_SLID_WIN_MAX				100
 #define	PHY_LINKQUALITY_SLID_WIN_MAX			20
 #define	PHY_BEACON_RSSI_SLID_WIN_MAX			10
 
-#define RESET_DELAY_8185				20
-
-#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
 #define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
 
-#define NUM_OF_FIRMWARE_QUEUE				10
-#define NUM_OF_PAGES_IN_FW				0x100
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT			0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH			0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
-
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM			0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM			0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM			0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM			0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM			0x00
-
-#define MAX_LINES_HWCONFIG_TXT				1000
-#define MAX_BYTES_LINE_HWCONFIG_TXT			256
-
-#define SW_THREE_WIRE					0
-#define HW_THREE_WIRE					2
-
-#define BT_DEMO_BOARD					0
-#define BT_QA_BOARD					1
-#define BT_FPGA						2
-
 #define RX_SMOOTH_FACTOR				20
 
 #define HAL_PRIME_CHNL_OFFSET_DONT_CARE			0
 #define HAL_PRIME_CHNL_OFFSET_LOWER			1
 #define HAL_PRIME_CHNL_OFFSET_UPPER			2
 
-#define MAX_H2C_QUEUE_NUM				10
-
 #define RX_MPDU_QUEUE					0
 #define RX_CMD_QUEUE					1
-#define RX_MAX_QUEUE					2
 
 #define	C2H_RX_CMD_HDR_LEN				8
 #define	GET_C2H_CMD_CMD_LEN(__prxhdr)			\
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
index 01bcc2d218dc..f49b60d31450 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
@@ -1126,7 +1126,7 @@ static int _rtl92de_set_media_status(struct ieee80211_hw *hw,
 		break;
 
 	}
-	rtl_write_byte(rtlpriv, REG_CR + 2, bt_msr);
+	rtl_write_byte(rtlpriv, MSR, bt_msr);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
 	if ((bt_msr & MSR_MASK) == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
index b461b3128da5..da0a6125f314 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
@@ -562,7 +562,7 @@ void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				acm_ctrl &= (~ACMHW_VIQEN);
 				break;
 			case AC3_VO:
-				acm_ctrl &= (~ACMHW_BEQEN);
+				acm_ctrl &= (~ACMHW_VOQEN);
 				break;
 			default:
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_DMESG,
@@ -1510,7 +1510,7 @@ static int _rtl92ee_set_media_status(struct ieee80211_hw *hw,
 			 mode);
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
+	rtl_write_byte(rtlpriv, MSR, bt_msr | mode);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
 	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/rf.h b/drivers/net/wireless/rtlwifi/rtl8192ee/rf.h
index 8bdeed3c064e..039c0133ad6b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/rf.h
@@ -27,7 +27,6 @@
 #define __RTL92E_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
 
 void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
 				      u8 bandwidth);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/def.h b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
index ef87c09b77d0..41466f957cdc 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
@@ -31,7 +31,6 @@
 
 #define RX_MPDU_QUEUE				0
 #define RX_CMD_QUEUE				1
-#define RX_MAX_QUEUE				2
 
 #define SHORT_SLOT_TIME				9
 #define NON_SHORT_SLOT_TIME			20
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
index 5761d5b49e39..12b0978ba4fa 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
@@ -293,7 +293,7 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 					acm_ctrl &= (~AcmHw_ViqEn);
 					break;
 				case AC3_VO:
-					acm_ctrl &= (~AcmHw_BeqEn);
+					acm_ctrl &= (~AcmHw_VoqEn);
 					break;
 				default:
 					RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -1204,7 +1204,7 @@ static int _rtl92se_set_media_status(struct ieee80211_hw *hw,
 	if (type != NL80211_IFTYPE_AP &&
 	    rtlpriv->mac80211.link_state < MAC80211_LINKED)
 		bt_msr = rtl_read_byte(rtlpriv, MSR) & ~MSR_LINK_MASK;
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtl_write_byte(rtlpriv, MSR, bt_msr);
 
 	temp = rtl_read_dword(rtlpriv, TCR);
 	rtl_write_dword(rtlpriv, TCR, temp & (~BIT(8)));
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/def.h b/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
index 94bdd4bbca5d..bcdf2273688e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
@@ -26,53 +26,12 @@
 #ifndef __RTL8723E_DEF_H__
 #define __RTL8723E_DEF_H__
 
-#define HAL_RETRY_LIMIT_INFRA				48
-#define HAL_RETRY_LIMIT_AP_ADHOC			7
-
-#define RESET_DELAY_8185					20
-
-#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
-#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
-
-#define NUM_OF_FIRMWARE_QUEUE				10
-#define NUM_OF_PAGES_IN_FW					0x100
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA		0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT		0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH		0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
-
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM		0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM		0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM		0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM		0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM		0x00
-
-#define MAX_LINES_HWCONFIG_TXT				1000
-#define MAX_BYTES_LINE_HWCONFIG_TXT			256
-
-#define SW_THREE_WIRE						0
-#define HW_THREE_WIRE						2
-
-#define BT_DEMO_BOARD						0
-#define BT_QA_BOARD							1
-#define BT_FPGA								2
-
 #define HAL_PRIME_CHNL_OFFSET_DONT_CARE		0
 #define HAL_PRIME_CHNL_OFFSET_LOWER			1
 #define HAL_PRIME_CHNL_OFFSET_UPPER			2
 
-#define MAX_H2C_QUEUE_NUM					10
-
 #define RX_MPDU_QUEUE						0
 #define RX_CMD_QUEUE						1
-#define RX_MAX_QUEUE						2
-#define AC2QUEUEID(_AC)						(_AC)
 
 #define	C2H_RX_CMD_HDR_LEN					8
 #define	GET_C2H_CMD_CMD_LEN(__prxhdr)		\
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
index aa085462d0e9..67bb47d77b68 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
@@ -362,7 +362,7 @@ void rtl8723e_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 					acm_ctrl &= (~ACMHW_VIQEN);
 					break;
 				case AC3_VO:
-					acm_ctrl &= (~ACMHW_BEQEN);
+					acm_ctrl &= (~ACMHW_VOQEN);
 					break;
 				default:
 					RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
@@ -1183,7 +1183,7 @@ static int _rtl8723e_set_media_status(struct ieee80211_hw *hw,
 			 mode);
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
+	rtl_write_byte(rtlpriv, MSR, bt_msr | mode);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
 	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h b/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h
index f3f45b16361f..7b44ebc0fac9 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h
@@ -27,7 +27,6 @@
 #define __RTL8723E_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
 
 void rtl8723e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
 				       u8 bandwidth);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
index 2367e8f47a5b..e77c3a46c94a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
@@ -309,7 +309,7 @@ static void rtl8723be_dm_find_minimum_rssi(struct ieee80211_hw *hw)
 		rtl_dm_dig->min_undec_pwdb_for_dm =
 				rtlpriv->dm.entry_min_undec_sm_pwdb;
 		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
-			 "AP Ext Port or disconnet PWDB = 0x%x\n",
+			 "AP Ext Port or disconnect PWDB = 0x%x\n",
 			  rtl_dm_dig->min_undec_pwdb_for_dm);
 	}
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "MinUndecoratedPWDBForDM =%d\n",
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/hw.c b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
index 6dad28e77bbb..b681af3c7a35 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
@@ -603,7 +603,7 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				acm_ctrl &= (~ACMHW_VIQEN);
 				break;
 			case AC3_VO:
-				acm_ctrl &= (~ACMHW_BEQEN);
+				acm_ctrl &= (~ACMHW_VOQEN);
 				break;
 			default:
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
@@ -1558,7 +1558,7 @@ static int _rtl8723be_set_media_status(struct ieee80211_hw *hw,
 			 mode);
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
+	rtl_write_byte(rtlpriv, MSR, bt_msr | mode);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
 	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/rf.h b/drivers/net/wireless/rtlwifi/rtl8723be/rf.h
index a6fea106ced4..f423e157020f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/rf.h
@@ -27,7 +27,6 @@
 #define __RTL8723BE_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
 
 void rtl8723be_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
 					u8 bandwidth);
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/def.h b/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
index ee7c208bd070..dfbdf539de1a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
@@ -118,55 +118,14 @@
 #define	WIFI_NAV_UPPER_US				30000
 #define HAL_92C_NAV_UPPER_UNIT			128
 
-#define HAL_RETRY_LIMIT_INFRA				48
-#define HAL_RETRY_LIMIT_AP_ADHOC			7
-
-#define RESET_DELAY_8185					20
-
-#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
-#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
-
-#define NUM_OF_FIRMWARE_QUEUE				10
-#define NUM_OF_PAGES_IN_FW					0x100
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA		0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT		0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH		0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
-
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM		0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM		0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM		0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM		0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM		0x00
-
 #define MAX_RX_DMA_BUFFER_SIZE				0x3E80
 
-#define MAX_LINES_HWCONFIG_TXT				1000
-#define MAX_BYTES_LINE_HWCONFIG_TXT			256
-
-#define SW_THREE_WIRE						0
-#define HW_THREE_WIRE						2
-
-#define BT_DEMO_BOARD						0
-#define BT_QA_BOARD							1
-#define BT_FPGA								2
-
 #define HAL_PRIME_CHNL_OFFSET_DONT_CARE		0
 #define HAL_PRIME_CHNL_OFFSET_LOWER			1
 #define HAL_PRIME_CHNL_OFFSET_UPPER			2
 
-#define MAX_H2C_QUEUE_NUM					10
-
 #define RX_MPDU_QUEUE						0
 #define RX_CMD_QUEUE						1
-#define RX_MAX_QUEUE						2
-#define AC2QUEUEID(_AC)						(_AC)
 
 #define MAX_RX_DMA_BUFFER_SIZE_8812	0x3E80
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
index 0b2082dc48f1..342678d2ed42 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
@@ -873,7 +873,7 @@ static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
 
 	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
-			 "Abnornally false alarm case.\n");
+			 "Abnormally false alarm case.\n");
 
 		if (dm_digtable->large_fa_hit != 3)
 			dm_digtable->large_fa_hit++;
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
index 8ec8200002c7..8704eee9f3a4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
@@ -423,7 +423,7 @@ void rtl8821ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		*((u16 *)(val+4)) = rtl_read_word(rtlpriv, REG_BSSID+4);
 		break;
 	case HW_VAR_MEDIA_STATUS:
-		val[0] = rtl_read_byte(rtlpriv, REG_CR+2) & 0x3;
+		val[0] = rtl_read_byte(rtlpriv, MSR) & 0x3;
 		break;
 	case HW_VAR_SLOT_TIME:
 		*((u8 *)(val)) = mac->slot_time;
@@ -667,7 +667,7 @@ void rtl8821ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				acm_ctrl &= (~ACMHW_VIQEN);
 				break;
 			case AC3_VO:
-				acm_ctrl &= (~ACMHW_BEQEN);
+				acm_ctrl &= (~ACMHW_VOQEN);
 				break;
 			default:
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
@@ -1515,7 +1515,7 @@ static bool _rtl8821ae_dynamic_rqpn(struct ieee80211_hw *hw, u32 boundary,
 				      (u8 *)(&support_remote_wakeup));
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "boundary=0x%#X, NPQ_RQPNValue=0x%#X, RQPNValue=0x%#X\n",
+		 "boundary=%#X, NPQ_RQPNValue=%#X, RQPNValue=%#X\n",
 		  boundary, npq_rqpn_value, rqpn_val);
 
 	/* stop PCIe DMA
@@ -2178,7 +2178,7 @@ static int _rtl8821ae_set_media_status(struct ieee80211_hw *hw,
 		return 1;
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtl_write_byte(rtlpriv, MSR, bt_msr);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
 	if ((bt_msr & 0xfc) == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/rf.h b/drivers/net/wireless/rtlwifi/rtl8821ae/rf.h
index d9582ee1c335..efd22bd0b139 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/rf.h
@@ -27,7 +27,6 @@
 #define __RTL8821AE_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
 
 void rtl8821ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
 					u8 bandwidth);
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
index 72af4b9ee32b..174743aef943 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
@@ -64,6 +64,20 @@ static u16 odm_cfo(char value)
 	return ret_val;
 }
 
+static u8 _rtl8821ae_evm_dbm_jaguar(char value)
+{
+	char ret_val = value;
+
+	/* -33dB~0dB to 33dB ~ 0dB*/
+	if (ret_val == -128)
+		ret_val = 127;
+	else if (ret_val < 0)
+		ret_val = 0 - ret_val;
+
+	ret_val  = ret_val >> 1;
+	return ret_val;
+}
+
 static void query_rxphystatus(struct ieee80211_hw *hw,
 			      struct rtl_stats *pstatus, u8 *pdesc,
 			      struct rx_fwinfo_8821ae *p_drvinfo,
@@ -246,7 +260,7 @@ static void query_rxphystatus(struct ieee80211_hw *hw,
 
 		for (i = 0; i < max_spatial_stream; i++) {
 			evm = rtl_evm_db_to_percentage(p_phystrpt->rxevm[i]);
-			evmdbm = rtl_evm_dbm_jaguar(p_phystrpt->rxevm[i]);
+			evmdbm = _rtl8821ae_evm_dbm_jaguar(p_phystrpt->rxevm[i]);
 
 			if (bpacket_match_bssid) {
 				/* Fill value in RFD, Get the first
diff --git a/drivers/net/wireless/rtlwifi/stats.c b/drivers/net/wireless/rtlwifi/stats.c
index 2d0736a09fc0..d8b30690b00d 100644
--- a/drivers/net/wireless/rtlwifi/stats.c
+++ b/drivers/net/wireless/rtlwifi/stats.c
@@ -39,15 +39,8 @@ EXPORT_SYMBOL(rtl_query_rxpwrpercentage);
 
 u8 rtl_evm_db_to_percentage(char value)
 {
-	char ret_val;
-	ret_val = value;
+	char ret_val = clamp(-value, 0, 33) * 3;
 
-	if (ret_val >= 0)
-		ret_val = 0;
-	if (ret_val <= -33)
-		ret_val = -33;
-	ret_val = 0 - ret_val;
-	ret_val *= 3;
 	if (ret_val == 99)
 		ret_val = 100;
 
@@ -55,21 +48,6 @@ u8 rtl_evm_db_to_percentage(char value)
 }
 EXPORT_SYMBOL(rtl_evm_db_to_percentage);
 
-u8 rtl_evm_dbm_jaguar(char value)
-{
-	char ret_val = value;
-
-	/* -33dB~0dB to 33dB ~ 0dB*/
-	if (ret_val == -128)
-		ret_val = 127;
-	else if (ret_val < 0)
-		ret_val = 0 - ret_val;
-
-	ret_val  = ret_val >> 1;
-	return ret_val;
-}
-EXPORT_SYMBOL(rtl_evm_dbm_jaguar);
-
 static long rtl_translate_todbm(struct ieee80211_hw *hw,
 			 u8 signal_strength_index)
 {
diff --git a/drivers/net/wireless/rtlwifi/stats.h b/drivers/net/wireless/rtlwifi/stats.h
index aa4eec80ccf7..2b57dffef572 100644
--- a/drivers/net/wireless/rtlwifi/stats.h
+++ b/drivers/net/wireless/rtlwifi/stats.h
@@ -35,7 +35,6 @@
 
 u8 rtl_query_rxpwrpercentage(char antpower);
 u8 rtl_evm_db_to_percentage(char value);
-u8 rtl_evm_dbm_jaguar(char value);
 long rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig);
 void rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
 			 struct rtl_stats *pstatus);
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index 46ee956d0235..f0188c83c79f 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -701,12 +701,18 @@ free:
 
 static void _rtl_usb_cleanup_rx(struct ieee80211_hw *hw)
 {
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
 	struct urb *urb;
 
 	usb_kill_anchored_urbs(&rtlusb->rx_submitted);
 
 	tasklet_kill(&rtlusb->rx_work_tasklet);
+	cancel_work_sync(&rtlpriv->works.lps_change_work);
+
+	flush_workqueue(rtlpriv->works.rtl_wq);
+	destroy_workqueue(rtlpriv->works.rtl_wq);
+
 	skb_queue_purge(&rtlusb->rx_queue);
 
 	while ((urb = usb_get_from_anchor(&rtlusb->rx_cleanup_urbs))) {
@@ -794,8 +800,6 @@ static void rtl_usb_cleanup(struct ieee80211_hw *hw)
 	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
 	struct ieee80211_tx_info *txinfo;
 
-	SET_USB_STOP(rtlusb);
-
 	/* clean up rx stuff. */
 	_rtl_usb_cleanup_rx(hw);
 
@@ -834,7 +838,6 @@ static void rtl_usb_stop(struct ieee80211_hw *hw)
 	cancel_work_sync(&rtlpriv->works.fill_h2c_cmd);
 	/* Enable software */
 	SET_USB_STOP(rtlusb);
-	rtl_usb_deinit(hw);
 	rtlpriv->cfg->ops->hw_disable(hw);
 }
 
@@ -1147,9 +1150,9 @@ void rtl_usb_disconnect(struct usb_interface *intf)
 
 	if (unlikely(!rtlpriv))
 		return;
-
 	/* just in case driver is removed before firmware callback */
 	wait_for_completion(&rtlpriv->firmware_loading_complete);
+	clear_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
 	/*ieee80211_unregister_hw will call ops_stop */
 	if (rtlmac->mac80211_registered == 1) {
 		ieee80211_unregister_hw(hw);
diff --git a/drivers/net/wireless/ti/wl1251/main.c b/drivers/net/wireless/ti/wl1251/main.c
index d4ba009ac9aa..5d54d16a59e7 100644
--- a/drivers/net/wireless/ti/wl1251/main.c
+++ b/drivers/net/wireless/ti/wl1251/main.c
@@ -468,7 +468,7 @@ static void wl1251_op_stop(struct ieee80211_hw *hw)
 	wl1251_tx_flush(wl);
 	wl1251_power_off(wl);
 
-	memset(wl->bssid, 0, ETH_ALEN);
+	eth_zero_addr(wl->bssid);
 	wl->listen_int = 1;
 	wl->bss_type = MAX_BSS_TYPE;
 
@@ -547,7 +547,7 @@ static void wl1251_op_remove_interface(struct ieee80211_hw *hw,
 	mutex_lock(&wl->mutex);
 	wl1251_debug(DEBUG_MAC80211, "mac80211 remove interface");
 	wl->vif = NULL;
-	memset(wl->bssid, 0, ETH_ALEN);
+	eth_zero_addr(wl->bssid);
 	mutex_unlock(&wl->mutex);
 }
 
@@ -1608,7 +1608,7 @@ int wl1251_free_hw(struct wl1251 *wl)
 }
 EXPORT_SYMBOL_GPL(wl1251_free_hw);
 
-MODULE_DESCRIPTION("TI wl1251 Wireles LAN Driver Core");
+MODULE_DESCRIPTION("TI wl1251 Wireless LAN Driver Core");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Kalle Valo <kvalo@adurom.com>");
 MODULE_FIRMWARE(WL1251_FW_NAME);
diff --git a/drivers/net/wireless/ti/wl18xx/debugfs.c b/drivers/net/wireless/ti/wl18xx/debugfs.c
index c93fae95baac..5fbd2230f372 100644
--- a/drivers/net/wireless/ti/wl18xx/debugfs.c
+++ b/drivers/net/wireless/ti/wl18xx/debugfs.c
@@ -139,7 +139,7 @@ WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, protection_filter, "%u");
 WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, accum_arp_pend_requests, "%u");
 WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, max_arp_queue_dep, "%u");
 
-WL18XX_DEBUGFS_FWSTATS_FILE(rx_rate, rx_frames_per_rates, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE_ARRAY(rx_rate, rx_frames_per_rates, 50);
 
 WL18XX_DEBUGFS_FWSTATS_FILE_ARRAY(aggr_size, tx_agg_vs_rate,
 				  AGGR_STATS_TX_AGG*AGGR_STATS_TX_RATE);
diff --git a/drivers/net/wireless/ti/wl18xx/event.c b/drivers/net/wireless/ti/wl18xx/event.c
index c28f06854195..548bb9e7e91e 100644
--- a/drivers/net/wireless/ti/wl18xx/event.c
+++ b/drivers/net/wireless/ti/wl18xx/event.c
@@ -77,7 +77,7 @@ static int wlcore_smart_config_sync_event(struct wl1271 *wl, u8 sync_channel,
 	wl1271_debug(DEBUG_EVENT,
 		     "SMART_CONFIG_SYNC_EVENT_ID, freq: %d (chan: %d band %d)",
 		     freq, sync_channel, sync_band);
-	skb = cfg80211_vendor_event_alloc(wl->hw->wiphy, 20,
+	skb = cfg80211_vendor_event_alloc(wl->hw->wiphy, NULL, 20,
 					  WLCORE_VENDOR_EVENT_SC_SYNC,
 					  GFP_KERNEL);
 
@@ -98,7 +98,7 @@ static int wlcore_smart_config_decode_event(struct wl1271 *wl,
 	wl1271_debug(DEBUG_EVENT, "SMART_CONFIG_DECODE_EVENT_ID");
 	wl1271_dump_ascii(DEBUG_EVENT, "SSID:", ssid, ssid_len);
 
-	skb = cfg80211_vendor_event_alloc(wl->hw->wiphy,
+	skb = cfg80211_vendor_event_alloc(wl->hw->wiphy, NULL,
 					  ssid_len + pwd_len + 20,
 					  WLCORE_VENDOR_EVENT_SC_DECODE,
 					  GFP_KERNEL);
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index c26fc2106e5b..68919f8d4310 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -367,7 +367,7 @@ void wl12xx_free_link(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 *hlid)
 	wl->links[*hlid].allocated_pkts = 0;
 	wl->links[*hlid].prev_freed_pkts = 0;
 	wl->links[*hlid].ba_bitmap = 0;
-	memset(wl->links[*hlid].addr, 0, ETH_ALEN);
+	eth_zero_addr(wl->links[*hlid].addr);
 
 	/*
 	 * At this point op_tx() will not add more packets to the queues. We
@@ -1293,7 +1293,7 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	hdr->frame_control = cpu_to_le16(fc);
 	memcpy(hdr->addr1, vif->bss_conf.bssid, ETH_ALEN);
 	memcpy(hdr->addr2, vif->addr, ETH_ALEN);
-	memset(hdr->addr3, 0xff, ETH_ALEN);
+	eth_broadcast_addr(hdr->addr3);
 
 	ret = wl1271_cmd_template_set(wl, wlvif->role_id, CMD_TEMPL_ARP_RSP,
 				      skb->data, skb->len, 0,
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.h b/drivers/net/wireless/ti/wlcore/debugfs.h
index 0f2cfb0d2a9e..bf14676e6515 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.h
+++ b/drivers/net/wireless/ti/wlcore/debugfs.h
@@ -26,8 +26,8 @@
 
 #include "wlcore.h"
 
-int wl1271_format_buffer(char __user *userbuf, size_t count,
-			 loff_t *ppos, char *fmt, ...);
+__printf(4, 5) int wl1271_format_buffer(char __user *userbuf, size_t count,
+					loff_t *ppos, char *fmt, ...);
 
 int wl1271_debugfs_init(struct wl1271 *wl);
 void wl1271_debugfs_exit(struct wl1271 *wl);
diff --git a/drivers/net/xen-netback/common.h b/drivers/net/xen-netback/common.h
index 589fa256256b..8a495b318b6f 100644
--- a/drivers/net/xen-netback/common.h
+++ b/drivers/net/xen-netback/common.h
@@ -238,6 +238,8 @@ struct xenvif {
 	unsigned int num_queues; /* active queues, resource allocated */
 	unsigned int stalled_queues;
 
+	struct xenbus_watch credit_watch;
+
 	spinlock_t lock;
 
 #ifdef CONFIG_DEBUG_FS
@@ -260,6 +262,8 @@ static inline struct xenbus_device *xenvif_to_xenbus_device(struct xenvif *vif)
 	return to_xenbus_device(vif->dev->dev.parent);
 }
 
+void xenvif_tx_credit_callback(unsigned long data);
+
 struct xenvif *xenvif_alloc(struct device *parent,
 			    domid_t domid,
 			    unsigned int handle);
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index 3aa8648080c8..1a83e190fc15 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -437,7 +437,7 @@ struct xenvif *xenvif_alloc(struct device *parent, domid_t domid,
 	 * stolen by an Ethernet bridge for STP purposes.
 	 * (FE:FF:FF:FF:FF:FF)
 	 */
-	memset(dev->dev_addr, 0xFF, ETH_ALEN);
+	eth_broadcast_addr(dev->dev_addr);
 	dev->dev_addr[0] &= ~0x01;
 
 	netif_carrier_off(dev);
@@ -463,6 +463,7 @@ int xenvif_init_queue(struct xenvif_queue *queue)
 	queue->credit_bytes = queue->remaining_credit = ~0UL;
 	queue->credit_usec  = 0UL;
 	init_timer(&queue->credit_timeout);
+	queue->credit_timeout.function = xenvif_tx_credit_callback;
 	queue->credit_window_start = get_jiffies_64();
 
 	queue->rx_queue_max = XENVIF_RX_QUEUE_BYTES;
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index cab9f5257f57..b8c471813f4c 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);
 
@@ -641,7 +642,7 @@ static void tx_add_credit(struct xenvif_queue *queue)
 	queue->remaining_credit = min(max_credit, max_burst);
 }
 
-static void tx_credit_callback(unsigned long data)
+void xenvif_tx_credit_callback(unsigned long data)
 {
 	struct xenvif_queue *queue = (struct xenvif_queue *)data;
 	tx_add_credit(queue);
@@ -655,15 +656,10 @@ static void xenvif_tx_err(struct xenvif_queue *queue,
 	unsigned long flags;
 
 	do {
-		int notify;
-
 		spin_lock_irqsave(&queue->response_lock, flags);
 		make_tx_response(queue, txp, XEN_NETIF_RSP_ERROR);
-		RING_PUSH_RESPONSES_AND_CHECK_NOTIFY(&queue->tx, notify);
+		push_tx_responses(queue);
 		spin_unlock_irqrestore(&queue->response_lock, flags);
-		if (notify)
-			notify_remote_via_irq(queue->tx_irq);
-
 		if (cons == end)
 			break;
 		txp = RING_GET_REQUEST(&queue->tx, cons++);
@@ -1169,8 +1165,6 @@ static bool tx_credit_exceeded(struct xenvif_queue *queue, unsigned size)
 	if (size > queue->remaining_credit) {
 		queue->credit_timeout.data     =
 			(unsigned long)queue;
-		queue->credit_timeout.function =
-			tx_credit_callback;
 		mod_timer(&queue->credit_timeout,
 			  next_credit);
 		queue->credit_window_start = next_credit;
@@ -1657,7 +1651,6 @@ static void xenvif_idx_release(struct xenvif_queue *queue, u16 pending_idx,
 {
 	struct pending_tx_info *pending_tx_info;
 	pending_ring_idx_t index;
-	int notify;
 	unsigned long flags;
 
 	pending_tx_info = &queue->pending_tx_info[pending_idx];
@@ -1673,12 +1666,9 @@ static void xenvif_idx_release(struct xenvif_queue *queue, u16 pending_idx,
 	index = pending_index(queue->pending_prod++);
 	queue->pending_ring[index] = pending_idx;
 
-	RING_PUSH_RESPONSES_AND_CHECK_NOTIFY(&queue->tx, notify);
+	push_tx_responses(queue);
 
 	spin_unlock_irqrestore(&queue->response_lock, flags);
-
-	if (notify)
-		notify_remote_via_irq(queue->tx_irq);
 }
 
 
@@ -1699,6 +1689,15 @@ static void make_tx_response(struct xenvif_queue *queue,
 	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);
+}
+
 static struct xen_netif_rx_response *make_rx_response(struct xenvif_queue *queue,
 					     u16      id,
 					     s8       st,
diff --git a/drivers/net/xen-netback/xenbus.c b/drivers/net/xen-netback/xenbus.c
index 794204e34fba..3d8dbf5f2d39 100644
--- a/drivers/net/xen-netback/xenbus.c
+++ b/drivers/net/xen-netback/xenbus.c
@@ -41,6 +41,7 @@ static void connect(struct backend_info *be);
 static int read_xenbus_vif_flags(struct backend_info *be);
 static int backend_create_xenvif(struct backend_info *be);
 static void unregister_hotplug_status_watch(struct backend_info *be);
+static void xen_unregister_watchers(struct xenvif *vif);
 static void set_backend_state(struct backend_info *be,
 			      enum xenbus_state state);
 
@@ -232,6 +233,7 @@ static int netback_remove(struct xenbus_device *dev)
 	unregister_hotplug_status_watch(be);
 	if (be->vif) {
 		kobject_uevent(&dev->dev.kobj, KOBJ_OFFLINE);
+		xen_unregister_watchers(be->vif);
 		xenbus_rm(XBT_NIL, dev->nodename, "hotplug-status");
 		xenvif_free(be->vif);
 		be->vif = NULL;
@@ -430,6 +432,7 @@ static int backend_create_xenvif(struct backend_info *be)
 static void backend_disconnect(struct backend_info *be)
 {
 	if (be->vif) {
+		xen_unregister_watchers(be->vif);
 #ifdef CONFIG_DEBUG_FS
 		xenvif_debugfs_delif(be->vif);
 #endif /* CONFIG_DEBUG_FS */
@@ -645,6 +648,59 @@ static int xen_net_read_mac(struct xenbus_device *dev, u8 mac[])
 	return 0;
 }
 
+static void xen_net_rate_changed(struct xenbus_watch *watch,
+				const char **vec, unsigned int len)
+{
+	struct xenvif *vif = container_of(watch, struct xenvif, credit_watch);
+	struct xenbus_device *dev = xenvif_to_xenbus_device(vif);
+	unsigned long   credit_bytes;
+	unsigned long   credit_usec;
+	unsigned int queue_index;
+
+	xen_net_read_rate(dev, &credit_bytes, &credit_usec);
+	for (queue_index = 0; queue_index < vif->num_queues; queue_index++) {
+		struct xenvif_queue *queue = &vif->queues[queue_index];
+
+		queue->credit_bytes = credit_bytes;
+		queue->credit_usec = credit_usec;
+		if (!mod_timer_pending(&queue->credit_timeout, jiffies) &&
+			queue->remaining_credit > queue->credit_bytes) {
+			queue->remaining_credit = queue->credit_bytes;
+		}
+	}
+}
+
+static int xen_register_watchers(struct xenbus_device *dev, struct xenvif *vif)
+{
+	int err = 0;
+	char *node;
+	unsigned maxlen = strlen(dev->nodename) + sizeof("/rate");
+
+	node = kmalloc(maxlen, GFP_KERNEL);
+	if (!node)
+		return -ENOMEM;
+	snprintf(node, maxlen, "%s/rate", dev->nodename);
+	vif->credit_watch.node = node;
+	vif->credit_watch.callback = xen_net_rate_changed;
+	err = register_xenbus_watch(&vif->credit_watch);
+	if (err) {
+		pr_err("Failed to set watcher %s\n", vif->credit_watch.node);
+		kfree(node);
+		vif->credit_watch.node = NULL;
+		vif->credit_watch.callback = NULL;
+	}
+	return err;
+}
+
+static void xen_unregister_watchers(struct xenvif *vif)
+{
+	if (vif->credit_watch.node) {
+		unregister_xenbus_watch(&vif->credit_watch);
+		kfree(vif->credit_watch.node);
+		vif->credit_watch.node = NULL;
+	}
+}
+
 static void unregister_hotplug_status_watch(struct backend_info *be)
 {
 	if (be->have_hotplug_status_watch) {
@@ -709,6 +765,7 @@ static void connect(struct backend_info *be)
 	}
 
 	xen_net_read_rate(dev, &credit_bytes, &credit_usec);
+	xen_register_watchers(dev, be->vif);
 	read_xenbus_vif_flags(be);
 
 	/* Use the number of queues requested by the frontend */
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/nfc/Kconfig b/drivers/nfc/Kconfig
index 7929fac13e1c..107714e4405f 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -73,4 +73,5 @@ source "drivers/nfc/microread/Kconfig"
 source "drivers/nfc/nfcmrvl/Kconfig"
 source "drivers/nfc/st21nfca/Kconfig"
 source "drivers/nfc/st21nfcb/Kconfig"
+source "drivers/nfc/nxp-nci/Kconfig"
 endmenu
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index 6b23a2c6e34a..a4292d790f9b 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -13,5 +13,6 @@ obj-$(CONFIG_NFC_MRVL)		+= nfcmrvl/
 obj-$(CONFIG_NFC_TRF7970A)	+= trf7970a.o
 obj-$(CONFIG_NFC_ST21NFCA)  	+= st21nfca/
 obj-$(CONFIG_NFC_ST21NFCB)	+= st21nfcb/
+obj-$(CONFIG_NFC_NXP_NCI)	+= nxp-nci/
 
 ccflags-$(CONFIG_NFC_DEBUG) := -DDEBUG
diff --git a/drivers/nfc/microread/i2c.c b/drivers/nfc/microread/i2c.c
index df85cd3d9db0..661e2c8143c4 100644
--- a/drivers/nfc/microread/i2c.c
+++ b/drivers/nfc/microread/i2c.c
@@ -286,7 +286,7 @@ static int microread_i2c_probe(struct i2c_client *client,
 	if (r < 0)
 		goto err_irq;
 
-	nfc_info(&client->dev, "Probed");
+	nfc_info(&client->dev, "Probed\n");
 
 	return 0;
 
diff --git a/drivers/nfc/nfcmrvl/main.c b/drivers/nfc/nfcmrvl/main.c
index 85e8bcf98693..ad4933cefbd1 100644
--- a/drivers/nfc/nfcmrvl/main.c
+++ b/drivers/nfc/nfcmrvl/main.c
@@ -111,7 +111,7 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 
 	priv->ndev = nci_allocate_device(&nfcmrvl_nci_ops, protocols, 0, 0);
 	if (!priv->ndev) {
-		nfc_err(dev, "nci_allocate_device failed");
+		nfc_err(dev, "nci_allocate_device failed\n");
 		rc = -ENOMEM;
 		goto error;
 	}
@@ -120,7 +120,7 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 
 	rc = nci_register_device(priv->ndev);
 	if (rc) {
-		nfc_err(dev, "nci_register_device failed %d", rc);
+		nfc_err(dev, "nci_register_device failed %d\n", rc);
 		nci_free_device(priv->ndev);
 		goto error;
 	}
diff --git a/drivers/nfc/nfcmrvl/usb.c b/drivers/nfc/nfcmrvl/usb.c
index 3221ca37d6c9..6cf15c1a2618 100644
--- a/drivers/nfc/nfcmrvl/usb.c
+++ b/drivers/nfc/nfcmrvl/usb.c
@@ -80,7 +80,7 @@ static void nfcmrvl_bulk_complete(struct urb *urb)
 	if (!urb->status) {
 		if (nfcmrvl_nci_recv_frame(drv_data->priv, urb->transfer_buffer,
 					   urb->actual_length) < 0)
-			nfc_err(&drv_data->udev->dev, "corrupted Rx packet");
+			nfc_err(&drv_data->udev->dev, "corrupted Rx packet\n");
 	}
 
 	if (!test_bit(NFCMRVL_USB_BULK_RUNNING, &drv_data->flags))
@@ -96,7 +96,7 @@ static void nfcmrvl_bulk_complete(struct urb *urb)
 		 */
 		if (err != -EPERM && err != -ENODEV)
 			nfc_err(&drv_data->udev->dev,
-				"urb %p failed to resubmit (%d)", urb, -err);
+				"urb %p failed to resubmit (%d)\n", urb, -err);
 		usb_unanchor_urb(urb);
 	}
 }
@@ -137,7 +137,7 @@ nfcmrvl_submit_bulk_urb(struct nfcmrvl_usb_drv_data *drv_data, gfp_t mem_flags)
 	if (err) {
 		if (err != -EPERM && err != -ENODEV)
 			nfc_err(&drv_data->udev->dev,
-				"urb %p submission failed (%d)", urb, -err);
+				"urb %p submission failed (%d)\n", urb, -err);
 		usb_unanchor_urb(urb);
 	}
 
@@ -153,7 +153,7 @@ static void nfcmrvl_tx_complete(struct urb *urb)
 	struct nfcmrvl_private *priv = nci_get_drvdata(ndev);
 	struct nfcmrvl_usb_drv_data *drv_data = priv->drv_data;
 
-	nfc_info(priv->dev, "urb %p status %d count %d",
+	nfc_info(priv->dev, "urb %p status %d count %d\n",
 		 urb, urb->status, urb->actual_length);
 
 	spin_lock(&drv_data->txlock);
@@ -253,7 +253,7 @@ static int nfcmrvl_usb_nci_send(struct nfcmrvl_private *priv,
 	if (err) {
 		if (err != -EPERM && err != -ENODEV)
 			nfc_err(&drv_data->udev->dev,
-				"urb %p submission failed (%d)", urb, -err);
+				"urb %p submission failed (%d)\n", urb, -err);
 		kfree(urb->setup_packet);
 		usb_unanchor_urb(urb);
 	} else {
@@ -293,7 +293,7 @@ static int nfcmrvl_probe(struct usb_interface *intf,
 	int i;
 	struct usb_device *udev = interface_to_usbdev(intf);
 
-	nfc_info(&udev->dev, "intf %p id %p", intf, id);
+	nfc_info(&udev->dev, "intf %p id %p\n", intf, id);
 
 	drv_data = devm_kzalloc(&intf->dev, sizeof(*drv_data), GFP_KERNEL);
 	if (!drv_data)
@@ -348,7 +348,7 @@ static void nfcmrvl_disconnect(struct usb_interface *intf)
 	if (!drv_data)
 		return;
 
-	nfc_info(&drv_data->udev->dev, "intf %p", intf);
+	nfc_info(&drv_data->udev->dev, "intf %p\n", intf);
 
 	nfcmrvl_nci_unregister_dev(drv_data->priv);
 
@@ -360,7 +360,7 @@ static int nfcmrvl_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct nfcmrvl_usb_drv_data *drv_data = usb_get_intfdata(intf);
 
-	nfc_info(&drv_data->udev->dev, "intf %p", intf);
+	nfc_info(&drv_data->udev->dev, "intf %p\n", intf);
 
 	if (drv_data->suspend_count++)
 		return 0;
@@ -401,7 +401,7 @@ static int nfcmrvl_resume(struct usb_interface *intf)
 	struct nfcmrvl_usb_drv_data *drv_data = usb_get_intfdata(intf);
 	int err = 0;
 
-	nfc_info(&drv_data->udev->dev, "intf %p", intf);
+	nfc_info(&drv_data->udev->dev, "intf %p\n", intf);
 
 	if (--drv_data->suspend_count)
 		return 0;
diff --git a/drivers/nfc/nxp-nci/Kconfig b/drivers/nfc/nxp-nci/Kconfig
new file mode 100644
index 000000000000..37b40612520d
--- /dev/null
+++ b/drivers/nfc/nxp-nci/Kconfig
@@ -0,0 +1,25 @@
+config NFC_NXP_NCI
+	tristate "NXP-NCI NFC driver"
+	depends on NFC_NCI
+	default n
+	---help---
+	  Generic core driver for NXP NCI chips such as the NPC100
+	  or PN7150 families.
+	  This is a driver based on the NCI NFC kernel layers and
+	  will thus not work with NXP libnfc library.
+
+	  To compile this driver as a module, choose m here. The module will
+	  be called nxp_nci.
+	  Say N if unsure.
+
+config NFC_NXP_NCI_I2C
+	tristate "NXP-NCI I2C support"
+	depends on NFC_NXP_NCI && I2C
+	---help---
+	  This module adds support for an I2C interface to the NXP NCI
+	  chips.
+	  Select this if your platform is using the I2C bus.
+
+	  To compile this driver as a module, choose m here. The module will
+	  be called nxp_nci_i2c.
+	  Say Y if unsure.
diff --git a/drivers/nfc/nxp-nci/Makefile b/drivers/nfc/nxp-nci/Makefile
new file mode 100644
index 000000000000..c008be30bb18
--- /dev/null
+++ b/drivers/nfc/nxp-nci/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for NXP-NCI NFC driver
+#
+
+nxp-nci-objs = core.o firmware.o
+nxp-nci_i2c-objs = i2c.o
+
+obj-$(CONFIG_NFC_NXP_NCI) += nxp-nci.o
+obj-$(CONFIG_NFC_NXP_NCI_I2C) += nxp-nci_i2c.o
+
+ccflags-$(CONFIG_NFC_DEBUG) := -DDEBUG
diff --git a/drivers/nfc/nxp-nci/core.c b/drivers/nfc/nxp-nci/core.c
new file mode 100644
index 000000000000..8979636d48ea
--- /dev/null
+++ b/drivers/nfc/nxp-nci/core.c
@@ -0,0 +1,186 @@
+/*
+ * Generic driver for NXP NCI NFC chips
+ *
+ * Copyright (C) 2014  NXP Semiconductors  All rights reserved.
+ *
+ * Authors: Clément Perrochaud <clement.perrochaud@nxp.com>
+ *
+ * Derived from PN544 device driver:
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+#include <linux/platform_data/nxp-nci.h>
+
+#include <net/nfc/nci_core.h>
+
+#include "nxp-nci.h"
+
+#define NXP_NCI_HDR_LEN	4
+
+#define NXP_NCI_NFC_PROTOCOLS (NFC_PROTO_JEWEL_MASK | \
+			       NFC_PROTO_MIFARE_MASK | \
+			       NFC_PROTO_FELICA_MASK | \
+			       NFC_PROTO_ISO14443_MASK | \
+			       NFC_PROTO_ISO14443_B_MASK | \
+			       NFC_PROTO_NFC_DEP_MASK)
+
+static int nxp_nci_open(struct nci_dev *ndev)
+{
+	struct nxp_nci_info *info = nci_get_drvdata(ndev);
+	int r = 0;
+
+	mutex_lock(&info->info_lock);
+
+	if (info->mode != NXP_NCI_MODE_COLD) {
+		r = -EBUSY;
+		goto open_exit;
+	}
+
+	if (info->phy_ops->set_mode)
+		r = info->phy_ops->set_mode(info->phy_id, NXP_NCI_MODE_NCI);
+
+	info->mode = NXP_NCI_MODE_NCI;
+
+open_exit:
+	mutex_unlock(&info->info_lock);
+	return r;
+}
+
+static int nxp_nci_close(struct nci_dev *ndev)
+{
+	struct nxp_nci_info *info = nci_get_drvdata(ndev);
+	int r = 0;
+
+	mutex_lock(&info->info_lock);
+
+	if (info->phy_ops->set_mode)
+		r = info->phy_ops->set_mode(info->phy_id, NXP_NCI_MODE_COLD);
+
+	info->mode = NXP_NCI_MODE_COLD;
+
+	mutex_unlock(&info->info_lock);
+	return r;
+}
+
+static int nxp_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct nxp_nci_info *info = nci_get_drvdata(ndev);
+	int r;
+
+	if (!info->phy_ops->write) {
+		r = -ENOTSUPP;
+		goto send_exit;
+	}
+
+	if (info->mode != NXP_NCI_MODE_NCI) {
+		r = -EINVAL;
+		goto send_exit;
+	}
+
+	r = info->phy_ops->write(info->phy_id, skb);
+	if (r < 0)
+		kfree_skb(skb);
+
+send_exit:
+	return r;
+}
+
+static struct nci_ops nxp_nci_ops = {
+	.open = nxp_nci_open,
+	.close = nxp_nci_close,
+	.send = nxp_nci_send,
+	.fw_download = nxp_nci_fw_download,
+};
+
+int nxp_nci_probe(void *phy_id, struct device *pdev,
+		  struct nxp_nci_phy_ops *phy_ops, unsigned int max_payload,
+		  struct nci_dev **ndev)
+{
+	struct nxp_nci_info *info;
+	int r;
+
+	info = devm_kzalloc(pdev, sizeof(struct nxp_nci_info), GFP_KERNEL);
+	if (!info) {
+		r = -ENOMEM;
+		goto probe_exit;
+	}
+
+	info->phy_id = phy_id;
+	info->pdev = pdev;
+	info->phy_ops = phy_ops;
+	info->max_payload = max_payload;
+	INIT_WORK(&info->fw_info.work, nxp_nci_fw_work);
+	init_completion(&info->fw_info.cmd_completion);
+	mutex_init(&info->info_lock);
+
+	if (info->phy_ops->set_mode) {
+		r = info->phy_ops->set_mode(info->phy_id, NXP_NCI_MODE_COLD);
+		if (r < 0)
+			goto probe_exit;
+	}
+
+	info->mode = NXP_NCI_MODE_COLD;
+
+	info->ndev = nci_allocate_device(&nxp_nci_ops, NXP_NCI_NFC_PROTOCOLS,
+					 NXP_NCI_HDR_LEN, 0);
+	if (!info->ndev) {
+		r = -ENOMEM;
+		goto probe_exit;
+	}
+
+	nci_set_parent_dev(info->ndev, pdev);
+	nci_set_drvdata(info->ndev, info);
+	r = nci_register_device(info->ndev);
+	if (r < 0)
+		goto probe_exit_free_nci;
+
+	*ndev = info->ndev;
+
+	goto probe_exit;
+
+probe_exit_free_nci:
+	nci_free_device(info->ndev);
+probe_exit:
+	return r;
+}
+EXPORT_SYMBOL(nxp_nci_probe);
+
+void nxp_nci_remove(struct nci_dev *ndev)
+{
+	struct nxp_nci_info *info = nci_get_drvdata(ndev);
+
+	if (info->mode == NXP_NCI_MODE_FW)
+		nxp_nci_fw_work_complete(info, -ESHUTDOWN);
+	cancel_work_sync(&info->fw_info.work);
+
+	mutex_lock(&info->info_lock);
+
+	if (info->phy_ops->set_mode)
+		info->phy_ops->set_mode(info->phy_id, NXP_NCI_MODE_COLD);
+
+	nci_unregister_device(ndev);
+	nci_free_device(ndev);
+
+	mutex_unlock(&info->info_lock);
+}
+EXPORT_SYMBOL(nxp_nci_remove);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("NXP NCI NFC driver");
+MODULE_AUTHOR("Clément Perrochaud <clement.perrochaud@nxp.com>");
diff --git a/drivers/nfc/nxp-nci/firmware.c b/drivers/nfc/nxp-nci/firmware.c
new file mode 100644
index 000000000000..5291797324ba
--- /dev/null
+++ b/drivers/nfc/nxp-nci/firmware.c
@@ -0,0 +1,325 @@
+/*
+ * Generic driver for NXP NCI NFC chips
+ *
+ * Copyright (C) 2014  NXP Semiconductors  All rights reserved.
+ *
+ * Author: Clément Perrochaud <clement.perrochaud@nxp.com>
+ *
+ * Derived from PN544 device driver:
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/completion.h>
+#include <linux/firmware.h>
+#include <linux/nfc.h>
+#include <linux/unaligned/access_ok.h>
+
+#include "nxp-nci.h"
+
+/* Crypto operations can take up to 30 seconds */
+#define NXP_NCI_FW_ANSWER_TIMEOUT	msecs_to_jiffies(30000)
+
+#define NXP_NCI_FW_CMD_RESET		0xF0
+#define NXP_NCI_FW_CMD_GETVERSION	0xF1
+#define NXP_NCI_FW_CMD_CHECKINTEGRITY	0xE0
+#define NXP_NCI_FW_CMD_WRITE		0xC0
+#define NXP_NCI_FW_CMD_READ		0xA2
+#define NXP_NCI_FW_CMD_GETSESSIONSTATE	0xF2
+#define NXP_NCI_FW_CMD_LOG		0xA7
+#define NXP_NCI_FW_CMD_FORCE		0xD0
+#define NXP_NCI_FW_CMD_GET_DIE_ID	0xF4
+
+#define NXP_NCI_FW_CHUNK_FLAG	0x0400
+
+#define NXP_NCI_FW_RESULT_OK				0x00
+#define NXP_NCI_FW_RESULT_INVALID_ADDR			0x01
+#define NXP_NCI_FW_RESULT_GENERIC_ERROR			0x02
+#define NXP_NCI_FW_RESULT_UNKNOWN_CMD			0x0B
+#define NXP_NCI_FW_RESULT_ABORTED_CMD			0x0C
+#define NXP_NCI_FW_RESULT_PLL_ERROR			0x0D
+#define NXP_NCI_FW_RESULT_ADDR_RANGE_OFL_ERROR		0x1E
+#define NXP_NCI_FW_RESULT_BUFFER_OFL_ERROR		0x1F
+#define NXP_NCI_FW_RESULT_MEM_BSY			0x20
+#define NXP_NCI_FW_RESULT_SIGNATURE_ERROR		0x21
+#define NXP_NCI_FW_RESULT_FIRMWARE_VERSION_ERROR	0x24
+#define NXP_NCI_FW_RESULT_PROTOCOL_ERROR		0x28
+#define NXP_NCI_FW_RESULT_SFWU_DEGRADED			0x2A
+#define NXP_NCI_FW_RESULT_PH_STATUS_FIRST_CHUNK		0x2D
+#define NXP_NCI_FW_RESULT_PH_STATUS_NEXT_CHUNK		0x2E
+#define NXP_NCI_FW_RESULT_PH_STATUS_INTERNAL_ERROR_5	0xC5
+
+void nxp_nci_fw_work_complete(struct nxp_nci_info *info, int result)
+{
+	struct nxp_nci_fw_info *fw_info = &info->fw_info;
+	int r;
+
+	if (info->phy_ops->set_mode) {
+		r = info->phy_ops->set_mode(info->phy_id, NXP_NCI_MODE_COLD);
+		if (r < 0 && result == 0)
+			result = -r;
+	}
+
+	info->mode = NXP_NCI_MODE_COLD;
+
+	if (fw_info->fw) {
+		release_firmware(fw_info->fw);
+		fw_info->fw = NULL;
+	}
+
+	nfc_fw_download_done(info->ndev->nfc_dev, fw_info->name, (u32) -result);
+}
+
+/* crc_ccitt cannot be used since it is computed MSB first and not LSB first */
+static u16 nxp_nci_fw_crc(u8 const *buffer, size_t len)
+{
+	u16 crc = 0xffff;
+
+	while (len--) {
+		crc = ((crc >> 8) | (crc << 8)) ^ *buffer++;
+		crc ^= (crc & 0xff) >> 4;
+		crc ^= (crc & 0xff) << 12;
+		crc ^= (crc & 0xff) << 5;
+	}
+
+	return crc;
+}
+
+static int nxp_nci_fw_send_chunk(struct nxp_nci_info *info)
+{
+	struct nxp_nci_fw_info *fw_info = &info->fw_info;
+	u16 header, crc;
+	struct sk_buff *skb;
+	size_t chunk_len;
+	size_t remaining_len;
+	int r;
+
+	skb = nci_skb_alloc(info->ndev, info->max_payload, GFP_KERNEL);
+	if (!skb) {
+		r = -ENOMEM;
+		goto chunk_exit;
+	}
+
+	chunk_len = info->max_payload - NXP_NCI_FW_HDR_LEN - NXP_NCI_FW_CRC_LEN;
+	remaining_len = fw_info->frame_size - fw_info->written;
+
+	if (remaining_len > chunk_len) {
+		header = NXP_NCI_FW_CHUNK_FLAG;
+	} else {
+		chunk_len = remaining_len;
+		header = 0x0000;
+	}
+
+	header |= chunk_len & NXP_NCI_FW_FRAME_LEN_MASK;
+	put_unaligned_be16(header, skb_put(skb, NXP_NCI_FW_HDR_LEN));
+
+	memcpy(skb_put(skb, chunk_len), fw_info->data + fw_info->written,
+	       chunk_len);
+
+	crc = nxp_nci_fw_crc(skb->data, chunk_len + NXP_NCI_FW_HDR_LEN);
+	put_unaligned_be16(crc, skb_put(skb, NXP_NCI_FW_CRC_LEN));
+
+	r = info->phy_ops->write(info->phy_id, skb);
+	if (r >= 0)
+		r = chunk_len;
+
+	kfree_skb(skb);
+
+chunk_exit:
+	return r;
+}
+
+static int nxp_nci_fw_send(struct nxp_nci_info *info)
+{
+	struct nxp_nci_fw_info *fw_info = &info->fw_info;
+	long completion_rc;
+	int r;
+
+	reinit_completion(&fw_info->cmd_completion);
+
+	if (fw_info->written == 0) {
+		fw_info->frame_size = get_unaligned_be16(fw_info->data) &
+				      NXP_NCI_FW_FRAME_LEN_MASK;
+		fw_info->data += NXP_NCI_FW_HDR_LEN;
+		fw_info->size -= NXP_NCI_FW_HDR_LEN;
+	}
+
+	if (fw_info->frame_size > fw_info->size)
+		return -EMSGSIZE;
+
+	r = nxp_nci_fw_send_chunk(info);
+	if (r < 0)
+		return r;
+
+	fw_info->written += r;
+
+	if (*fw_info->data == NXP_NCI_FW_CMD_RESET) {
+		fw_info->cmd_result = 0;
+		if (fw_info->fw)
+			schedule_work(&fw_info->work);
+	} else {
+		completion_rc = wait_for_completion_interruptible_timeout(
+			&fw_info->cmd_completion, NXP_NCI_FW_ANSWER_TIMEOUT);
+		if (completion_rc == 0)
+			return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+void nxp_nci_fw_work(struct work_struct *work)
+{
+	struct nxp_nci_info *info;
+	struct nxp_nci_fw_info *fw_info;
+	int r;
+
+	fw_info = container_of(work, struct nxp_nci_fw_info, work);
+	info = container_of(fw_info, struct nxp_nci_info, fw_info);
+
+	mutex_lock(&info->info_lock);
+
+	r = fw_info->cmd_result;
+	if (r < 0)
+		goto exit_work;
+
+	if (fw_info->written == fw_info->frame_size) {
+		fw_info->data += fw_info->frame_size;
+		fw_info->size -= fw_info->frame_size;
+		fw_info->written = 0;
+	}
+
+	if (fw_info->size > 0)
+		r = nxp_nci_fw_send(info);
+
+exit_work:
+	if (r < 0 || fw_info->size == 0)
+		nxp_nci_fw_work_complete(info, r);
+	mutex_unlock(&info->info_lock);
+}
+
+int nxp_nci_fw_download(struct nci_dev *ndev, const char *firmware_name)
+{
+	struct nxp_nci_info *info = nci_get_drvdata(ndev);
+	struct nxp_nci_fw_info *fw_info = &info->fw_info;
+	int r;
+
+	mutex_lock(&info->info_lock);
+
+	if (!info->phy_ops->set_mode || !info->phy_ops->write) {
+		r = -ENOTSUPP;
+		goto fw_download_exit;
+	}
+
+	if (!firmware_name || firmware_name[0] == '\0') {
+		r = -EINVAL;
+		goto fw_download_exit;
+	}
+
+	strcpy(fw_info->name, firmware_name);
+
+	r = request_firmware(&fw_info->fw, firmware_name,
+			     ndev->nfc_dev->dev.parent);
+	if (r < 0)
+		goto fw_download_exit;
+
+	r = info->phy_ops->set_mode(info->phy_id, NXP_NCI_MODE_FW);
+	if (r < 0) {
+		release_firmware(fw_info->fw);
+		goto fw_download_exit;
+	}
+
+	info->mode = NXP_NCI_MODE_FW;
+
+	fw_info->data = fw_info->fw->data;
+	fw_info->size = fw_info->fw->size;
+	fw_info->written = 0;
+	fw_info->frame_size = 0;
+	fw_info->cmd_result = 0;
+
+	schedule_work(&fw_info->work);
+
+fw_download_exit:
+	mutex_unlock(&info->info_lock);
+	return r;
+}
+
+static int nxp_nci_fw_read_status(u8 stat)
+{
+	switch (stat) {
+	case NXP_NCI_FW_RESULT_OK:
+		return 0;
+	case NXP_NCI_FW_RESULT_INVALID_ADDR:
+		return -EINVAL;
+	case NXP_NCI_FW_RESULT_UNKNOWN_CMD:
+		return -EINVAL;
+	case NXP_NCI_FW_RESULT_ABORTED_CMD:
+		return -EMSGSIZE;
+	case NXP_NCI_FW_RESULT_ADDR_RANGE_OFL_ERROR:
+		return -EADDRNOTAVAIL;
+	case NXP_NCI_FW_RESULT_BUFFER_OFL_ERROR:
+		return -ENOBUFS;
+	case NXP_NCI_FW_RESULT_MEM_BSY:
+		return -ENOKEY;
+	case NXP_NCI_FW_RESULT_SIGNATURE_ERROR:
+		return -EKEYREJECTED;
+	case NXP_NCI_FW_RESULT_FIRMWARE_VERSION_ERROR:
+		return -EALREADY;
+	case NXP_NCI_FW_RESULT_PROTOCOL_ERROR:
+		return -EPROTO;
+	case NXP_NCI_FW_RESULT_SFWU_DEGRADED:
+		return -EHWPOISON;
+	case NXP_NCI_FW_RESULT_PH_STATUS_FIRST_CHUNK:
+		return 0;
+	case NXP_NCI_FW_RESULT_PH_STATUS_NEXT_CHUNK:
+		return 0;
+	case NXP_NCI_FW_RESULT_PH_STATUS_INTERNAL_ERROR_5:
+		return -EINVAL;
+	default:
+		return -EIO;
+	}
+}
+
+static u16 nxp_nci_fw_check_crc(struct sk_buff *skb)
+{
+	u16 crc, frame_crc;
+	size_t len = skb->len - NXP_NCI_FW_CRC_LEN;
+
+	crc = nxp_nci_fw_crc(skb->data, len);
+	frame_crc = get_unaligned_be16(skb->data + len);
+
+	return (crc ^ frame_crc);
+}
+
+void nxp_nci_fw_recv_frame(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct nxp_nci_info *info = nci_get_drvdata(ndev);
+	struct nxp_nci_fw_info *fw_info = &info->fw_info;
+
+	complete(&fw_info->cmd_completion);
+
+	if (skb) {
+		if (nxp_nci_fw_check_crc(skb) != 0x00)
+			fw_info->cmd_result = -EBADMSG;
+		else
+			fw_info->cmd_result = nxp_nci_fw_read_status(
+					*skb_pull(skb, NXP_NCI_FW_HDR_LEN));
+		kfree_skb(skb);
+	} else {
+		fw_info->cmd_result = -EIO;
+	}
+
+	if (fw_info->fw)
+		schedule_work(&fw_info->work);
+}
+EXPORT_SYMBOL(nxp_nci_fw_recv_frame);
diff --git a/drivers/nfc/nxp-nci/i2c.c b/drivers/nfc/nxp-nci/i2c.c
new file mode 100644
index 000000000000..17bd67dbebf0
--- /dev/null
+++ b/drivers/nfc/nxp-nci/i2c.c
@@ -0,0 +1,415 @@
+/*
+ * I2C link layer for the NXP NCI driver
+ *
+ * Copyright (C) 2014  NXP Semiconductors  All rights reserved.
+ *
+ * Authors: Clément Perrochaud <clement.perrochaud@nxp.com>
+ *
+ * Derived from PN544 device driver:
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+#include <linux/platform_data/nxp-nci.h>
+#include <linux/unaligned/access_ok.h>
+
+#include <net/nfc/nfc.h>
+
+#include "nxp-nci.h"
+
+#define NXP_NCI_I2C_DRIVER_NAME	"nxp-nci_i2c"
+
+#define NXP_NCI_I2C_MAX_PAYLOAD	32
+
+struct nxp_nci_i2c_phy {
+	struct i2c_client *i2c_dev;
+	struct nci_dev *ndev;
+
+	unsigned int gpio_en;
+	unsigned int gpio_fw;
+
+	int hard_fault; /*
+			 * < 0 if hardware error occurred (e.g. i2c err)
+			 * and prevents normal operation.
+			 */
+};
+
+static int nxp_nci_i2c_set_mode(void *phy_id,
+				    enum nxp_nci_mode mode)
+{
+	struct nxp_nci_i2c_phy *phy = (struct nxp_nci_i2c_phy *) phy_id;
+
+	gpio_set_value(phy->gpio_fw, (mode == NXP_NCI_MODE_FW) ? 1 : 0);
+	gpio_set_value(phy->gpio_en, (mode != NXP_NCI_MODE_COLD) ? 1 : 0);
+	usleep_range(10000, 15000);
+
+	if (mode == NXP_NCI_MODE_COLD)
+		phy->hard_fault = 0;
+
+	return 0;
+}
+
+static int nxp_nci_i2c_write(void *phy_id, struct sk_buff *skb)
+{
+	int r;
+	struct nxp_nci_i2c_phy *phy = phy_id;
+	struct i2c_client *client = phy->i2c_dev;
+
+	if (phy->hard_fault != 0)
+		return phy->hard_fault;
+
+	r = i2c_master_send(client, skb->data, skb->len);
+	if (r == -EREMOTEIO) {
+		/* Retry, chip was in standby */
+		usleep_range(110000, 120000);
+		r = i2c_master_send(client, skb->data, skb->len);
+	}
+
+	if (r < 0) {
+		nfc_err(&client->dev, "Error %d on I2C send\n", r);
+	} else if (r != skb->len) {
+		nfc_err(&client->dev,
+			"Invalid length sent: %u (expected %u)\n",
+			r, skb->len);
+		r = -EREMOTEIO;
+	} else {
+		/* Success but return 0 and not number of bytes */
+		r = 0;
+	}
+
+	return r;
+}
+
+static struct nxp_nci_phy_ops i2c_phy_ops = {
+	.set_mode = nxp_nci_i2c_set_mode,
+	.write = nxp_nci_i2c_write,
+};
+
+static int nxp_nci_i2c_fw_read(struct nxp_nci_i2c_phy *phy,
+			       struct sk_buff **skb)
+{
+	struct i2c_client *client = phy->i2c_dev;
+	u16 header;
+	size_t frame_len;
+	int r;
+
+	r = i2c_master_recv(client, (u8 *) &header, NXP_NCI_FW_HDR_LEN);
+	if (r < 0) {
+		goto fw_read_exit;
+	} else if (r != NXP_NCI_FW_HDR_LEN) {
+		nfc_err(&client->dev, "Incorrect header length: %u\n", r);
+		r = -EBADMSG;
+		goto fw_read_exit;
+	}
+
+	frame_len = (get_unaligned_be16(&header) & NXP_NCI_FW_FRAME_LEN_MASK) +
+		    NXP_NCI_FW_CRC_LEN;
+
+	*skb = alloc_skb(NXP_NCI_FW_HDR_LEN + frame_len, GFP_KERNEL);
+	if (*skb == NULL) {
+		r = -ENOMEM;
+		goto fw_read_exit;
+	}
+
+	memcpy(skb_put(*skb, NXP_NCI_FW_HDR_LEN), &header, NXP_NCI_FW_HDR_LEN);
+
+	r = i2c_master_recv(client, skb_put(*skb, frame_len), frame_len);
+	if (r != frame_len) {
+		nfc_err(&client->dev,
+			"Invalid frame length: %u (expected %zu)\n",
+			r, frame_len);
+		r = -EBADMSG;
+		goto fw_read_exit_free_skb;
+	}
+
+	return 0;
+
+fw_read_exit_free_skb:
+	kfree_skb(*skb);
+fw_read_exit:
+	return r;
+}
+
+static int nxp_nci_i2c_nci_read(struct nxp_nci_i2c_phy *phy,
+				struct sk_buff **skb)
+{
+	struct nci_ctrl_hdr header; /* May actually be a data header */
+	struct i2c_client *client = phy->i2c_dev;
+	int r;
+
+	r = i2c_master_recv(client, (u8 *) &header, NCI_CTRL_HDR_SIZE);
+	if (r < 0) {
+		goto nci_read_exit;
+	} else if (r != NCI_CTRL_HDR_SIZE) {
+		nfc_err(&client->dev, "Incorrect header length: %u\n", r);
+		r = -EBADMSG;
+		goto nci_read_exit;
+	}
+
+	*skb = alloc_skb(NCI_CTRL_HDR_SIZE + header.plen, GFP_KERNEL);
+	if (*skb == NULL) {
+		r = -ENOMEM;
+		goto nci_read_exit;
+	}
+
+	memcpy(skb_put(*skb, NCI_CTRL_HDR_SIZE), (void *) &header,
+	       NCI_CTRL_HDR_SIZE);
+
+	r = i2c_master_recv(client, skb_put(*skb, header.plen), header.plen);
+	if (r != header.plen) {
+		nfc_err(&client->dev,
+			"Invalid frame payload length: %u (expected %u)\n",
+			r, header.plen);
+		r = -EBADMSG;
+		goto nci_read_exit_free_skb;
+	}
+
+	return 0;
+
+nci_read_exit_free_skb:
+	kfree_skb(*skb);
+nci_read_exit:
+	return r;
+}
+
+static irqreturn_t nxp_nci_i2c_irq_thread_fn(int irq, void *phy_id)
+{
+	struct nxp_nci_i2c_phy *phy = phy_id;
+	struct i2c_client *client;
+	struct nxp_nci_info *info;
+
+	struct sk_buff *skb = NULL;
+	int r = 0;
+
+	if (!phy || !phy->ndev)
+		goto exit_irq_none;
+
+	client = phy->i2c_dev;
+
+	if (!client || irq != client->irq)
+		goto exit_irq_none;
+
+	info = nci_get_drvdata(phy->ndev);
+
+	if (!info)
+		goto exit_irq_none;
+
+	mutex_lock(&info->info_lock);
+
+	if (phy->hard_fault != 0)
+		goto exit_irq_handled;
+
+	switch (info->mode) {
+	case NXP_NCI_MODE_NCI:
+		r = nxp_nci_i2c_nci_read(phy, &skb);
+		break;
+	case NXP_NCI_MODE_FW:
+		r = nxp_nci_i2c_fw_read(phy, &skb);
+		break;
+	case NXP_NCI_MODE_COLD:
+		r = -EREMOTEIO;
+		break;
+	}
+
+	if (r == -EREMOTEIO) {
+		phy->hard_fault = r;
+		skb = NULL;
+	} else if (r < 0) {
+		nfc_err(&client->dev, "Read failed with error %d\n", r);
+		goto exit_irq_handled;
+	}
+
+	switch (info->mode) {
+	case NXP_NCI_MODE_NCI:
+		nci_recv_frame(phy->ndev, skb);
+		break;
+	case NXP_NCI_MODE_FW:
+		nxp_nci_fw_recv_frame(phy->ndev, skb);
+		break;
+	case NXP_NCI_MODE_COLD:
+		break;
+	}
+
+exit_irq_handled:
+	mutex_unlock(&info->info_lock);
+	return IRQ_HANDLED;
+exit_irq_none:
+	WARN_ON_ONCE(1);
+	return IRQ_NONE;
+}
+
+#ifdef CONFIG_OF
+
+static int nxp_nci_i2c_parse_devtree(struct i2c_client *client)
+{
+	struct nxp_nci_i2c_phy *phy = i2c_get_clientdata(client);
+	struct device_node *pp;
+	int r;
+
+	pp = client->dev.of_node;
+	if (!pp)
+		return -ENODEV;
+
+	r = of_get_named_gpio(pp, "enable-gpios", 0);
+	if (r == -EPROBE_DEFER)
+		r = of_get_named_gpio(pp, "enable-gpios", 0);
+	if (r < 0) {
+		nfc_err(&client->dev, "Failed to get EN gpio, error: %d\n", r);
+		return r;
+	}
+	phy->gpio_en = r;
+
+	r = of_get_named_gpio(pp, "firmware-gpios", 0);
+	if (r == -EPROBE_DEFER)
+		r = of_get_named_gpio(pp, "firmware-gpios", 0);
+	if (r < 0) {
+		nfc_err(&client->dev, "Failed to get FW gpio, error: %d\n", r);
+		return r;
+	}
+	phy->gpio_fw = r;
+
+	r = irq_of_parse_and_map(pp, 0);
+	if (r < 0) {
+		nfc_err(&client->dev, "Unable to get irq, error: %d\n", r);
+		return r;
+	}
+	client->irq = r;
+
+	return 0;
+}
+
+#else
+
+static int nxp_nci_i2c_parse_devtree(struct i2c_client *client)
+{
+	return -ENODEV;
+}
+
+#endif
+
+static int nxp_nci_i2c_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct nxp_nci_i2c_phy *phy;
+	struct nxp_nci_nfc_platform_data *pdata;
+	int r;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		nfc_err(&client->dev, "Need I2C_FUNC_I2C\n");
+		r = -ENODEV;
+		goto probe_exit;
+	}
+
+	phy = devm_kzalloc(&client->dev, sizeof(struct nxp_nci_i2c_phy),
+			   GFP_KERNEL);
+	if (!phy) {
+		r = -ENOMEM;
+		goto probe_exit;
+	}
+
+	phy->i2c_dev = client;
+	i2c_set_clientdata(client, phy);
+
+	pdata = client->dev.platform_data;
+
+	if (!pdata && client->dev.of_node) {
+		r = nxp_nci_i2c_parse_devtree(client);
+		if (r < 0) {
+			nfc_err(&client->dev, "Failed to get DT data\n");
+			goto probe_exit;
+		}
+	} else if (pdata) {
+		phy->gpio_en = pdata->gpio_en;
+		phy->gpio_fw = pdata->gpio_fw;
+		client->irq = pdata->irq;
+	} else {
+		nfc_err(&client->dev, "No platform data\n");
+		r = -EINVAL;
+		goto probe_exit;
+	}
+
+	r = devm_gpio_request_one(&phy->i2c_dev->dev, phy->gpio_en,
+				  GPIOF_OUT_INIT_LOW, "nxp_nci_en");
+	if (r < 0)
+		goto probe_exit;
+
+	r = devm_gpio_request_one(&phy->i2c_dev->dev, phy->gpio_fw,
+				  GPIOF_OUT_INIT_LOW, "nxp_nci_fw");
+	if (r < 0)
+		goto probe_exit;
+
+	r = nxp_nci_probe(phy, &client->dev, &i2c_phy_ops,
+			  NXP_NCI_I2C_MAX_PAYLOAD, &phy->ndev);
+	if (r < 0)
+		goto probe_exit;
+
+	r = request_threaded_irq(client->irq, NULL,
+				 nxp_nci_i2c_irq_thread_fn,
+				 IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+				 NXP_NCI_I2C_DRIVER_NAME, phy);
+	if (r < 0)
+		nfc_err(&client->dev, "Unable to register IRQ handler\n");
+
+probe_exit:
+	return r;
+}
+
+static int nxp_nci_i2c_remove(struct i2c_client *client)
+{
+	struct nxp_nci_i2c_phy *phy = i2c_get_clientdata(client);
+
+	nxp_nci_remove(phy->ndev);
+	free_irq(client->irq, phy);
+
+	return 0;
+}
+
+static struct i2c_device_id nxp_nci_i2c_id_table[] = {
+	{"nxp-nci_i2c", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, nxp_nci_i2c_id_table);
+
+static const struct of_device_id of_nxp_nci_i2c_match[] = {
+	{ .compatible = "nxp,nxp-nci-i2c", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_nxp_nci_i2c_match);
+
+static struct i2c_driver nxp_nci_i2c_driver = {
+	.driver = {
+		   .name = NXP_NCI_I2C_DRIVER_NAME,
+		   .owner  = THIS_MODULE,
+		   .of_match_table = of_match_ptr(of_nxp_nci_i2c_match),
+		  },
+	.probe = nxp_nci_i2c_probe,
+	.id_table = nxp_nci_i2c_id_table,
+	.remove = nxp_nci_i2c_remove,
+};
+
+module_i2c_driver(nxp_nci_i2c_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("I2C driver for NXP NCI NFC controllers");
+MODULE_AUTHOR("Clément Perrochaud <clement.perrochaud@nxp.com>");
diff --git a/drivers/nfc/nxp-nci/nxp-nci.h b/drivers/nfc/nxp-nci/nxp-nci.h
new file mode 100644
index 000000000000..f1fecc4e2457
--- /dev/null
+++ b/drivers/nfc/nxp-nci/nxp-nci.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2014  NXP Semiconductors  All rights reserved.
+ *
+ * Authors: Clément Perrochaud <clement.perrochaud@nxp.com>
+ *
+ * Derived from PN544 device driver:
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef __LOCAL_NXP_NCI_H_
+#define __LOCAL_NXP_NCI_H_
+
+#include <linux/completion.h>
+#include <linux/firmware.h>
+#include <linux/nfc.h>
+#include <linux/platform_data/nxp-nci.h>
+
+#include <net/nfc/nci_core.h>
+
+#define NXP_NCI_FW_HDR_LEN	2
+#define NXP_NCI_FW_CRC_LEN	2
+
+#define NXP_NCI_FW_FRAME_LEN_MASK	0x03FF
+
+enum nxp_nci_mode {
+	NXP_NCI_MODE_COLD,
+	NXP_NCI_MODE_NCI,
+	NXP_NCI_MODE_FW
+};
+
+struct nxp_nci_phy_ops {
+	int (*set_mode)(void *id, enum nxp_nci_mode mode);
+	int (*write)(void *id, struct sk_buff *skb);
+};
+
+struct nxp_nci_fw_info {
+	char name[NFC_FIRMWARE_NAME_MAXSIZE + 1];
+	const struct firmware *fw;
+
+	size_t size;
+	size_t written;
+
+	const u8 *data;
+	size_t frame_size;
+
+	struct work_struct work;
+	struct completion cmd_completion;
+
+	int cmd_result;
+};
+
+struct nxp_nci_info {
+	struct nci_dev *ndev;
+	void *phy_id;
+	struct device *pdev;
+
+	enum nxp_nci_mode mode;
+
+	struct nxp_nci_phy_ops *phy_ops;
+	unsigned int max_payload;
+
+	struct mutex info_lock;
+
+	struct nxp_nci_fw_info fw_info;
+};
+
+int nxp_nci_fw_download(struct nci_dev *ndev, const char *firmware_name);
+void nxp_nci_fw_work(struct work_struct *work);
+void nxp_nci_fw_recv_frame(struct nci_dev *ndev, struct sk_buff *skb);
+void nxp_nci_fw_work_complete(struct nxp_nci_info *info, int result);
+
+int nxp_nci_probe(void *phy_id, struct device *pdev,
+		  struct nxp_nci_phy_ops *phy_ops, unsigned int max_payload,
+		  struct nci_dev **ndev);
+void nxp_nci_remove(struct nci_dev *ndev);
+
+#endif /* __LOCAL_NXP_NCI_H_ */
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index d46a700a9637..a03e4eb5fe29 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -1820,7 +1820,7 @@ static int pn533_rf_complete(struct pn533 *dev, void *arg,
 	if (IS_ERR(resp)) {
 		rc = PTR_ERR(resp);
 
-		nfc_err(&dev->interface->dev, "RF setting error %d", rc);
+		nfc_err(&dev->interface->dev, "RF setting error %d\n", rc);
 
 		return rc;
 	}
@@ -2554,8 +2554,10 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
 	}
 
 	skb = pn533_build_response(dev);
-	if (!skb)
+	if (!skb) {
+		rc = -ENOMEM;
 		goto error;
+	}
 
 	arg->cb(arg->cb_context, skb, 0);
 	kfree(arg);
diff --git a/drivers/nfc/pn544/i2c.c b/drivers/nfc/pn544/i2c.c
index cdde745b96bd..6fd986f5ac3e 100644
--- a/drivers/nfc/pn544/i2c.c
+++ b/drivers/nfc/pn544/i2c.c
@@ -953,7 +953,7 @@ static int pn544_hci_i2c_acpi_request_resources(struct i2c_client *client)
 	}
 
 	nfc_info(dev, "GPIO resource, no:%d irq:%d\n",
-			desc_to_gpio(gpiod_irq), ret);
+		 desc_to_gpio(gpiod_irq), ret);
 	client->irq = ret;
 
 	return 0;
@@ -1062,11 +1062,8 @@ static int pn544_hci_i2c_probe(struct i2c_client *client,
 
 	phy = devm_kzalloc(&client->dev, sizeof(struct pn544_i2c_phy),
 			   GFP_KERNEL);
-	if (!phy) {
-		nfc_err(&client->dev,
-			"Cannot allocate memory for pn544 i2c phy.\n");
+	if (!phy)
 		return -ENOMEM;
-	}
 
 	INIT_WORK(&phy->fw_work, pn544_hci_i2c_fw_work);
 	phy->fw_work_state = FW_WORK_STATE_IDLE;
diff --git a/drivers/nfc/port100.c b/drivers/nfc/port100.c
index 4ac4d31f6c59..87d509996704 100644
--- a/drivers/nfc/port100.c
+++ b/drivers/nfc/port100.c
@@ -604,11 +604,11 @@ static void port100_recv_response(struct urb *urb)
 	case -ECONNRESET:
 	case -ENOENT:
 		nfc_err(&dev->interface->dev,
-			"The urb has been canceled (status %d)", urb->status);
+			"The urb has been canceled (status %d)\n", urb->status);
 		goto sched_wq;
 	case -ESHUTDOWN:
 	default:
-		nfc_err(&dev->interface->dev, "Urb failure (status %d)",
+		nfc_err(&dev->interface->dev, "Urb failure (status %d)\n",
 			urb->status);
 		goto sched_wq;
 	}
@@ -616,7 +616,7 @@ static void port100_recv_response(struct urb *urb)
 	in_frame = dev->in_urb->transfer_buffer;
 
 	if (!port100_rx_frame_is_valid(in_frame)) {
-		nfc_err(&dev->interface->dev, "Received an invalid frame");
+		nfc_err(&dev->interface->dev, "Received an invalid frame\n");
 		cmd->status = -EIO;
 		goto sched_wq;
 	}
@@ -626,7 +626,7 @@ static void port100_recv_response(struct urb *urb)
 
 	if (!port100_rx_frame_is_cmd_response(dev, in_frame)) {
 		nfc_err(&dev->interface->dev,
-			"It's not the response to the last command");
+			"It's not the response to the last command\n");
 		cmd->status = -EIO;
 		goto sched_wq;
 	}
@@ -657,11 +657,11 @@ static void port100_recv_ack(struct urb *urb)
 	case -ECONNRESET:
 	case -ENOENT:
 		nfc_err(&dev->interface->dev,
-			"The urb has been stopped (status %d)", urb->status);
+			"The urb has been stopped (status %d)\n", urb->status);
 		goto sched_wq;
 	case -ESHUTDOWN:
 	default:
-		nfc_err(&dev->interface->dev, "Urb failure (status %d)",
+		nfc_err(&dev->interface->dev, "Urb failure (status %d)\n",
 			urb->status);
 		goto sched_wq;
 	}
@@ -669,7 +669,7 @@ static void port100_recv_ack(struct urb *urb)
 	in_frame = dev->in_urb->transfer_buffer;
 
 	if (!port100_rx_frame_is_ack(in_frame)) {
-		nfc_err(&dev->interface->dev, "Received an invalid ack");
+		nfc_err(&dev->interface->dev, "Received an invalid ack\n");
 		cmd->status = -EIO;
 		goto sched_wq;
 	}
@@ -677,7 +677,7 @@ static void port100_recv_ack(struct urb *urb)
 	rc = port100_submit_urb_for_response(dev, GFP_ATOMIC);
 	if (rc) {
 		nfc_err(&dev->interface->dev,
-			"usb_submit_urb failed with result %d", rc);
+			"usb_submit_urb failed with result %d\n", rc);
 		cmd->status = rc;
 		goto sched_wq;
 	}
@@ -873,11 +873,11 @@ static void port100_send_complete(struct urb *urb)
 	case -ECONNRESET:
 	case -ENOENT:
 		nfc_err(&dev->interface->dev,
-			"The urb has been stopped (status %d)", urb->status);
+			"The urb has been stopped (status %d)\n", urb->status);
 		break;
 	case -ESHUTDOWN:
 	default:
-		nfc_err(&dev->interface->dev, "Urb failure (status %d)",
+		nfc_err(&dev->interface->dev, "Urb failure (status %d)\n",
 			urb->status);
 	}
 }
@@ -1094,7 +1094,7 @@ static void port100_in_comm_rf_complete(struct port100 *dev, void *arg,
 
 	if (resp->len < 4) {
 		nfc_err(&dev->interface->dev,
-			"Invalid packet length received.\n");
+			"Invalid packet length received\n");
 		rc = -EIO;
 		goto error;
 	}
@@ -1250,7 +1250,7 @@ static bool port100_tg_target_activated(struct port100 *dev, u8 tgt_activated)
 		       PORT100_MDAA_TGT_WAS_ACTIVATED_MASK;
 		break;
 	default:
-		nfc_err(&dev->interface->dev, "Unknonwn command type.\n");
+		nfc_err(&dev->interface->dev, "Unknown command type\n");
 		return false;
 	}
 
@@ -1481,7 +1481,7 @@ static int port100_probe(struct usb_interface *interface,
 	cmd_type_mask = port100_get_command_type_mask(dev);
 	if (!cmd_type_mask) {
 		nfc_err(&interface->dev,
-			"Could not get supported command types.\n");
+			"Could not get supported command types\n");
 		rc = -ENODEV;
 		goto error;
 	}
@@ -1494,7 +1494,7 @@ static int port100_probe(struct usb_interface *interface,
 	rc = port100_set_command_type(dev, dev->cmd_type);
 	if (rc) {
 		nfc_err(&interface->dev,
-			"The device does not support command type %u.\n",
+			"The device does not support command type %u\n",
 			dev->cmd_type);
 		goto error;
 	}
@@ -1502,7 +1502,7 @@ static int port100_probe(struct usb_interface *interface,
 	fw_version = port100_get_firmware_version(dev);
 	if (!fw_version)
 		nfc_err(&interface->dev,
-			"Could not get device firmware version.\n");
+			"Could not get device firmware version\n");
 
 	nfc_info(&interface->dev,
 		 "Sony NFC Port-100 Series attached (firmware v%x.%02x)\n",
@@ -1515,7 +1515,7 @@ static int port100_probe(struct usb_interface *interface,
 							   dev->skb_tailroom);
 	if (!dev->nfc_digital_dev) {
 		nfc_err(&interface->dev,
-			"Could not allocate nfc_digital_dev.\n");
+			"Could not allocate nfc_digital_dev\n");
 		rc = -ENOMEM;
 		goto error;
 	}
@@ -1526,7 +1526,7 @@ static int port100_probe(struct usb_interface *interface,
 	rc = nfc_digital_register_device(dev->nfc_digital_dev);
 	if (rc) {
 		nfc_err(&interface->dev,
-			"Could not register digital device.\n");
+			"Could not register digital device\n");
 		goto free_nfc_dev;
 	}
 
@@ -1562,7 +1562,7 @@ static void port100_disconnect(struct usb_interface *interface)
 
 	kfree(dev->cmd);
 
-	nfc_info(&interface->dev, "Sony Port-100 NFC device disconnected");
+	nfc_info(&interface->dev, "Sony Port-100 NFC device disconnected\n");
 }
 
 static struct usb_driver port100_driver = {
diff --git a/drivers/nfc/st21nfca/st21nfca.c b/drivers/nfc/st21nfca/st21nfca.c
index 24d3d240d5f4..d251f7229c4e 100644
--- a/drivers/nfc/st21nfca/st21nfca.c
+++ b/drivers/nfc/st21nfca/st21nfca.c
@@ -572,7 +572,7 @@ exit:
 	return r;
 }
 
-static int st21nfca_get_iso14443_3_uid(struct nfc_hci_dev *hdev, u8 *gate,
+static int st21nfca_get_iso14443_3_uid(struct nfc_hci_dev *hdev, u8 *uid,
 				       int *len)
 {
 	int r;
@@ -588,7 +588,7 @@ static int st21nfca_get_iso14443_3_uid(struct nfc_hci_dev *hdev, u8 *gate,
 		goto exit;
 	}
 
-	gate = uid_skb->data;
+	memcpy(uid, uid_skb->data, uid_skb->len);
 	*len = uid_skb->len;
 exit:
 	kfree_skb(uid_skb);
diff --git a/drivers/nfc/st21nfca/st21nfca_se.c b/drivers/nfc/st21nfca/st21nfca_se.c
index bd13cac9c66a..3197e9bb66f7 100644
--- a/drivers/nfc/st21nfca/st21nfca_se.c
+++ b/drivers/nfc/st21nfca/st21nfca_se.c
@@ -310,6 +310,13 @@ int st21nfca_connectivity_event_received(struct nfc_hci_dev *hdev, u8 host,
 	case ST21NFCA_EVT_CONNECTIVITY:
 		break;
 	case ST21NFCA_EVT_TRANSACTION:
+		/*
+		 * According to specification etsi 102 622
+		 * 11.2.2.4 EVT_TRANSACTION Table 52
+		 * Description	Tag	Length
+		 * AID		81	5 to 16
+		 * PARAMETERS	82	0 to 255
+		 */
 		if (skb->len < NFC_MIN_AID_LENGTH + 2 &&
 		    skb->data[0] != NFC_EVT_TRANSACTION_AID_TAG)
 			return -EPROTO;
@@ -318,8 +325,10 @@ int st21nfca_connectivity_event_received(struct nfc_hci_dev *hdev, u8 host,
 						   skb->len - 2, GFP_KERNEL);
 
 		transaction->aid_len = skb->data[1];
-		memcpy(transaction->aid, &skb->data[2], skb->data[1]);
+		memcpy(transaction->aid, &skb->data[2],
+		       transaction->aid_len);
 
+		/* Check next byte is PARAMETERS tag (82) */
 		if (skb->data[transaction->aid_len + 2] !=
 		    NFC_EVT_TRANSACTION_PARAMS_TAG)
 			return -EPROTO;
diff --git a/drivers/nfc/st21nfcb/i2c.c b/drivers/nfc/st21nfcb/i2c.c
index eb886932d972..76a4cad41cec 100644
--- a/drivers/nfc/st21nfcb/i2c.c
+++ b/drivers/nfc/st21nfcb/i2c.c
@@ -109,7 +109,7 @@ static int st21nfcb_nci_i2c_write(void *phy_id, struct sk_buff *skb)
 		return phy->ndlc->hard_fault;
 
 	r = i2c_master_send(client, skb->data, skb->len);
-	if (r == -EREMOTEIO) {  /* Retry, chip was in standby */
+	if (r < 0) {  /* Retry, chip was in standby */
 		usleep_range(1000, 4000);
 		r = i2c_master_send(client, skb->data, skb->len);
 	}
@@ -148,7 +148,7 @@ static int st21nfcb_nci_i2c_read(struct st21nfcb_i2c_phy *phy,
 	struct i2c_client *client = phy->i2c_dev;
 
 	r = i2c_master_recv(client, buf, ST21NFCB_NCI_I2C_MIN_SIZE);
-	if (r == -EREMOTEIO) {  /* Retry, chip was in standby */
+	if (r < 0) {  /* Retry, chip was in standby */
 		usleep_range(1000, 4000);
 		r = i2c_master_recv(client, buf, ST21NFCB_NCI_I2C_MIN_SIZE);
 	}
@@ -313,11 +313,8 @@ static int st21nfcb_nci_i2c_probe(struct i2c_client *client,
 
 	phy = devm_kzalloc(&client->dev, sizeof(struct st21nfcb_i2c_phy),
 			   GFP_KERNEL);
-	if (!phy) {
-		nfc_err(&client->dev,
-			"Cannot allocate memory for st21nfcb i2c phy.\n");
+	if (!phy)
 		return -ENOMEM;
-	}
 
 	phy->i2c_dev = client;
 
diff --git a/drivers/nfc/st21nfcb/ndlc.c b/drivers/nfc/st21nfcb/ndlc.c
index 5fbf59d2138c..6014b5859465 100644
--- a/drivers/nfc/st21nfcb/ndlc.c
+++ b/drivers/nfc/st21nfcb/ndlc.c
@@ -256,10 +256,9 @@ int ndlc_probe(void *phy_id, struct nfc_phy_ops *phy_ops, struct device *dev,
 	struct llt_ndlc *ndlc;
 
 	ndlc = devm_kzalloc(dev, sizeof(struct llt_ndlc), GFP_KERNEL);
-	if (!ndlc) {
-		nfc_err(dev, "Cannot allocate memory for ndlc.\n");
+	if (!ndlc)
 		return -ENOMEM;
-	}
+
 	ndlc->ops = phy_ops;
 	ndlc->phy_id = phy_id;
 	ndlc->dev = dev;
diff --git a/drivers/nfc/st21nfcb/st21nfcb_se.c b/drivers/nfc/st21nfcb/st21nfcb_se.c
index 7c82e9d87a65..24862a525fb5 100644
--- a/drivers/nfc/st21nfcb/st21nfcb_se.c
+++ b/drivers/nfc/st21nfcb/st21nfcb_se.c
@@ -321,6 +321,12 @@ static int st21nfcb_hci_connectivity_event_received(struct nci_dev *ndev,
 
 	break;
 	case ST21NFCB_EVT_TRANSACTION:
+		/* According to specification etsi 102 622
+		 * 11.2.2.4 EVT_TRANSACTION Table 52
+		 * Description  Tag     Length
+		 * AID          81      5 to 16
+		 * PARAMETERS   82      0 to 255
+		 */
 		if (skb->len < NFC_MIN_AID_LENGTH + 2 &&
 		    skb->data[0] != NFC_EVT_TRANSACTION_AID_TAG)
 			return -EPROTO;
@@ -329,8 +335,9 @@ static int st21nfcb_hci_connectivity_event_received(struct nci_dev *ndev,
 					    skb->len - 2, GFP_KERNEL);
 
 		transaction->aid_len = skb->data[1];
-		memcpy(transaction->aid, &skb->data[2], skb->data[1]);
+		memcpy(transaction->aid, &skb->data[2], transaction->aid_len);
 
+		/* Check next byte is PARAMETERS tag (82) */
 		if (skb->data[transaction->aid_len + 2] !=
 		    NFC_EVT_TRANSACTION_PARAMS_TAG)
 			return -EPROTO;
@@ -340,6 +347,7 @@ static int st21nfcb_hci_connectivity_event_received(struct nci_dev *ndev,
 		       transaction->aid_len + 4, transaction->params_len);
 
 		r = nfc_se_transaction(ndev->nfc_dev, host, transaction);
+		break;
 	default:
 		return 1;
 	}
@@ -542,14 +550,12 @@ static int st21nfcb_hci_network_init(struct nci_dev *ndev)
 
 	r = nci_hci_dev_session_init(ndev);
 	if (r != NCI_HCI_ANY_OK)
-		goto exit;
+		goto free_dest_params;
 
 	r = nci_nfcee_mode_set(ndev, ndev->hci_dev->conn_info->id,
 			       NCI_NFCEE_ENABLE);
 	if (r != NCI_STATUS_OK)
-		goto exit;
-
-	return 0;
+		goto free_dest_params;
 
 free_dest_params:
 	kfree(dest_params);
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 adb8764861c0..4cc06c702c41 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -715,13 +715,8 @@ static struct device_node *__of_find_node_by_path(struct device_node *parent,
 {
 	struct device_node *child;
 	int len;
-	const char *end;
 
-	end = strchr(path, ':');
-	if (!end)
-		end = strchrnul(path, '/');
-
-	len = end - path;
+	len = strcspn(path, "/:");
 	if (!len)
 		return NULL;
 
@@ -1893,10 +1888,8 @@ void of_alias_scan(void * (*dt_alloc)(u64 size, u64 align))
 			name = of_get_property(of_chosen, "linux,stdout-path", NULL);
 		if (IS_ENABLED(CONFIG_PPC) && !name)
 			name = of_get_property(of_aliases, "stdout", NULL);
-		if (name) {
+		if (name)
 			of_stdout = of_find_node_opts_by_path(name, &of_stdout_options);
-			add_preferred_console("stdout-path", 0, NULL);
-		}
 	}
 
 	if (!of_aliases)
@@ -1967,6 +1960,32 @@ int of_alias_get_id(struct device_node *np, const char *stem)
 }
 EXPORT_SYMBOL_GPL(of_alias_get_id);
 
+/**
+ * of_alias_get_highest_id - Get highest alias id for the given stem
+ * @stem:	Alias stem to be examined
+ *
+ * The function travels the lookup table to get the highest alias id for the
+ * given alias stem.  It returns the alias id if found.
+ */
+int of_alias_get_highest_id(const char *stem)
+{
+	struct alias_prop *app;
+	int id = -ENODEV;
+
+	mutex_lock(&of_mutex);
+	list_for_each_entry(app, &aliases_lookup, link) {
+		if (strcmp(app->stem, stem) != 0)
+			continue;
+
+		if (app->id > id)
+			id = app->id;
+	}
+	mutex_unlock(&of_mutex);
+
+	return id;
+}
+EXPORT_SYMBOL_GPL(of_alias_get_highest_id);
+
 const __be32 *of_prop_next_u32(struct property *prop, const __be32 *cur,
 			       u32 *pu)
 {
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_mdio.c b/drivers/of/of_mdio.c
index 1bd43053b8c7..0c064485d1c2 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -88,7 +88,7 @@ static int of_mdiobus_register_phy(struct mii_bus *mdio, struct device_node *chi
 	return 0;
 }
 
-static int of_mdio_parse_addr(struct device *dev, const struct device_node *np)
+int of_mdio_parse_addr(struct device *dev, const struct device_node *np)
 {
 	u32 addr;
 	int ret;
@@ -108,6 +108,7 @@ static int of_mdio_parse_addr(struct device *dev, const struct device_node *np)
 
 	return addr;
 }
+EXPORT_SYMBOL(of_mdio_parse_addr);
 
 /**
  * of_mdiobus_register - Register mii_bus and create PHYs from the device tree
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/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 aba8946cac46..52c45c7df07f 100644
--- a/drivers/of/unittest.c
+++ b/drivers/of/unittest.c
@@ -97,6 +97,11 @@ static void __init of_selftest_find_node_by_name(void)
 		 "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);
diff --git a/drivers/parisc/ccio-dma.c b/drivers/parisc/ccio-dma.c
index 8b490d77054f..6bc16809c504 100644
--- a/drivers/parisc/ccio-dma.c
+++ b/drivers/parisc/ccio-dma.c
@@ -1021,7 +1021,6 @@ static struct hppa_dma_ops ccio_ops = {
 #ifdef CONFIG_PROC_FS
 static int ccio_proc_info(struct seq_file *m, void *p)
 {
-	int len = 0;
 	struct ioc *ioc = ioc_list;
 
 	while (ioc != NULL) {
@@ -1031,22 +1030,22 @@ static int ccio_proc_info(struct seq_file *m, void *p)
 		int j;
 #endif
 
-		len += seq_printf(m, "%s\n", ioc->name);
+		seq_printf(m, "%s\n", ioc->name);
 		
-		len += seq_printf(m, "Cujo 2.0 bug    : %s\n",
-				  (ioc->cujo20_bug ? "yes" : "no"));
+		seq_printf(m, "Cujo 2.0 bug    : %s\n",
+			   (ioc->cujo20_bug ? "yes" : "no"));
 		
-		len += seq_printf(m, "IO PDIR size    : %d bytes (%d entries)\n",
-			       total_pages * 8, total_pages);
+		seq_printf(m, "IO PDIR size    : %d bytes (%d entries)\n",
+			   total_pages * 8, total_pages);
 
 #ifdef CCIO_COLLECT_STATS
-		len += seq_printf(m, "IO PDIR entries : %ld free  %ld used (%d%%)\n",
-				  total_pages - ioc->used_pages, ioc->used_pages,
-				  (int)(ioc->used_pages * 100 / total_pages));
+		seq_printf(m, "IO PDIR entries : %ld free  %ld used (%d%%)\n",
+			   total_pages - ioc->used_pages, ioc->used_pages,
+			   (int)(ioc->used_pages * 100 / total_pages));
 #endif
 
-		len += seq_printf(m, "Resource bitmap : %d bytes (%d pages)\n", 
-				  ioc->res_size, total_pages);
+		seq_printf(m, "Resource bitmap : %d bytes (%d pages)\n",
+			   ioc->res_size, total_pages);
 
 #ifdef CCIO_COLLECT_STATS
 		min = max = ioc->avg_search[0];
@@ -1058,26 +1057,26 @@ static int ccio_proc_info(struct seq_file *m, void *p)
 				min = ioc->avg_search[j];
 		}
 		avg /= CCIO_SEARCH_SAMPLE;
-		len += seq_printf(m, "  Bitmap search : %ld/%ld/%ld (min/avg/max CPU Cycles)\n",
-				  min, avg, max);
+		seq_printf(m, "  Bitmap search : %ld/%ld/%ld (min/avg/max CPU Cycles)\n",
+			   min, avg, max);
 
-		len += seq_printf(m, "pci_map_single(): %8ld calls  %8ld pages (avg %d/1000)\n",
-				  ioc->msingle_calls, ioc->msingle_pages,
-				  (int)((ioc->msingle_pages * 1000)/ioc->msingle_calls));
+		seq_printf(m, "pci_map_single(): %8ld calls  %8ld pages (avg %d/1000)\n",
+			   ioc->msingle_calls, ioc->msingle_pages,
+			   (int)((ioc->msingle_pages * 1000)/ioc->msingle_calls));
 
 		/* KLUGE - unmap_sg calls unmap_single for each mapped page */
 		min = ioc->usingle_calls - ioc->usg_calls;
 		max = ioc->usingle_pages - ioc->usg_pages;
-		len += seq_printf(m, "pci_unmap_single: %8ld calls  %8ld pages (avg %d/1000)\n",
-				  min, max, (int)((max * 1000)/min));
+		seq_printf(m, "pci_unmap_single: %8ld calls  %8ld pages (avg %d/1000)\n",
+			   min, max, (int)((max * 1000)/min));
  
-		len += seq_printf(m, "pci_map_sg()    : %8ld calls  %8ld pages (avg %d/1000)\n",
-				  ioc->msg_calls, ioc->msg_pages,
-				  (int)((ioc->msg_pages * 1000)/ioc->msg_calls));
+		seq_printf(m, "pci_map_sg()    : %8ld calls  %8ld pages (avg %d/1000)\n",
+			   ioc->msg_calls, ioc->msg_pages,
+			   (int)((ioc->msg_pages * 1000)/ioc->msg_calls));
 
-		len += seq_printf(m, "pci_unmap_sg()  : %8ld calls  %8ld pages (avg %d/1000)\n\n\n",
-				  ioc->usg_calls, ioc->usg_pages,
-				  (int)((ioc->usg_pages * 1000)/ioc->usg_calls));
+		seq_printf(m, "pci_unmap_sg()  : %8ld calls  %8ld pages (avg %d/1000)\n\n\n",
+			   ioc->usg_calls, ioc->usg_pages,
+			   (int)((ioc->usg_pages * 1000)/ioc->usg_calls));
 #endif	/* CCIO_COLLECT_STATS */
 
 		ioc = ioc->next;
@@ -1101,7 +1100,6 @@ static const struct file_operations ccio_proc_info_fops = {
 
 static int ccio_proc_bitmap_info(struct seq_file *m, void *p)
 {
-	int len = 0;
 	struct ioc *ioc = ioc_list;
 
 	while (ioc != NULL) {
@@ -1110,11 +1108,11 @@ static int ccio_proc_bitmap_info(struct seq_file *m, void *p)
 
 		for (j = 0; j < (ioc->res_size / sizeof(u32)); j++) {
 			if ((j & 7) == 0)
-				len += seq_puts(m, "\n   ");
-			len += seq_printf(m, "%08x", *res_ptr);
+				seq_puts(m, "\n   ");
+			seq_printf(m, "%08x", *res_ptr);
 			res_ptr++;
 		}
-		len += seq_puts(m, "\n\n");
+		seq_puts(m, "\n\n");
 		ioc = ioc->next;
 		break; /* XXX - remove me */
 	}
diff --git a/drivers/parisc/eisa_enumerator.c b/drivers/parisc/eisa_enumerator.c
index caa153133754..a656d9e83343 100644
--- a/drivers/parisc/eisa_enumerator.c
+++ b/drivers/parisc/eisa_enumerator.c
@@ -357,7 +357,7 @@ static int parse_slot_config(int slot,
 		}
 		if (flags & HPEE_FUNCTION_INFO_CFG_FREE_FORM) {
 			/* I have no idea how to handle this */
-			printk("function %d have free-form confgiuration, skipping ",
+			printk("function %d have free-form configuration, skipping ",
 				num_func);
 			pos = p0 + function_len;
 			continue;
diff --git a/drivers/parisc/sba_iommu.c b/drivers/parisc/sba_iommu.c
index 1ff1b67e8b27..f07471264689 100644
--- a/drivers/parisc/sba_iommu.c
+++ b/drivers/parisc/sba_iommu.c
@@ -1774,37 +1774,35 @@ static int sba_proc_info(struct seq_file *m, void *p)
 #ifdef SBA_COLLECT_STATS
 	unsigned long avg = 0, min, max;
 #endif
-	int i, len = 0;
-
-	len += seq_printf(m, "%s rev %d.%d\n",
-		sba_dev->name,
-		(sba_dev->hw_rev & 0x7) + 1,
-		(sba_dev->hw_rev & 0x18) >> 3
-		);
-	len += seq_printf(m, "IO PDIR size    : %d bytes (%d entries)\n",
-		(int) ((ioc->res_size << 3) * sizeof(u64)), /* 8 bits/byte */
-		total_pages);
-
-	len += seq_printf(m, "Resource bitmap : %d bytes (%d pages)\n", 
-		ioc->res_size, ioc->res_size << 3);   /* 8 bits per byte */
-
-	len += seq_printf(m, "LMMIO_BASE/MASK/ROUTE %08x %08x %08x\n",
-		READ_REG32(sba_dev->sba_hpa + LMMIO_DIST_BASE),
-		READ_REG32(sba_dev->sba_hpa + LMMIO_DIST_MASK),
-		READ_REG32(sba_dev->sba_hpa + LMMIO_DIST_ROUTE)
-		);
+	int i;
+
+	seq_printf(m, "%s rev %d.%d\n",
+		   sba_dev->name,
+		   (sba_dev->hw_rev & 0x7) + 1,
+		   (sba_dev->hw_rev & 0x18) >> 3);
+	seq_printf(m, "IO PDIR size    : %d bytes (%d entries)\n",
+		   (int)((ioc->res_size << 3) * sizeof(u64)), /* 8 bits/byte */
+		   total_pages);
+
+	seq_printf(m, "Resource bitmap : %d bytes (%d pages)\n",
+		   ioc->res_size, ioc->res_size << 3);   /* 8 bits per byte */
+
+	seq_printf(m, "LMMIO_BASE/MASK/ROUTE %08x %08x %08x\n",
+		   READ_REG32(sba_dev->sba_hpa + LMMIO_DIST_BASE),
+		   READ_REG32(sba_dev->sba_hpa + LMMIO_DIST_MASK),
+		   READ_REG32(sba_dev->sba_hpa + LMMIO_DIST_ROUTE));
 
 	for (i=0; i<4; i++)
-		len += seq_printf(m, "DIR%d_BASE/MASK/ROUTE %08x %08x %08x\n", i,
-			READ_REG32(sba_dev->sba_hpa + LMMIO_DIRECT0_BASE  + i*0x18),
-			READ_REG32(sba_dev->sba_hpa + LMMIO_DIRECT0_MASK  + i*0x18),
-			READ_REG32(sba_dev->sba_hpa + LMMIO_DIRECT0_ROUTE + i*0x18)
-		);
+		seq_printf(m, "DIR%d_BASE/MASK/ROUTE %08x %08x %08x\n",
+			   i,
+			   READ_REG32(sba_dev->sba_hpa + LMMIO_DIRECT0_BASE  + i*0x18),
+			   READ_REG32(sba_dev->sba_hpa + LMMIO_DIRECT0_MASK  + i*0x18),
+			   READ_REG32(sba_dev->sba_hpa + LMMIO_DIRECT0_ROUTE + i*0x18));
 
 #ifdef SBA_COLLECT_STATS
-	len += seq_printf(m, "IO PDIR entries : %ld free  %ld used (%d%%)\n",
-		total_pages - ioc->used_pages, ioc->used_pages,
-		(int) (ioc->used_pages * 100 / total_pages));
+	seq_printf(m, "IO PDIR entries : %ld free  %ld used (%d%%)\n",
+		   total_pages - ioc->used_pages, ioc->used_pages,
+		   (int)(ioc->used_pages * 100 / total_pages));
 
 	min = max = ioc->avg_search[0];
 	for (i = 0; i < SBA_SEARCH_SAMPLE; i++) {
@@ -1813,26 +1811,26 @@ static int sba_proc_info(struct seq_file *m, void *p)
 		if (ioc->avg_search[i] < min) min = ioc->avg_search[i];
 	}
 	avg /= SBA_SEARCH_SAMPLE;
-	len += seq_printf(m, "  Bitmap search : %ld/%ld/%ld (min/avg/max CPU Cycles)\n",
-		min, avg, max);
+	seq_printf(m, "  Bitmap search : %ld/%ld/%ld (min/avg/max CPU Cycles)\n",
+		   min, avg, max);
 
-	len += seq_printf(m, "pci_map_single(): %12ld calls  %12ld pages (avg %d/1000)\n",
-		ioc->msingle_calls, ioc->msingle_pages,
-		(int) ((ioc->msingle_pages * 1000)/ioc->msingle_calls));
+	seq_printf(m, "pci_map_single(): %12ld calls  %12ld pages (avg %d/1000)\n",
+		   ioc->msingle_calls, ioc->msingle_pages,
+		   (int)((ioc->msingle_pages * 1000)/ioc->msingle_calls));
 
 	/* KLUGE - unmap_sg calls unmap_single for each mapped page */
 	min = ioc->usingle_calls;
 	max = ioc->usingle_pages - ioc->usg_pages;
-	len += seq_printf(m, "pci_unmap_single: %12ld calls  %12ld pages (avg %d/1000)\n",
-		min, max, (int) ((max * 1000)/min));
+	seq_printf(m, "pci_unmap_single: %12ld calls  %12ld pages (avg %d/1000)\n",
+		   min, max, (int)((max * 1000)/min));
 
-	len += seq_printf(m, "pci_map_sg()    : %12ld calls  %12ld pages (avg %d/1000)\n",
-		ioc->msg_calls, ioc->msg_pages, 
-		(int) ((ioc->msg_pages * 1000)/ioc->msg_calls));
+	seq_printf(m, "pci_map_sg()    : %12ld calls  %12ld pages (avg %d/1000)\n",
+		   ioc->msg_calls, ioc->msg_pages,
+		   (int)((ioc->msg_pages * 1000)/ioc->msg_calls));
 
-	len += seq_printf(m, "pci_unmap_sg()  : %12ld calls  %12ld pages (avg %d/1000)\n",
-		ioc->usg_calls, ioc->usg_pages,
-		(int) ((ioc->usg_pages * 1000)/ioc->usg_calls));
+	seq_printf(m, "pci_unmap_sg()  : %12ld calls  %12ld pages (avg %d/1000)\n",
+		   ioc->usg_calls, ioc->usg_pages,
+		   (int)((ioc->usg_pages * 1000)/ioc->usg_calls));
 #endif
 
 	return 0;
@@ -1858,14 +1856,14 @@ sba_proc_bitmap_info(struct seq_file *m, void *p)
 	struct sba_device *sba_dev = sba_list;
 	struct ioc *ioc = &sba_dev->ioc[0];	/* FIXME: Multi-IOC support! */
 	unsigned int *res_ptr = (unsigned int *)ioc->res_map;
-	int i, len = 0;
+	int i;
 
 	for (i = 0; i < (ioc->res_size/sizeof(unsigned int)); ++i, ++res_ptr) {
 		if ((i & 7) == 0)
-			len += seq_printf(m, "\n   ");
-		len += seq_printf(m, " %08x", *res_ptr);
+			seq_puts(m, "\n   ");
+		seq_printf(m, " %08x", *res_ptr);
 	}
-	len += seq_printf(m, "\n");
+	seq_putc(m, '\n');
 
 	return 0;
 }
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 464bf492ee2a..0863d9cc25f8 100644
--- a/drivers/pci/host/pci-versatile.c
+++ b/drivers/pci/host/pci-versatile.c
@@ -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/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.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 bae593c04541..9bd762c237ab 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -323,4 +323,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 6603d401bb7c..4fd0cacf7ca0 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -1813,3 +1813,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/Kconfig b/drivers/pinctrl/Kconfig
index ee9f44ad7f02..c6f299ba25cb 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -67,23 +67,20 @@ config PINCTRL_AT91
 	help
 	  Say Y here to enable the at91 pinctrl driver
 
-config PINCTRL_BCM2835
-	bool
-	select PINMUX
-	select PINCONF
-
-config PINCTRL_BCM281XX
-	bool "Broadcom BCM281xx pinctrl driver"
-	depends on OF && (ARCH_BCM_MOBILE || COMPILE_TEST)
-	select PINMUX
+config PINCTRL_AMD
+	bool "AMD GPIO pin control"
+	depends on GPIOLIB
+	select GPIOLIB_IRQCHIP
 	select PINCONF
 	select GENERIC_PINCONF
-	select REGMAP_MMIO
 	help
-	  Say Y here to support Broadcom BCM281xx pinctrl driver, which is used
-	  for the BCM281xx SoC family, including BCM11130, BCM11140, BCM11351,
-	  BCM28145, and BCM28155 SoCs.  This driver requires the pinctrl
-	  framework.  GPIO is provided by a separate GPIO driver.
+	  driver for memory mapped GPIO functionality on AMD platforms
+	  (x86 or arm).Most pins are usually muxed to some other
+	  functionality by firmware,so only a small amount is available
+	  for gpio use.
+
+	  Requires ACPI/FDT device enumeration code to set up a platform
+	  device.
 
 config PINCTRL_LANTIQ
 	bool
@@ -154,6 +151,10 @@ config PINCTRL_TEGRA124
 	bool
 	select PINCTRL_TEGRA
 
+config PINCTRL_TEGRA210
+	bool
+	select PINCTRL_TEGRA
+
 config PINCTRL_TEGRA_XUSB
 	def_bool y if ARCH_TEGRA
 	select GENERIC_PHY
@@ -207,6 +208,7 @@ config PINCTRL_ZYNQ
 	help
 	  This selectes the pinctrl driver for Xilinx Zynq.
 
+source "drivers/pinctrl/bcm/Kconfig"
 source "drivers/pinctrl/berlin/Kconfig"
 source "drivers/pinctrl/freescale/Kconfig"
 source "drivers/pinctrl/intel/Kconfig"
@@ -218,6 +220,7 @@ source "drivers/pinctrl/sh-pfc/Kconfig"
 source "drivers/pinctrl/spear/Kconfig"
 source "drivers/pinctrl/sunxi/Kconfig"
 source "drivers/pinctrl/vt8500/Kconfig"
+source "drivers/pinctrl/mediatek/Kconfig"
 
 config PINCTRL_XWAY
 	bool
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 0475206dd600..6eadf04a33b3 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -14,8 +14,7 @@ obj-$(CONFIG_PINCTRL_AS3722)	+= pinctrl-as3722.o
 obj-$(CONFIG_PINCTRL_BF54x)	+= pinctrl-adi2-bf54x.o
 obj-$(CONFIG_PINCTRL_BF60x)	+= pinctrl-adi2-bf60x.o
 obj-$(CONFIG_PINCTRL_AT91)	+= pinctrl-at91.o
-obj-$(CONFIG_PINCTRL_BCM2835)	+= pinctrl-bcm2835.o
-obj-$(CONFIG_PINCTRL_BCM281XX)	+= pinctrl-bcm281xx.o
+obj-$(CONFIG_PINCTRL_AMD)	+= pinctrl-amd.o
 obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
 obj-$(CONFIG_PINCTRL_MESON)	+= meson/
 obj-$(CONFIG_PINCTRL_PALMAS)	+= pinctrl-palmas.o
@@ -27,6 +26,7 @@ obj-$(CONFIG_PINCTRL_TEGRA20)	+= pinctrl-tegra20.o
 obj-$(CONFIG_PINCTRL_TEGRA30)	+= pinctrl-tegra30.o
 obj-$(CONFIG_PINCTRL_TEGRA114)	+= pinctrl-tegra114.o
 obj-$(CONFIG_PINCTRL_TEGRA124)	+= pinctrl-tegra124.o
+obj-$(CONFIG_PINCTRL_TEGRA210)	+= pinctrl-tegra210.o
 obj-$(CONFIG_PINCTRL_TEGRA_XUSB)	+= pinctrl-tegra-xusb.o
 obj-$(CONFIG_PINCTRL_TZ1090)	+= pinctrl-tz1090.o
 obj-$(CONFIG_PINCTRL_TZ1090_PDC)	+= pinctrl-tz1090-pdc.o
@@ -38,6 +38,7 @@ obj-$(CONFIG_PINCTRL_TB10X)	+= pinctrl-tb10x.o
 obj-$(CONFIG_PINCTRL_ST) 	+= pinctrl-st.o
 obj-$(CONFIG_PINCTRL_ZYNQ)	+= pinctrl-zynq.o
 
+obj-$(CONFIG_ARCH_BCM)		+= bcm/
 obj-$(CONFIG_ARCH_BERLIN)	+= berlin/
 obj-y				+= freescale/
 obj-$(CONFIG_X86)		+= intel/
@@ -49,3 +50,4 @@ obj-$(CONFIG_PINCTRL_SH_PFC)	+= sh-pfc/
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
 obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
 obj-$(CONFIG_ARCH_VT8500)	+= vt8500/
+obj-$(CONFIG_ARCH_MEDIATEK)	+= mediatek/
diff --git a/drivers/pinctrl/bcm/Kconfig b/drivers/pinctrl/bcm/Kconfig
new file mode 100644
index 000000000000..cd11d4d9ad58
--- /dev/null
+++ b/drivers/pinctrl/bcm/Kconfig
@@ -0,0 +1,56 @@
+#
+# Broadcom pinctrl drivers
+#
+
+config PINCTRL_BCM281XX
+	bool "Broadcom BCM281xx pinctrl driver"
+	depends on OF && (ARCH_BCM_MOBILE || COMPILE_TEST)
+	select PINMUX
+	select PINCONF
+	select GENERIC_PINCONF
+	select REGMAP_MMIO
+	help
+	  Say Y here to support Broadcom BCM281xx pinctrl driver, which is used
+	  for the BCM281xx SoC family, including BCM11130, BCM11140, BCM11351,
+	  BCM28145, and BCM28155 SoCs.  This driver requires the pinctrl
+	  framework.  GPIO is provided by a separate GPIO driver.
+
+config PINCTRL_BCM2835
+	bool
+	select PINMUX
+	select PINCONF
+
+config PINCTRL_CYGNUS_GPIO
+	bool "Broadcom Cygnus GPIO (with PINCONF) driver"
+	depends on OF_GPIO && ARCH_BCM_CYGNUS
+	select GPIOLIB_IRQCHIP
+	select PINCONF
+	select GENERIC_PINCONF
+	default ARCH_BCM_CYGNUS
+	help
+	  Say yes here to enable the Broadcom Cygnus GPIO driver.
+
+	  The Broadcom Cygnus SoC has 3 GPIO controllers including the ASIU
+	  GPIO controller (ASIU), the chipCommonG GPIO controller (CCM), and
+	  the always-ON GPIO controller (CRMU/AON). All 3 GPIO controllers are
+	  supported by this driver.
+
+	  All 3 Cygnus GPIO controllers support basic PINCONF functions such
+	  as bias pull up, pull down, and drive strength configurations, when
+	  these pins are muxed to GPIO.
+
+	  Pins from the ASIU GPIO can be individually muxed to GPIO function,
+	  through interaction with the Cygnus IOMUX controller.
+
+config PINCTRL_CYGNUS_MUX
+	bool "Broadcom Cygnus IOMUX driver"
+	depends on (ARCH_BCM_CYGNUS || COMPILE_TEST)
+	select PINMUX
+	select GENERIC_PINCONF
+	default ARCH_BCM_CYGNUS
+	help
+	  Say yes here to enable the Broadcom Cygnus IOMUX driver.
+
+	  The Broadcom Cygnus IOMUX driver supports group based IOMUX
+	  configuration, with the exception that certain individual pins
+	  can be overrided to GPIO function
diff --git a/drivers/pinctrl/bcm/Makefile b/drivers/pinctrl/bcm/Makefile
new file mode 100644
index 000000000000..2b2f70ee804c
--- /dev/null
+++ b/drivers/pinctrl/bcm/Makefile
@@ -0,0 +1,6 @@
+# Broadcom pinctrl support
+
+obj-$(CONFIG_PINCTRL_BCM281XX)		+= pinctrl-bcm281xx.o
+obj-$(CONFIG_PINCTRL_BCM2835)		+= pinctrl-bcm2835.o
+obj-$(CONFIG_PINCTRL_CYGNUS_GPIO)	+= pinctrl-cygnus-gpio.o
+obj-$(CONFIG_PINCTRL_CYGNUS_MUX)	+= pinctrl-cygnus-mux.o
diff --git a/drivers/pinctrl/pinctrl-bcm281xx.c b/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
index b88cfe5ed55a..9641f1c7617e 100644
--- a/drivers/pinctrl/pinctrl-bcm281xx.c
+++ b/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
@@ -21,8 +21,8 @@
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/regmap.h>
 #include <linux/slab.h>
-#include "core.h"
-#include "pinctrl-utils.h"
+#include "../core.h"
+#include "../pinctrl-utils.h"
 
 /* BCM281XX Pin Control Registers Definitions */
 
diff --git a/drivers/pinctrl/pinctrl-bcm2835.c b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
index 9aa8a3f10b10..8d908e3f42c3 100644
--- a/drivers/pinctrl/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
@@ -403,15 +403,7 @@ static irqreturn_t bcm2835_gpio_irq_handler(int irq, void *dev_id)
 		gpio = (32 * bank) + offset;
 		type = pc->irq_type[gpio];
 
-		/* ack edge triggered IRQs immediately */
-		if (!(type & IRQ_TYPE_LEVEL_MASK))
-			bcm2835_gpio_set_bit(pc, GPEDS0, gpio);
-
 		generic_handle_irq(irq_linear_revmap(pc->irq_domain, gpio));
-
-		/* ack level triggered IRQ after handling them */
-		if (type & IRQ_TYPE_LEVEL_MASK)
-			bcm2835_gpio_set_bit(pc, GPEDS0, gpio);
 	}
 	return events ? IRQ_HANDLED : IRQ_NONE;
 }
@@ -591,16 +583,32 @@ static int bcm2835_gpio_irq_set_type(struct irq_data *data, unsigned int type)
 	else
 		ret = __bcm2835_gpio_irq_set_type_disabled(pc, gpio, type);
 
+	if (type & IRQ_TYPE_EDGE_BOTH)
+		__irq_set_handler_locked(data->irq, handle_edge_irq);
+	else
+		__irq_set_handler_locked(data->irq, handle_level_irq);
+
 	spin_unlock_irqrestore(&pc->irq_lock[bank], flags);
 
 	return ret;
 }
 
+static void bcm2835_gpio_irq_ack(struct irq_data *data)
+{
+	struct bcm2835_pinctrl *pc = irq_data_get_irq_chip_data(data);
+	unsigned gpio = irqd_to_hwirq(data);
+
+	bcm2835_gpio_set_bit(pc, GPEDS0, gpio);
+}
+
 static struct irq_chip bcm2835_gpio_irq_chip = {
 	.name = MODULE_NAME,
 	.irq_enable = bcm2835_gpio_irq_enable,
 	.irq_disable = bcm2835_gpio_irq_disable,
 	.irq_set_type = bcm2835_gpio_irq_set_type,
+	.irq_ack = bcm2835_gpio_irq_ack,
+	.irq_mask = bcm2835_gpio_irq_disable,
+	.irq_unmask = bcm2835_gpio_irq_enable,
 };
 
 static int bcm2835_pctl_get_groups_count(struct pinctrl_dev *pctldev)
@@ -977,7 +985,7 @@ static int bcm2835_pinctrl_probe(struct platform_device *pdev)
 		int irq = irq_create_mapping(pc->irq_domain, i);
 		irq_set_lockdep_class(irq, &gpio_lock_class);
 		irq_set_chip_and_handler(irq, &bcm2835_gpio_irq_chip,
-				handle_simple_irq);
+				handle_level_irq);
 		irq_set_chip_data(irq, pc);
 		set_irq_flags(irq, IRQF_VALID);
 	}
@@ -1051,7 +1059,7 @@ static int bcm2835_pinctrl_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id bcm2835_pinctrl_match[] = {
+static const struct of_device_id bcm2835_pinctrl_match[] = {
 	{ .compatible = "brcm,bcm2835-gpio" },
 	{}
 };
diff --git a/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c b/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c
new file mode 100644
index 000000000000..4ad5c1a996e3
--- /dev/null
+++ b/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c
@@ -0,0 +1,907 @@
+/*
+ * 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.
+ *
+ * This file contains the Broadcom Cygnus GPIO driver that supports 3
+ * GPIO controllers on Cygnus including the ASIU GPIO controller, the
+ * chipCommonG GPIO controller, and the always-on GPIO controller. Basic
+ * PINCONF such as bias pull up/down, and drive strength are also supported
+ * in this driver.
+ *
+ * Pins from the ASIU GPIO can be individually muxed to GPIO function,
+ * through the interaction with the Cygnus IOMUX controller
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/ioport.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+
+#include "../pinctrl-utils.h"
+
+#define CYGNUS_GPIO_DATA_IN_OFFSET   0x00
+#define CYGNUS_GPIO_DATA_OUT_OFFSET  0x04
+#define CYGNUS_GPIO_OUT_EN_OFFSET    0x08
+#define CYGNUS_GPIO_IN_TYPE_OFFSET   0x0c
+#define CYGNUS_GPIO_INT_DE_OFFSET    0x10
+#define CYGNUS_GPIO_INT_EDGE_OFFSET  0x14
+#define CYGNUS_GPIO_INT_MSK_OFFSET   0x18
+#define CYGNUS_GPIO_INT_STAT_OFFSET  0x1c
+#define CYGNUS_GPIO_INT_MSTAT_OFFSET 0x20
+#define CYGNUS_GPIO_INT_CLR_OFFSET   0x24
+#define CYGNUS_GPIO_PAD_RES_OFFSET   0x34
+#define CYGNUS_GPIO_RES_EN_OFFSET    0x38
+
+/* drive strength control for ASIU GPIO */
+#define CYGNUS_GPIO_ASIU_DRV0_CTRL_OFFSET 0x58
+
+/* drive strength control for CCM/CRMU (AON) GPIO */
+#define CYGNUS_GPIO_DRV0_CTRL_OFFSET  0x00
+
+#define GPIO_BANK_SIZE 0x200
+#define NGPIOS_PER_BANK 32
+#define GPIO_BANK(pin) ((pin) / NGPIOS_PER_BANK)
+
+#define CYGNUS_GPIO_REG(pin, reg) (GPIO_BANK(pin) * GPIO_BANK_SIZE + (reg))
+#define CYGNUS_GPIO_SHIFT(pin) ((pin) % NGPIOS_PER_BANK)
+
+#define GPIO_DRV_STRENGTH_BIT_SHIFT  20
+#define GPIO_DRV_STRENGTH_BITS       3
+#define GPIO_DRV_STRENGTH_BIT_MASK   ((1 << GPIO_DRV_STRENGTH_BITS) - 1)
+
+/*
+ * Cygnus GPIO core
+ *
+ * @dev: pointer to device
+ * @base: I/O register base for Cygnus GPIO controller
+ * @io_ctrl: I/O register base for certain type of Cygnus GPIO controller that
+ * has the PINCONF support implemented outside of the GPIO block
+ * @lock: lock to protect access to I/O registers
+ * @gc: GPIO chip
+ * @num_banks: number of GPIO banks, each bank supports up to 32 GPIOs
+ * @pinmux_is_supported: flag to indicate this GPIO controller contains pins
+ * that can be individually muxed to GPIO
+ * @pctl: pointer to pinctrl_dev
+ * @pctldesc: pinctrl descriptor
+ */
+struct cygnus_gpio {
+	struct device *dev;
+
+	void __iomem *base;
+	void __iomem *io_ctrl;
+
+	spinlock_t lock;
+
+	struct gpio_chip gc;
+	unsigned num_banks;
+
+	bool pinmux_is_supported;
+
+	struct pinctrl_dev *pctl;
+	struct pinctrl_desc pctldesc;
+};
+
+static inline struct cygnus_gpio *to_cygnus_gpio(struct gpio_chip *gc)
+{
+	return container_of(gc, struct cygnus_gpio, gc);
+}
+
+/*
+ * Mapping from PINCONF pins to GPIO pins is 1-to-1
+ */
+static inline unsigned cygnus_pin_to_gpio(unsigned pin)
+{
+	return pin;
+}
+
+/**
+ *  cygnus_set_bit - set or clear one bit (corresponding to the GPIO pin) in a
+ *  Cygnus GPIO register
+ *
+ *  @cygnus_gpio: Cygnus GPIO device
+ *  @reg: register offset
+ *  @gpio: GPIO pin
+ *  @set: set or clear
+ */
+static inline void cygnus_set_bit(struct cygnus_gpio *chip, unsigned int reg,
+				  unsigned gpio, bool set)
+{
+	unsigned int offset = CYGNUS_GPIO_REG(gpio, reg);
+	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+	u32 val;
+
+	val = readl(chip->base + offset);
+	if (set)
+		val |= BIT(shift);
+	else
+		val &= ~BIT(shift);
+	writel(val, chip->base + offset);
+}
+
+static inline bool cygnus_get_bit(struct cygnus_gpio *chip, unsigned int reg,
+				  unsigned gpio)
+{
+	unsigned int offset = CYGNUS_GPIO_REG(gpio, reg);
+	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+
+	return !!(readl(chip->base + offset) & BIT(shift));
+}
+
+static void cygnus_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
+{
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct irq_chip *irq_chip = irq_desc_get_chip(desc);
+	int i, bit;
+
+	chained_irq_enter(irq_chip, desc);
+
+	/* go through the entire GPIO banks and handle all interrupts */
+	for (i = 0; i < chip->num_banks; i++) {
+		unsigned long val = readl(chip->base + (i * GPIO_BANK_SIZE) +
+					  CYGNUS_GPIO_INT_MSTAT_OFFSET);
+
+		for_each_set_bit(bit, &val, NGPIOS_PER_BANK) {
+			unsigned pin = NGPIOS_PER_BANK * i + bit;
+			int child_irq = irq_find_mapping(gc->irqdomain, pin);
+
+			/*
+			 * Clear the interrupt before invoking the
+			 * handler, so we do not leave any window
+			 */
+			writel(BIT(bit), chip->base + (i * GPIO_BANK_SIZE) +
+			       CYGNUS_GPIO_INT_CLR_OFFSET);
+
+			generic_handle_irq(child_irq);
+		}
+	}
+
+	chained_irq_exit(irq_chip, desc);
+}
+
+
+static void cygnus_gpio_irq_ack(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned gpio = d->hwirq;
+	unsigned int offset = CYGNUS_GPIO_REG(gpio,
+			CYGNUS_GPIO_INT_CLR_OFFSET);
+	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+	u32 val = BIT(shift);
+
+	writel(val, chip->base + offset);
+}
+
+/**
+ *  cygnus_gpio_irq_set_mask - mask/unmask a GPIO interrupt
+ *
+ *  @d: IRQ chip data
+ *  @unmask: mask/unmask GPIO interrupt
+ */
+static void cygnus_gpio_irq_set_mask(struct irq_data *d, bool unmask)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned gpio = d->hwirq;
+
+	cygnus_set_bit(chip, CYGNUS_GPIO_INT_MSK_OFFSET, gpio, unmask);
+}
+
+static void cygnus_gpio_irq_mask(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	cygnus_gpio_irq_set_mask(d, false);
+	spin_unlock_irqrestore(&chip->lock, flags);
+}
+
+static void cygnus_gpio_irq_unmask(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	cygnus_gpio_irq_set_mask(d, true);
+	spin_unlock_irqrestore(&chip->lock, flags);
+}
+
+static int cygnus_gpio_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned gpio = d->hwirq;
+	bool level_triggered = false;
+	bool dual_edge = false;
+	bool rising_or_high = false;
+	unsigned long flags;
+
+	switch (type & IRQ_TYPE_SENSE_MASK) {
+	case IRQ_TYPE_EDGE_RISING:
+		rising_or_high = true;
+		break;
+
+	case IRQ_TYPE_EDGE_FALLING:
+		break;
+
+	case IRQ_TYPE_EDGE_BOTH:
+		dual_edge = true;
+		break;
+
+	case IRQ_TYPE_LEVEL_HIGH:
+		level_triggered = true;
+		rising_or_high = true;
+		break;
+
+	case IRQ_TYPE_LEVEL_LOW:
+		level_triggered = true;
+		break;
+
+	default:
+		dev_err(chip->dev, "invalid GPIO IRQ type 0x%x\n",
+			type);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&chip->lock, flags);
+	cygnus_set_bit(chip, CYGNUS_GPIO_IN_TYPE_OFFSET, gpio,
+		       level_triggered);
+	cygnus_set_bit(chip, CYGNUS_GPIO_INT_DE_OFFSET, gpio, dual_edge);
+	cygnus_set_bit(chip, CYGNUS_GPIO_INT_EDGE_OFFSET, gpio,
+		       rising_or_high);
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev,
+		"gpio:%u level_triggered:%d dual_edge:%d rising_or_high:%d\n",
+		gpio, level_triggered, dual_edge, rising_or_high);
+
+	return 0;
+}
+
+static struct irq_chip cygnus_gpio_irq_chip = {
+	.name = "bcm-cygnus-gpio",
+	.irq_ack = cygnus_gpio_irq_ack,
+	.irq_mask = cygnus_gpio_irq_mask,
+	.irq_unmask = cygnus_gpio_irq_unmask,
+	.irq_set_type = cygnus_gpio_irq_set_type,
+};
+
+/*
+ * Request the Cygnus IOMUX pinmux controller to mux individual pins to GPIO
+ */
+static int cygnus_gpio_request(struct gpio_chip *gc, unsigned offset)
+{
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned gpio = gc->base + offset;
+
+	/* not all Cygnus GPIO pins can be muxed individually */
+	if (!chip->pinmux_is_supported)
+		return 0;
+
+	return pinctrl_request_gpio(gpio);
+}
+
+static void cygnus_gpio_free(struct gpio_chip *gc, unsigned offset)
+{
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned gpio = gc->base + offset;
+
+	if (!chip->pinmux_is_supported)
+		return;
+
+	pinctrl_free_gpio(gpio);
+}
+
+static int cygnus_gpio_direction_input(struct gpio_chip *gc, unsigned gpio)
+{
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	cygnus_set_bit(chip, CYGNUS_GPIO_OUT_EN_OFFSET, gpio, false);
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set input\n", gpio);
+
+	return 0;
+}
+
+static int cygnus_gpio_direction_output(struct gpio_chip *gc, unsigned gpio,
+					int val)
+{
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	cygnus_set_bit(chip, CYGNUS_GPIO_OUT_EN_OFFSET, gpio, true);
+	cygnus_set_bit(chip, CYGNUS_GPIO_DATA_OUT_OFFSET, gpio, !!(val));
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set output, value:%d\n", gpio, val);
+
+	return 0;
+}
+
+static void cygnus_gpio_set(struct gpio_chip *gc, unsigned gpio, int val)
+{
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	cygnus_set_bit(chip, CYGNUS_GPIO_DATA_OUT_OFFSET, gpio, !!(val));
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set, value:%d\n", gpio, val);
+}
+
+static int cygnus_gpio_get(struct gpio_chip *gc, unsigned gpio)
+{
+	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	unsigned int offset = CYGNUS_GPIO_REG(gpio,
+					      CYGNUS_GPIO_DATA_IN_OFFSET);
+	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+
+	return !!(readl(chip->base + offset) & BIT(shift));
+}
+
+static int cygnus_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	return 1;
+}
+
+/*
+ * Only one group: "gpio_grp", since this local pinctrl device only performs
+ * GPIO specific PINCONF configurations
+ */
+static const char *cygnus_get_group_name(struct pinctrl_dev *pctldev,
+					 unsigned selector)
+{
+	return "gpio_grp";
+}
+
+static const struct pinctrl_ops cygnus_pctrl_ops = {
+	.get_groups_count = cygnus_get_groups_count,
+	.get_group_name = cygnus_get_group_name,
+	.dt_node_to_map = pinconf_generic_dt_node_to_map_pin,
+	.dt_free_map = pinctrl_utils_dt_free_map,
+};
+
+static int cygnus_gpio_set_pull(struct cygnus_gpio *chip, unsigned gpio,
+				bool disable, bool pull_up)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+
+	if (disable) {
+		cygnus_set_bit(chip, CYGNUS_GPIO_RES_EN_OFFSET, gpio, false);
+	} else {
+		cygnus_set_bit(chip, CYGNUS_GPIO_PAD_RES_OFFSET, gpio,
+			       pull_up);
+		cygnus_set_bit(chip, CYGNUS_GPIO_RES_EN_OFFSET, gpio, true);
+	}
+
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set pullup:%d\n", gpio, pull_up);
+
+	return 0;
+}
+
+static void cygnus_gpio_get_pull(struct cygnus_gpio *chip, unsigned gpio,
+				 bool *disable, bool *pull_up)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	*disable = !cygnus_get_bit(chip, CYGNUS_GPIO_RES_EN_OFFSET, gpio);
+	*pull_up = cygnus_get_bit(chip, CYGNUS_GPIO_PAD_RES_OFFSET, gpio);
+	spin_unlock_irqrestore(&chip->lock, flags);
+}
+
+static int cygnus_gpio_set_strength(struct cygnus_gpio *chip, unsigned gpio,
+				    unsigned strength)
+{
+	void __iomem *base;
+	unsigned int i, offset, shift;
+	u32 val;
+	unsigned long flags;
+
+	/* make sure drive strength is supported */
+	if (strength < 2 ||  strength > 16 || (strength % 2))
+		return -ENOTSUPP;
+
+	if (chip->io_ctrl) {
+		base = chip->io_ctrl;
+		offset = CYGNUS_GPIO_DRV0_CTRL_OFFSET;
+	} else {
+		base = chip->base;
+		offset = CYGNUS_GPIO_REG(gpio,
+					 CYGNUS_GPIO_ASIU_DRV0_CTRL_OFFSET);
+	}
+
+	shift = CYGNUS_GPIO_SHIFT(gpio);
+
+	dev_dbg(chip->dev, "gpio:%u set drive strength:%d mA\n", gpio,
+		strength);
+
+	spin_lock_irqsave(&chip->lock, flags);
+	strength = (strength / 2) - 1;
+	for (i = 0; i < GPIO_DRV_STRENGTH_BITS; i++) {
+		val = readl(base + offset);
+		val &= ~BIT(shift);
+		val |= ((strength >> i) & 0x1) << shift;
+		writel(val, base + offset);
+		offset += 4;
+	}
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	return 0;
+}
+
+static int cygnus_gpio_get_strength(struct cygnus_gpio *chip, unsigned gpio,
+				    u16 *strength)
+{
+	void __iomem *base;
+	unsigned int i, offset, shift;
+	u32 val;
+	unsigned long flags;
+
+	if (chip->io_ctrl) {
+		base = chip->io_ctrl;
+		offset = CYGNUS_GPIO_DRV0_CTRL_OFFSET;
+	} else {
+		base = chip->base;
+		offset = CYGNUS_GPIO_REG(gpio,
+					 CYGNUS_GPIO_ASIU_DRV0_CTRL_OFFSET);
+	}
+
+	shift = CYGNUS_GPIO_SHIFT(gpio);
+
+	spin_lock_irqsave(&chip->lock, flags);
+	*strength = 0;
+	for (i = 0; i < GPIO_DRV_STRENGTH_BITS; i++) {
+		val = readl(base + offset) & BIT(shift);
+		val >>= shift;
+		*strength += (val << i);
+		offset += 4;
+	}
+
+	/* convert to mA */
+	*strength = (*strength + 1) * 2;
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	return 0;
+}
+
+static int cygnus_pin_config_get(struct pinctrl_dev *pctldev, unsigned pin,
+				 unsigned long *config)
+{
+	struct cygnus_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	unsigned gpio = cygnus_pin_to_gpio(pin);
+	u16 arg;
+	bool disable, pull_up;
+	int ret;
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		cygnus_gpio_get_pull(chip, gpio, &disable, &pull_up);
+		if (disable)
+			return 0;
+		else
+			return -EINVAL;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		cygnus_gpio_get_pull(chip, gpio, &disable, &pull_up);
+		if (!disable && pull_up)
+			return 0;
+		else
+			return -EINVAL;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		cygnus_gpio_get_pull(chip, gpio, &disable, &pull_up);
+		if (!disable && !pull_up)
+			return 0;
+		else
+			return -EINVAL;
+
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		ret = cygnus_gpio_get_strength(chip, gpio, &arg);
+		if (ret)
+			return ret;
+		else
+			*config = pinconf_to_config_packed(param, arg);
+
+		return 0;
+
+	default:
+		return -ENOTSUPP;
+	}
+
+	return -ENOTSUPP;
+}
+
+static int cygnus_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
+				 unsigned long *configs, unsigned num_configs)
+{
+	struct cygnus_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param;
+	u16 arg;
+	unsigned i, gpio = cygnus_pin_to_gpio(pin);
+	int ret = -ENOTSUPP;
+
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		arg = pinconf_to_config_argument(configs[i]);
+
+		switch (param) {
+		case PIN_CONFIG_BIAS_DISABLE:
+			ret = cygnus_gpio_set_pull(chip, gpio, true, false);
+			if (ret < 0)
+				goto out;
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_UP:
+			ret = cygnus_gpio_set_pull(chip, gpio, false, true);
+			if (ret < 0)
+				goto out;
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			ret = cygnus_gpio_set_pull(chip, gpio, false, false);
+			if (ret < 0)
+				goto out;
+			break;
+
+		case PIN_CONFIG_DRIVE_STRENGTH:
+			ret = cygnus_gpio_set_strength(chip, gpio, arg);
+			if (ret < 0)
+				goto out;
+			break;
+
+		default:
+			dev_err(chip->dev, "invalid configuration\n");
+			return -ENOTSUPP;
+		}
+	} /* for each config */
+
+out:
+	return ret;
+}
+
+static const struct pinconf_ops cygnus_pconf_ops = {
+	.is_generic = true,
+	.pin_config_get = cygnus_pin_config_get,
+	.pin_config_set = cygnus_pin_config_set,
+};
+
+/*
+ * Map a GPIO in the local gpio_chip pin space to a pin in the Cygnus IOMUX
+ * pinctrl pin space
+ */
+struct cygnus_gpio_pin_range {
+	unsigned offset;
+	unsigned pin_base;
+	unsigned num_pins;
+};
+
+#define CYGNUS_PINRANGE(o, p, n) { .offset = o, .pin_base = p, .num_pins = n }
+
+/*
+ * Pin mapping table for mapping local GPIO pins to Cygnus IOMUX pinctrl pins
+ */
+static const struct cygnus_gpio_pin_range cygnus_gpio_pintable[] = {
+	CYGNUS_PINRANGE(0, 42, 1),
+	CYGNUS_PINRANGE(1, 44, 3),
+	CYGNUS_PINRANGE(4, 48, 1),
+	CYGNUS_PINRANGE(5, 50, 3),
+	CYGNUS_PINRANGE(8, 126, 1),
+	CYGNUS_PINRANGE(9, 155, 1),
+	CYGNUS_PINRANGE(10, 152, 1),
+	CYGNUS_PINRANGE(11, 154, 1),
+	CYGNUS_PINRANGE(12, 153, 1),
+	CYGNUS_PINRANGE(13, 127, 3),
+	CYGNUS_PINRANGE(16, 140, 1),
+	CYGNUS_PINRANGE(17, 145, 7),
+	CYGNUS_PINRANGE(24, 130, 10),
+	CYGNUS_PINRANGE(34, 141, 4),
+	CYGNUS_PINRANGE(38, 54, 1),
+	CYGNUS_PINRANGE(39, 56, 3),
+	CYGNUS_PINRANGE(42, 60, 3),
+	CYGNUS_PINRANGE(45, 64, 3),
+	CYGNUS_PINRANGE(48, 68, 2),
+	CYGNUS_PINRANGE(50, 84, 6),
+	CYGNUS_PINRANGE(56, 94, 6),
+	CYGNUS_PINRANGE(62, 72, 1),
+	CYGNUS_PINRANGE(63, 70, 1),
+	CYGNUS_PINRANGE(64, 80, 1),
+	CYGNUS_PINRANGE(65, 74, 3),
+	CYGNUS_PINRANGE(68, 78, 1),
+	CYGNUS_PINRANGE(69, 82, 1),
+	CYGNUS_PINRANGE(70, 156, 17),
+	CYGNUS_PINRANGE(87, 104, 12),
+	CYGNUS_PINRANGE(99, 102, 2),
+	CYGNUS_PINRANGE(101, 90, 4),
+	CYGNUS_PINRANGE(105, 116, 10),
+	CYGNUS_PINRANGE(123, 11, 1),
+	CYGNUS_PINRANGE(124, 38, 4),
+	CYGNUS_PINRANGE(128, 43, 1),
+	CYGNUS_PINRANGE(129, 47, 1),
+	CYGNUS_PINRANGE(130, 49, 1),
+	CYGNUS_PINRANGE(131, 53, 1),
+	CYGNUS_PINRANGE(132, 55, 1),
+	CYGNUS_PINRANGE(133, 59, 1),
+	CYGNUS_PINRANGE(134, 63, 1),
+	CYGNUS_PINRANGE(135, 67, 1),
+	CYGNUS_PINRANGE(136, 71, 1),
+	CYGNUS_PINRANGE(137, 73, 1),
+	CYGNUS_PINRANGE(138, 77, 1),
+	CYGNUS_PINRANGE(139, 79, 1),
+	CYGNUS_PINRANGE(140, 81, 1),
+	CYGNUS_PINRANGE(141, 83, 1),
+	CYGNUS_PINRANGE(142, 10, 1)
+};
+
+/*
+ * The Cygnus IOMUX controller mainly supports group based mux configuration,
+ * but certain pins can be muxed to GPIO individually. Only the ASIU GPIO
+ * controller can support this, so it's an optional configuration
+ *
+ * Return -ENODEV means no support and that's fine
+ */
+static int cygnus_gpio_pinmux_add_range(struct cygnus_gpio *chip)
+{
+	struct device_node *node = chip->dev->of_node;
+	struct device_node *pinmux_node;
+	struct platform_device *pinmux_pdev;
+	struct gpio_chip *gc = &chip->gc;
+	int i, ret = 0;
+
+	/* parse DT to find the phandle to the pinmux controller */
+	pinmux_node = of_parse_phandle(node, "pinmux", 0);
+	if (!pinmux_node)
+		return -ENODEV;
+
+	pinmux_pdev = of_find_device_by_node(pinmux_node);
+	/* no longer need the pinmux node */
+	of_node_put(pinmux_node);
+	if (!pinmux_pdev) {
+		dev_err(chip->dev, "failed to get pinmux device\n");
+		return -EINVAL;
+	}
+
+	/* now need to create the mapping between local GPIO and PINMUX pins */
+	for (i = 0; i < ARRAY_SIZE(cygnus_gpio_pintable); i++) {
+		ret = gpiochip_add_pin_range(gc, dev_name(&pinmux_pdev->dev),
+					     cygnus_gpio_pintable[i].offset,
+					     cygnus_gpio_pintable[i].pin_base,
+					     cygnus_gpio_pintable[i].num_pins);
+		if (ret) {
+			dev_err(chip->dev, "unable to add GPIO pin range\n");
+			goto err_put_device;
+		}
+	}
+
+	chip->pinmux_is_supported = true;
+
+	/* no need for pinmux_pdev device reference anymore */
+	put_device(&pinmux_pdev->dev);
+	return 0;
+
+err_put_device:
+	put_device(&pinmux_pdev->dev);
+	gpiochip_remove_pin_ranges(gc);
+	return ret;
+}
+
+/*
+ * Cygnus GPIO controller supports some PINCONF related configurations such as
+ * pull up, pull down, and drive strength, when the pin is configured to GPIO
+ *
+ * Here a local pinctrl device is created with simple 1-to-1 pin mapping to the
+ * local GPIO pins
+ */
+static int cygnus_gpio_register_pinconf(struct cygnus_gpio *chip)
+{
+	struct pinctrl_desc *pctldesc = &chip->pctldesc;
+	struct pinctrl_pin_desc *pins;
+	struct gpio_chip *gc = &chip->gc;
+	int i;
+
+	pins = devm_kcalloc(chip->dev, gc->ngpio, sizeof(*pins), GFP_KERNEL);
+	if (!pins)
+		return -ENOMEM;
+
+	for (i = 0; i < gc->ngpio; i++) {
+		pins[i].number = i;
+		pins[i].name = devm_kasprintf(chip->dev, GFP_KERNEL,
+					      "gpio-%d", i);
+		if (!pins[i].name)
+			return -ENOMEM;
+	}
+
+	pctldesc->name = dev_name(chip->dev);
+	pctldesc->pctlops = &cygnus_pctrl_ops;
+	pctldesc->pins = pins;
+	pctldesc->npins = gc->ngpio;
+	pctldesc->confops = &cygnus_pconf_ops;
+
+	chip->pctl = pinctrl_register(pctldesc, chip->dev, chip);
+	if (!chip->pctl) {
+		dev_err(chip->dev, "unable to register pinctrl device\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void cygnus_gpio_unregister_pinconf(struct cygnus_gpio *chip)
+{
+	if (chip->pctl)
+		pinctrl_unregister(chip->pctl);
+}
+
+struct cygnus_gpio_data {
+	unsigned num_gpios;
+};
+
+static const struct cygnus_gpio_data cygnus_cmm_gpio_data = {
+	.num_gpios = 24,
+};
+
+static const struct cygnus_gpio_data cygnus_asiu_gpio_data = {
+	.num_gpios = 146,
+};
+
+static const struct cygnus_gpio_data cygnus_crmu_gpio_data = {
+	.num_gpios = 6,
+};
+
+static const struct of_device_id cygnus_gpio_of_match[] = {
+	{
+		.compatible = "brcm,cygnus-ccm-gpio",
+		.data = &cygnus_cmm_gpio_data,
+	},
+	{
+		.compatible = "brcm,cygnus-asiu-gpio",
+		.data = &cygnus_asiu_gpio_data,
+	},
+	{
+		.compatible = "brcm,cygnus-crmu-gpio",
+		.data = &cygnus_crmu_gpio_data,
+	}
+};
+
+static int cygnus_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct cygnus_gpio *chip;
+	struct gpio_chip *gc;
+	u32 ngpios;
+	int irq, ret;
+	const struct of_device_id *match;
+	const struct cygnus_gpio_data *gpio_data;
+
+	match = of_match_device(cygnus_gpio_of_match, dev);
+	if (!match)
+		return -ENODEV;
+	gpio_data = match->data;
+	ngpios = gpio_data->num_gpios;
+
+	chip = devm_kzalloc(dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->dev = dev;
+	platform_set_drvdata(pdev, chip);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	chip->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(chip->base)) {
+		dev_err(dev, "unable to map I/O memory\n");
+		return PTR_ERR(chip->base);
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (res) {
+		chip->io_ctrl = devm_ioremap_resource(dev, res);
+		if (IS_ERR(chip->io_ctrl)) {
+			dev_err(dev, "unable to map I/O memory\n");
+			return PTR_ERR(chip->io_ctrl);
+		}
+	}
+
+	spin_lock_init(&chip->lock);
+
+	gc = &chip->gc;
+	gc->base = -1;
+	gc->ngpio = ngpios;
+	chip->num_banks = (ngpios + NGPIOS_PER_BANK - 1) / NGPIOS_PER_BANK;
+	gc->label = dev_name(dev);
+	gc->dev = dev;
+	gc->of_node = dev->of_node;
+	gc->request = cygnus_gpio_request;
+	gc->free = cygnus_gpio_free;
+	gc->direction_input = cygnus_gpio_direction_input;
+	gc->direction_output = cygnus_gpio_direction_output;
+	gc->set = cygnus_gpio_set;
+	gc->get = cygnus_gpio_get;
+
+	ret = gpiochip_add(gc);
+	if (ret < 0) {
+		dev_err(dev, "unable to add GPIO chip\n");
+		return ret;
+	}
+
+	ret = cygnus_gpio_pinmux_add_range(chip);
+	if (ret && ret != -ENODEV) {
+		dev_err(dev, "unable to add GPIO pin range\n");
+		goto err_rm_gpiochip;
+	}
+
+	ret = cygnus_gpio_register_pinconf(chip);
+	if (ret) {
+		dev_err(dev, "unable to register pinconf\n");
+		goto err_rm_gpiochip;
+	}
+
+	/* optional GPIO interrupt support */
+	irq = platform_get_irq(pdev, 0);
+	if (irq) {
+		ret = gpiochip_irqchip_add(gc, &cygnus_gpio_irq_chip, 0,
+					   handle_simple_irq, IRQ_TYPE_NONE);
+		if (ret) {
+			dev_err(dev, "no GPIO irqchip\n");
+			goto err_unregister_pinconf;
+		}
+
+		gpiochip_set_chained_irqchip(gc, &cygnus_gpio_irq_chip, irq,
+					     cygnus_gpio_irq_handler);
+	}
+
+	return 0;
+
+err_unregister_pinconf:
+	cygnus_gpio_unregister_pinconf(chip);
+
+err_rm_gpiochip:
+	gpiochip_remove(gc);
+
+	return ret;
+}
+
+static struct platform_driver cygnus_gpio_driver = {
+	.driver = {
+		.name = "cygnus-gpio",
+		.of_match_table = cygnus_gpio_of_match,
+	},
+	.probe = cygnus_gpio_probe,
+};
+
+static int __init cygnus_gpio_init(void)
+{
+	return platform_driver_probe(&cygnus_gpio_driver, cygnus_gpio_probe);
+}
+arch_initcall_sync(cygnus_gpio_init);
diff --git a/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c b/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
new file mode 100644
index 000000000000..f9a9283caf81
--- /dev/null
+++ b/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
@@ -0,0 +1,1022 @@
+/* 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.
+ *
+ * This file contains the Cygnus IOMUX driver that supports group based PINMUX
+ * configuration. Although PINMUX configuration is mainly group based, the
+ * Cygnus IOMUX controller allows certain pins to be individually muxed to GPIO
+ * function, and therefore be controlled by the Cygnus ASIU GPIO controller
+ */
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include "../core.h"
+#include "../pinctrl-utils.h"
+
+#define CYGNUS_NUM_IOMUX_REGS     8
+#define CYGNUS_NUM_MUX_PER_REG    8
+#define CYGNUS_NUM_IOMUX          (CYGNUS_NUM_IOMUX_REGS * \
+				   CYGNUS_NUM_MUX_PER_REG)
+
+/*
+ * Cygnus IOMUX register description
+ *
+ * @offset: register offset for mux configuration of a group
+ * @shift: bit shift for mux configuration of a group
+ * @alt: alternate function to set to
+ */
+struct cygnus_mux {
+	unsigned int offset;
+	unsigned int shift;
+	unsigned int alt;
+};
+
+/*
+ * Keep track of Cygnus IOMUX configuration and prevent double configuration
+ *
+ * @cygnus_mux: Cygnus IOMUX register description
+ * @is_configured: flag to indicate whether a mux setting has already been
+ * configured
+ */
+struct cygnus_mux_log {
+	struct cygnus_mux mux;
+	bool is_configured;
+};
+
+/*
+ * Group based IOMUX configuration
+ *
+ * @name: name of the group
+ * @pins: array of pins used by this group
+ * @num_pins: total number of pins used by this group
+ * @mux: Cygnus group based IOMUX configuration
+ */
+struct cygnus_pin_group {
+	const char *name;
+	const unsigned *pins;
+	unsigned num_pins;
+	struct cygnus_mux mux;
+};
+
+/*
+ * Cygnus mux function and supported pin groups
+ *
+ * @name: name of the function
+ * @groups: array of groups that can be supported by this function
+ * @num_groups: total number of groups that can be supported by this function
+ */
+struct cygnus_pin_function {
+	const char *name;
+	const char * const *groups;
+	unsigned num_groups;
+};
+
+/*
+ * Cygnus IOMUX pinctrl core
+ *
+ * @pctl: pointer to pinctrl_dev
+ * @dev: pointer to device
+ * @base0: first I/O register base of the Cygnus IOMUX controller
+ * @base1: second I/O register base
+ * @groups: pointer to array of groups
+ * @num_groups: total number of groups
+ * @functions: pointer to array of functions
+ * @num_functions: total number of functions
+ * @mux_log: pointer to the array of mux logs
+ * @lock: lock to protect register access
+ */
+struct cygnus_pinctrl {
+	struct pinctrl_dev *pctl;
+	struct device *dev;
+	void __iomem *base0;
+	void __iomem *base1;
+
+	const struct cygnus_pin_group *groups;
+	unsigned num_groups;
+
+	const struct cygnus_pin_function *functions;
+	unsigned num_functions;
+
+	struct cygnus_mux_log *mux_log;
+
+	spinlock_t lock;
+};
+
+/*
+ * Certain pins can be individually muxed to GPIO function
+ *
+ * @is_supported: flag to indicate GPIO mux is supported for this pin
+ * @offset: register offset for GPIO mux override of a pin
+ * @shift: bit shift for GPIO mux override of a pin
+ */
+struct cygnus_gpio_mux {
+	int is_supported;
+	unsigned int offset;
+	unsigned int shift;
+};
+
+/*
+ * Description of a pin in Cygnus
+ *
+ * @pin: pin number
+ * @name: pin name
+ * @gpio_mux: GPIO override related information
+ */
+struct cygnus_pin {
+	unsigned pin;
+	char *name;
+	struct cygnus_gpio_mux gpio_mux;
+};
+
+#define CYGNUS_PIN_DESC(p, n, i, o, s)	\
+{					\
+	.pin = p,			\
+	.name = n,			\
+	.gpio_mux = {			\
+		.is_supported = i,	\
+		.offset = o,		\
+		.shift = s,		\
+	},				\
+}
+
+/*
+ * List of pins in Cygnus
+ */
+static struct cygnus_pin cygnus_pins[] = {
+	CYGNUS_PIN_DESC(0, "ext_device_reset_n", 0, 0, 0),
+	CYGNUS_PIN_DESC(1, "chip_mode0", 0, 0, 0),
+	CYGNUS_PIN_DESC(2, "chip_mode1", 0, 0, 0),
+	CYGNUS_PIN_DESC(3, "chip_mode2", 0, 0, 0),
+	CYGNUS_PIN_DESC(4, "chip_mode3", 0, 0, 0),
+	CYGNUS_PIN_DESC(5, "chip_mode4", 0, 0, 0),
+	CYGNUS_PIN_DESC(6, "bsc0_scl", 0, 0, 0),
+	CYGNUS_PIN_DESC(7, "bsc0_sda", 0, 0, 0),
+	CYGNUS_PIN_DESC(8, "bsc1_scl", 0, 0, 0),
+	CYGNUS_PIN_DESC(9, "bsc1_sda", 0, 0, 0),
+	CYGNUS_PIN_DESC(10, "d1w_dq", 1, 0x28, 0),
+	CYGNUS_PIN_DESC(11, "d1wowstz_l", 1, 0x4, 28),
+	CYGNUS_PIN_DESC(12, "gpio0", 0, 0, 0),
+	CYGNUS_PIN_DESC(13, "gpio1", 0, 0, 0),
+	CYGNUS_PIN_DESC(14, "gpio2", 0, 0, 0),
+	CYGNUS_PIN_DESC(15, "gpio3", 0, 0, 0),
+	CYGNUS_PIN_DESC(16, "gpio4", 0, 0, 0),
+	CYGNUS_PIN_DESC(17, "gpio5", 0, 0, 0),
+	CYGNUS_PIN_DESC(18, "gpio6", 0, 0, 0),
+	CYGNUS_PIN_DESC(19, "gpio7", 0, 0, 0),
+	CYGNUS_PIN_DESC(20, "gpio8", 0, 0, 0),
+	CYGNUS_PIN_DESC(21, "gpio9", 0, 0, 0),
+	CYGNUS_PIN_DESC(22, "gpio10", 0, 0, 0),
+	CYGNUS_PIN_DESC(23, "gpio11", 0, 0, 0),
+	CYGNUS_PIN_DESC(24, "gpio12", 0, 0, 0),
+	CYGNUS_PIN_DESC(25, "gpio13", 0, 0, 0),
+	CYGNUS_PIN_DESC(26, "gpio14", 0, 0, 0),
+	CYGNUS_PIN_DESC(27, "gpio15", 0, 0, 0),
+	CYGNUS_PIN_DESC(28, "gpio16", 0, 0, 0),
+	CYGNUS_PIN_DESC(29, "gpio17", 0, 0, 0),
+	CYGNUS_PIN_DESC(30, "gpio18", 0, 0, 0),
+	CYGNUS_PIN_DESC(31, "gpio19", 0, 0, 0),
+	CYGNUS_PIN_DESC(32, "gpio20", 0, 0, 0),
+	CYGNUS_PIN_DESC(33, "gpio21", 0, 0, 0),
+	CYGNUS_PIN_DESC(34, "gpio22", 0, 0, 0),
+	CYGNUS_PIN_DESC(35, "gpio23", 0, 0, 0),
+	CYGNUS_PIN_DESC(36, "mdc", 0, 0, 0),
+	CYGNUS_PIN_DESC(37, "mdio", 0, 0, 0),
+	CYGNUS_PIN_DESC(38, "pwm0", 1, 0x10, 30),
+	CYGNUS_PIN_DESC(39, "pwm1", 1, 0x10, 28),
+	CYGNUS_PIN_DESC(40, "pwm2", 1, 0x10, 26),
+	CYGNUS_PIN_DESC(41, "pwm3", 1, 0x10, 24),
+	CYGNUS_PIN_DESC(42, "sc0_clk", 1, 0x10, 22),
+	CYGNUS_PIN_DESC(43, "sc0_cmdvcc_l", 1, 0x10, 20),
+	CYGNUS_PIN_DESC(44, "sc0_detect", 1, 0x10, 18),
+	CYGNUS_PIN_DESC(45, "sc0_fcb", 1, 0x10, 16),
+	CYGNUS_PIN_DESC(46, "sc0_io", 1, 0x10, 14),
+	CYGNUS_PIN_DESC(47, "sc0_rst_l", 1, 0x10, 12),
+	CYGNUS_PIN_DESC(48, "sc1_clk", 1, 0x10, 10),
+	CYGNUS_PIN_DESC(49, "sc1_cmdvcc_l", 1, 0x10, 8),
+	CYGNUS_PIN_DESC(50, "sc1_detect", 1, 0x10, 6),
+	CYGNUS_PIN_DESC(51, "sc1_fcb", 1, 0x10, 4),
+	CYGNUS_PIN_DESC(52, "sc1_io", 1, 0x10, 2),
+	CYGNUS_PIN_DESC(53, "sc1_rst_l", 1, 0x10, 0),
+	CYGNUS_PIN_DESC(54, "spi0_clk", 1, 0x18, 10),
+	CYGNUS_PIN_DESC(55, "spi0_mosi", 1, 0x18, 6),
+	CYGNUS_PIN_DESC(56, "spi0_miso", 1, 0x18, 8),
+	CYGNUS_PIN_DESC(57, "spi0_ss", 1, 0x18, 4),
+	CYGNUS_PIN_DESC(58, "spi1_clk", 1, 0x18, 2),
+	CYGNUS_PIN_DESC(59, "spi1_mosi", 1, 0x1c, 30),
+	CYGNUS_PIN_DESC(60, "spi1_miso", 1, 0x18, 0),
+	CYGNUS_PIN_DESC(61, "spi1_ss", 1, 0x1c, 28),
+	CYGNUS_PIN_DESC(62, "spi2_clk", 1, 0x1c, 26),
+	CYGNUS_PIN_DESC(63, "spi2_mosi", 1, 0x1c, 22),
+	CYGNUS_PIN_DESC(64, "spi2_miso", 1, 0x1c, 24),
+	CYGNUS_PIN_DESC(65, "spi2_ss", 1, 0x1c, 20),
+	CYGNUS_PIN_DESC(66, "spi3_clk", 1, 0x1c, 18),
+	CYGNUS_PIN_DESC(67, "spi3_mosi", 1, 0x1c, 14),
+	CYGNUS_PIN_DESC(68, "spi3_miso", 1, 0x1c, 16),
+	CYGNUS_PIN_DESC(69, "spi3_ss", 1, 0x1c, 12),
+	CYGNUS_PIN_DESC(70, "uart0_cts", 1, 0x1c, 10),
+	CYGNUS_PIN_DESC(71, "uart0_rts", 1, 0x1c, 8),
+	CYGNUS_PIN_DESC(72, "uart0_rx", 1, 0x1c, 6),
+	CYGNUS_PIN_DESC(73, "uart0_tx", 1, 0x1c, 4),
+	CYGNUS_PIN_DESC(74, "uart1_cts", 1, 0x1c, 2),
+	CYGNUS_PIN_DESC(75, "uart1_dcd", 1, 0x1c, 0),
+	CYGNUS_PIN_DESC(76, "uart1_dsr", 1, 0x20, 14),
+	CYGNUS_PIN_DESC(77, "uart1_dtr", 1, 0x20, 12),
+	CYGNUS_PIN_DESC(78, "uart1_ri", 1, 0x20, 10),
+	CYGNUS_PIN_DESC(79, "uart1_rts", 1, 0x20, 8),
+	CYGNUS_PIN_DESC(80, "uart1_rx", 1, 0x20, 6),
+	CYGNUS_PIN_DESC(81, "uart1_tx", 1, 0x20, 4),
+	CYGNUS_PIN_DESC(82, "uart3_rx", 1, 0x20, 2),
+	CYGNUS_PIN_DESC(83, "uart3_tx", 1, 0x20, 0),
+	CYGNUS_PIN_DESC(84, "sdio1_clk_sdcard", 1, 0x14, 6),
+	CYGNUS_PIN_DESC(85, "sdio1_cmd", 1, 0x14, 4),
+	CYGNUS_PIN_DESC(86, "sdio1_data0", 1, 0x14, 2),
+	CYGNUS_PIN_DESC(87, "sdio1_data1", 1, 0x14, 0),
+	CYGNUS_PIN_DESC(88, "sdio1_data2", 1, 0x18, 30),
+	CYGNUS_PIN_DESC(89, "sdio1_data3", 1, 0x18, 28),
+	CYGNUS_PIN_DESC(90, "sdio1_wp_n", 1, 0x18, 24),
+	CYGNUS_PIN_DESC(91, "sdio1_card_rst", 1, 0x14, 10),
+	CYGNUS_PIN_DESC(92, "sdio1_led_on", 1, 0x18, 26),
+	CYGNUS_PIN_DESC(93, "sdio1_cd", 1, 0x14, 8),
+	CYGNUS_PIN_DESC(94, "sdio0_clk_sdcard", 1, 0x14, 26),
+	CYGNUS_PIN_DESC(95, "sdio0_cmd", 1, 0x14, 24),
+	CYGNUS_PIN_DESC(96, "sdio0_data0", 1, 0x14, 22),
+	CYGNUS_PIN_DESC(97, "sdio0_data1", 1, 0x14, 20),
+	CYGNUS_PIN_DESC(98, "sdio0_data2", 1, 0x14, 18),
+	CYGNUS_PIN_DESC(99, "sdio0_data3", 1, 0x14, 16),
+	CYGNUS_PIN_DESC(100, "sdio0_wp_n", 1, 0x14, 12),
+	CYGNUS_PIN_DESC(101, "sdio0_card_rst", 1, 0x14, 30),
+	CYGNUS_PIN_DESC(102, "sdio0_led_on", 1, 0x14, 14),
+	CYGNUS_PIN_DESC(103, "sdio0_cd", 1, 0x14, 28),
+	CYGNUS_PIN_DESC(104, "sflash_clk", 1, 0x18, 22),
+	CYGNUS_PIN_DESC(105, "sflash_cs_l", 1, 0x18, 20),
+	CYGNUS_PIN_DESC(106, "sflash_mosi", 1, 0x18, 14),
+	CYGNUS_PIN_DESC(107, "sflash_miso", 1, 0x18, 16),
+	CYGNUS_PIN_DESC(108, "sflash_wp_n", 1, 0x18, 12),
+	CYGNUS_PIN_DESC(109, "sflash_hold_n", 1, 0x18, 18),
+	CYGNUS_PIN_DESC(110, "nand_ale", 1, 0xc, 30),
+	CYGNUS_PIN_DESC(111, "nand_ce0_l", 1, 0xc, 28),
+	CYGNUS_PIN_DESC(112, "nand_ce1_l", 1, 0xc, 26),
+	CYGNUS_PIN_DESC(113, "nand_cle", 1, 0xc, 24),
+	CYGNUS_PIN_DESC(114, "nand_dq0", 1, 0xc, 22),
+	CYGNUS_PIN_DESC(115, "nand_dq1", 1, 0xc, 20),
+	CYGNUS_PIN_DESC(116, "nand_dq2", 1, 0xc, 18),
+	CYGNUS_PIN_DESC(117, "nand_dq3", 1, 0xc, 16),
+	CYGNUS_PIN_DESC(118, "nand_dq4", 1, 0xc, 14),
+	CYGNUS_PIN_DESC(119, "nand_dq5", 1, 0xc, 12),
+	CYGNUS_PIN_DESC(120, "nand_dq6", 1, 0xc, 10),
+	CYGNUS_PIN_DESC(121, "nand_dq7", 1, 0xc, 8),
+	CYGNUS_PIN_DESC(122, "nand_rb_l", 1, 0xc, 6),
+	CYGNUS_PIN_DESC(123, "nand_re_l", 1, 0xc, 4),
+	CYGNUS_PIN_DESC(124, "nand_we_l", 1, 0xc, 2),
+	CYGNUS_PIN_DESC(125, "nand_wp_l", 1, 0xc, 0),
+	CYGNUS_PIN_DESC(126, "lcd_clac", 1, 0x4, 26),
+	CYGNUS_PIN_DESC(127, "lcd_clcp", 1, 0x4, 24),
+	CYGNUS_PIN_DESC(128, "lcd_cld0", 1, 0x4, 22),
+	CYGNUS_PIN_DESC(129, "lcd_cld1", 1, 0x4, 0),
+	CYGNUS_PIN_DESC(130, "lcd_cld10", 1, 0x4, 20),
+	CYGNUS_PIN_DESC(131, "lcd_cld11", 1, 0x4, 18),
+	CYGNUS_PIN_DESC(132, "lcd_cld12", 1, 0x4, 16),
+	CYGNUS_PIN_DESC(133, "lcd_cld13", 1, 0x4, 14),
+	CYGNUS_PIN_DESC(134, "lcd_cld14", 1, 0x4, 12),
+	CYGNUS_PIN_DESC(135, "lcd_cld15", 1, 0x4, 10),
+	CYGNUS_PIN_DESC(136, "lcd_cld16", 1, 0x4, 8),
+	CYGNUS_PIN_DESC(137, "lcd_cld17", 1, 0x4, 6),
+	CYGNUS_PIN_DESC(138, "lcd_cld18", 1, 0x4, 4),
+	CYGNUS_PIN_DESC(139, "lcd_cld19", 1, 0x4, 2),
+	CYGNUS_PIN_DESC(140, "lcd_cld2", 1, 0x8, 22),
+	CYGNUS_PIN_DESC(141, "lcd_cld20", 1, 0x8, 30),
+	CYGNUS_PIN_DESC(142, "lcd_cld21", 1, 0x8, 28),
+	CYGNUS_PIN_DESC(143, "lcd_cld22", 1, 0x8, 26),
+	CYGNUS_PIN_DESC(144, "lcd_cld23", 1, 0x8, 24),
+	CYGNUS_PIN_DESC(145, "lcd_cld3", 1, 0x8, 20),
+	CYGNUS_PIN_DESC(146, "lcd_cld4", 1, 0x8, 18),
+	CYGNUS_PIN_DESC(147, "lcd_cld5", 1, 0x8, 16),
+	CYGNUS_PIN_DESC(148, "lcd_cld6", 1, 0x8, 14),
+	CYGNUS_PIN_DESC(149, "lcd_cld7", 1, 0x8, 12),
+	CYGNUS_PIN_DESC(150, "lcd_cld8", 1, 0x8, 10),
+	CYGNUS_PIN_DESC(151, "lcd_cld9", 1, 0x8, 8),
+	CYGNUS_PIN_DESC(152, "lcd_clfp", 1, 0x8, 6),
+	CYGNUS_PIN_DESC(153, "lcd_clle", 1, 0x8, 4),
+	CYGNUS_PIN_DESC(154, "lcd_cllp", 1, 0x8, 2),
+	CYGNUS_PIN_DESC(155, "lcd_clpower", 1, 0x8, 0),
+	CYGNUS_PIN_DESC(156, "camera_vsync", 1, 0x4, 30),
+	CYGNUS_PIN_DESC(157, "camera_trigger", 1, 0x0, 0),
+	CYGNUS_PIN_DESC(158, "camera_strobe", 1, 0x0, 2),
+	CYGNUS_PIN_DESC(159, "camera_standby", 1, 0x0, 4),
+	CYGNUS_PIN_DESC(160, "camera_reset_n", 1, 0x0, 6),
+	CYGNUS_PIN_DESC(161, "camera_pixdata9", 1, 0x0, 8),
+	CYGNUS_PIN_DESC(162, "camera_pixdata8", 1, 0x0, 10),
+	CYGNUS_PIN_DESC(163, "camera_pixdata7", 1, 0x0, 12),
+	CYGNUS_PIN_DESC(164, "camera_pixdata6", 1, 0x0, 14),
+	CYGNUS_PIN_DESC(165, "camera_pixdata5", 1, 0x0, 16),
+	CYGNUS_PIN_DESC(166, "camera_pixdata4", 1, 0x0, 18),
+	CYGNUS_PIN_DESC(167, "camera_pixdata3", 1, 0x0, 20),
+	CYGNUS_PIN_DESC(168, "camera_pixdata2", 1, 0x0, 22),
+	CYGNUS_PIN_DESC(169, "camera_pixdata1", 1, 0x0, 24),
+	CYGNUS_PIN_DESC(170, "camera_pixdata0", 1, 0x0, 26),
+	CYGNUS_PIN_DESC(171, "camera_pixclk", 1, 0x0, 28),
+	CYGNUS_PIN_DESC(172, "camera_hsync", 1, 0x0, 30),
+	CYGNUS_PIN_DESC(173, "camera_pll_ref_clk", 0, 0, 0),
+	CYGNUS_PIN_DESC(174, "usb_id_indication", 0, 0, 0),
+	CYGNUS_PIN_DESC(175, "usb_vbus_indication", 0, 0, 0),
+	CYGNUS_PIN_DESC(176, "gpio0_3p3", 0, 0, 0),
+	CYGNUS_PIN_DESC(177, "gpio1_3p3", 0, 0, 0),
+	CYGNUS_PIN_DESC(178, "gpio2_3p3", 0, 0, 0),
+	CYGNUS_PIN_DESC(179, "gpio3_3p3", 0, 0, 0),
+};
+
+/*
+ * List of groups of pins
+ */
+static const unsigned bsc1_pins[] = { 8, 9 };
+static const unsigned pcie_clkreq_pins[] = { 8, 9 };
+
+static const unsigned i2s2_0_pins[] = { 12 };
+static const unsigned i2s2_1_pins[] = { 13 };
+static const unsigned i2s2_2_pins[] = { 14 };
+static const unsigned i2s2_3_pins[] = { 15 };
+static const unsigned i2s2_4_pins[] = { 16 };
+
+static const unsigned pwm4_pins[] = { 17 };
+static const unsigned pwm5_pins[] = { 18 };
+
+static const unsigned key0_pins[] = { 20 };
+static const unsigned key1_pins[] = { 21 };
+static const unsigned key2_pins[] = { 22 };
+static const unsigned key3_pins[] = { 23 };
+static const unsigned key4_pins[] = { 24 };
+static const unsigned key5_pins[] = { 25 };
+
+static const unsigned key6_pins[] = { 26 };
+static const unsigned audio_dte0_pins[] = { 26 };
+
+static const unsigned key7_pins[] = { 27 };
+static const unsigned audio_dte1_pins[] = { 27 };
+
+static const unsigned key8_pins[] = { 28 };
+static const unsigned key9_pins[] = { 29 };
+static const unsigned key10_pins[] = { 30 };
+static const unsigned key11_pins[] = { 31 };
+static const unsigned key12_pins[] = { 32 };
+static const unsigned key13_pins[] = { 33 };
+
+static const unsigned key14_pins[] = { 34 };
+static const unsigned audio_dte2_pins[] = { 34 };
+
+static const unsigned key15_pins[] = { 35 };
+static const unsigned audio_dte3_pins[] = { 35 };
+
+static const unsigned pwm0_pins[] = { 38 };
+static const unsigned pwm1_pins[] = { 39 };
+static const unsigned pwm2_pins[] = { 40 };
+static const unsigned pwm3_pins[] = { 41 };
+
+static const unsigned sdio0_pins[] = { 94, 95, 96, 97, 98, 99 };
+
+static const unsigned smart_card0_pins[] = { 42, 43, 44, 46, 47 };
+static const unsigned i2s0_0_pins[] = { 42, 43, 44, 46 };
+static const unsigned spdif_pins[] = { 47 };
+
+static const unsigned smart_card1_pins[] = { 48, 49, 50, 52, 53 };
+static const unsigned i2s1_0_pins[] = { 48, 49, 50, 52 };
+
+static const unsigned spi0_pins[] = { 54, 55, 56, 57 };
+
+static const unsigned spi1_pins[] = { 58, 59, 60, 61 };
+
+static const unsigned spi2_pins[] = { 62, 63, 64, 65 };
+
+static const unsigned spi3_pins[] = { 66, 67, 68, 69 };
+static const unsigned sw_led0_0_pins[] = { 66, 67, 68, 69 };
+
+static const unsigned d1w_pins[] = { 10, 11 };
+static const unsigned uart4_pins[] = { 10, 11 };
+static const unsigned sw_led2_0_pins[] = { 10, 11 };
+
+static const unsigned lcd_pins[] = { 126, 127, 128, 129, 130, 131, 132, 133,
+	134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147,
+	148, 149, 150, 151, 152, 153, 154, 155 };
+static const unsigned sram_0_pins[] = { 126, 127, 128, 129, 130, 131, 132, 133,
+	134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147,
+	148, 149, 150, 151, 152, 153, 154, 155 };
+static const unsigned spi5_pins[] = { 141, 142, 143, 144 };
+
+static const unsigned uart0_pins[] = { 70, 71, 72, 73 };
+static const unsigned sw_led0_1_pins[] = { 70, 71, 72, 73 };
+
+static const unsigned uart1_dte_pins[] = { 75, 76, 77, 78 };
+static const unsigned uart2_pins[] = { 75, 76, 77, 78 };
+
+static const unsigned uart1_pins[] = { 74, 79, 80, 81 };
+
+static const unsigned uart3_pins[] = { 82, 83 };
+
+static const unsigned qspi_0_pins[] = { 104, 105, 106, 107 };
+
+static const unsigned nand_pins[] = { 110, 111, 112, 113, 114, 115, 116, 117,
+	118, 119, 120, 121, 122, 123, 124, 125 };
+
+static const unsigned sdio0_cd_pins[] = { 103 };
+
+static const unsigned sdio0_mmc_pins[] = { 100, 101, 102 };
+
+static const unsigned sdio1_data_0_pins[] = { 86, 87 };
+static const unsigned can0_pins[] = { 86, 87 };
+static const unsigned spi4_0_pins[] = { 86, 87 };
+
+static const unsigned sdio1_data_1_pins[] = { 88, 89 };
+static const unsigned can1_pins[] = { 88, 89 };
+static const unsigned spi4_1_pins[] = { 88, 89 };
+
+static const unsigned sdio1_cd_pins[] = { 93 };
+
+static const unsigned sdio1_led_pins[] = { 84, 85 };
+static const unsigned sw_led2_1_pins[] = { 84, 85 };
+
+static const unsigned sdio1_mmc_pins[] = { 90, 91, 92 };
+
+static const unsigned cam_led_pins[] = { 156, 157, 158, 159, 160 };
+static const unsigned sw_led1_pins[] = { 156, 157, 158, 159 };
+
+static const unsigned cam_0_pins[] = { 169, 170, 171, 169, 170 };
+
+static const unsigned cam_1_pins[] = { 161, 162, 163, 164, 165, 166, 167,
+	168 };
+static const unsigned sram_1_pins[] = { 161, 162, 163, 164, 165, 166, 167,
+	168 };
+
+static const unsigned qspi_1_pins[] = { 108, 109 };
+
+static const unsigned smart_card0_fcb_pins[] = { 45 };
+static const unsigned i2s0_1_pins[] = { 45 };
+
+static const unsigned smart_card1_fcb_pins[] = { 51 };
+static const unsigned i2s1_1_pins[] = { 51 };
+
+static const unsigned gpio0_3p3_pins[] = { 176 };
+static const unsigned usb0_oc_pins[] = { 176 };
+
+static const unsigned gpio1_3p3_pins[] = { 177 };
+static const unsigned usb1_oc_pins[] = { 177 };
+
+static const unsigned gpio2_3p3_pins[] = { 178 };
+static const unsigned usb2_oc_pins[] = { 178 };
+
+#define CYGNUS_PIN_GROUP(group_name, off, sh, al)	\
+{							\
+	.name = __stringify(group_name) "_grp",		\
+	.pins = group_name ## _pins,			\
+	.num_pins = ARRAY_SIZE(group_name ## _pins),	\
+	.mux = {					\
+		.offset = off,				\
+		.shift = sh,				\
+		.alt = al,				\
+	}						\
+}
+
+/*
+ * List of Cygnus pin groups
+ */
+static const struct cygnus_pin_group cygnus_pin_groups[] = {
+	CYGNUS_PIN_GROUP(i2s2_0, 0x0, 0, 2),
+	CYGNUS_PIN_GROUP(i2s2_1, 0x0, 4, 2),
+	CYGNUS_PIN_GROUP(i2s2_2, 0x0, 8, 2),
+	CYGNUS_PIN_GROUP(i2s2_3, 0x0, 12, 2),
+	CYGNUS_PIN_GROUP(i2s2_4, 0x0, 16, 2),
+	CYGNUS_PIN_GROUP(pwm4, 0x0, 20, 0),
+	CYGNUS_PIN_GROUP(pwm5, 0x0, 24, 2),
+	CYGNUS_PIN_GROUP(key0, 0x4, 0, 1),
+	CYGNUS_PIN_GROUP(key1, 0x4, 4, 1),
+	CYGNUS_PIN_GROUP(key2, 0x4, 8, 1),
+	CYGNUS_PIN_GROUP(key3, 0x4, 12, 1),
+	CYGNUS_PIN_GROUP(key4, 0x4, 16, 1),
+	CYGNUS_PIN_GROUP(key5, 0x4, 20, 1),
+	CYGNUS_PIN_GROUP(key6, 0x4, 24, 1),
+	CYGNUS_PIN_GROUP(audio_dte0, 0x4, 24, 2),
+	CYGNUS_PIN_GROUP(key7, 0x4, 28, 1),
+	CYGNUS_PIN_GROUP(audio_dte1, 0x4, 28, 2),
+	CYGNUS_PIN_GROUP(key8, 0x8, 0, 1),
+	CYGNUS_PIN_GROUP(key9, 0x8, 4, 1),
+	CYGNUS_PIN_GROUP(key10, 0x8, 8, 1),
+	CYGNUS_PIN_GROUP(key11, 0x8, 12, 1),
+	CYGNUS_PIN_GROUP(key12, 0x8, 16, 1),
+	CYGNUS_PIN_GROUP(key13, 0x8, 20, 1),
+	CYGNUS_PIN_GROUP(key14, 0x8, 24, 1),
+	CYGNUS_PIN_GROUP(audio_dte2, 0x8, 24, 2),
+	CYGNUS_PIN_GROUP(key15, 0x8, 28, 1),
+	CYGNUS_PIN_GROUP(audio_dte3, 0x8, 28, 2),
+	CYGNUS_PIN_GROUP(pwm0, 0xc, 0, 0),
+	CYGNUS_PIN_GROUP(pwm1, 0xc, 4, 0),
+	CYGNUS_PIN_GROUP(pwm2, 0xc, 8, 0),
+	CYGNUS_PIN_GROUP(pwm3, 0xc, 12, 0),
+	CYGNUS_PIN_GROUP(sdio0, 0xc, 16, 0),
+	CYGNUS_PIN_GROUP(smart_card0, 0xc, 20, 0),
+	CYGNUS_PIN_GROUP(i2s0_0, 0xc, 20, 1),
+	CYGNUS_PIN_GROUP(spdif, 0xc, 20, 1),
+	CYGNUS_PIN_GROUP(smart_card1, 0xc, 24, 0),
+	CYGNUS_PIN_GROUP(i2s1_0, 0xc, 24, 1),
+	CYGNUS_PIN_GROUP(spi0, 0x10, 0, 0),
+	CYGNUS_PIN_GROUP(spi1, 0x10, 4, 0),
+	CYGNUS_PIN_GROUP(spi2, 0x10, 8, 0),
+	CYGNUS_PIN_GROUP(spi3, 0x10, 12, 0),
+	CYGNUS_PIN_GROUP(sw_led0_0, 0x10, 12, 2),
+	CYGNUS_PIN_GROUP(d1w, 0x10, 16, 0),
+	CYGNUS_PIN_GROUP(uart4, 0x10, 16, 1),
+	CYGNUS_PIN_GROUP(sw_led2_0, 0x10, 16, 2),
+	CYGNUS_PIN_GROUP(lcd, 0x10, 20, 0),
+	CYGNUS_PIN_GROUP(sram_0, 0x10, 20, 1),
+	CYGNUS_PIN_GROUP(spi5, 0x10, 20, 2),
+	CYGNUS_PIN_GROUP(uart0, 0x14, 0, 0),
+	CYGNUS_PIN_GROUP(sw_led0_1, 0x14, 0, 2),
+	CYGNUS_PIN_GROUP(uart1_dte, 0x14, 4, 0),
+	CYGNUS_PIN_GROUP(uart2, 0x14, 4, 1),
+	CYGNUS_PIN_GROUP(uart1, 0x14, 8, 0),
+	CYGNUS_PIN_GROUP(uart3, 0x14, 12, 0),
+	CYGNUS_PIN_GROUP(qspi_0, 0x14, 16, 0),
+	CYGNUS_PIN_GROUP(nand, 0x14, 20, 0),
+	CYGNUS_PIN_GROUP(sdio0_cd, 0x18, 0, 0),
+	CYGNUS_PIN_GROUP(sdio0_mmc, 0x18, 4, 0),
+	CYGNUS_PIN_GROUP(sdio1_data_0, 0x18, 8, 0),
+	CYGNUS_PIN_GROUP(can0, 0x18, 8, 1),
+	CYGNUS_PIN_GROUP(spi4_0, 0x18, 8, 2),
+	CYGNUS_PIN_GROUP(sdio1_data_1, 0x18, 12, 0),
+	CYGNUS_PIN_GROUP(can1, 0x18, 12, 1),
+	CYGNUS_PIN_GROUP(spi4_1, 0x18, 12, 2),
+	CYGNUS_PIN_GROUP(sdio1_cd, 0x18, 16, 0),
+	CYGNUS_PIN_GROUP(sdio1_led, 0x18, 20, 0),
+	CYGNUS_PIN_GROUP(sw_led2_1, 0x18, 20, 2),
+	CYGNUS_PIN_GROUP(sdio1_mmc, 0x18, 24, 0),
+	CYGNUS_PIN_GROUP(cam_led, 0x1c, 0, 0),
+	CYGNUS_PIN_GROUP(sw_led1, 0x1c, 0, 1),
+	CYGNUS_PIN_GROUP(cam_0, 0x1c, 4, 0),
+	CYGNUS_PIN_GROUP(cam_1, 0x1c, 8, 0),
+	CYGNUS_PIN_GROUP(sram_1, 0x1c, 8, 1),
+	CYGNUS_PIN_GROUP(qspi_1, 0x1c, 12, 0),
+	CYGNUS_PIN_GROUP(bsc1, 0x1c, 16, 0),
+	CYGNUS_PIN_GROUP(pcie_clkreq, 0x1c, 16, 1),
+	CYGNUS_PIN_GROUP(smart_card0_fcb, 0x20, 0, 0),
+	CYGNUS_PIN_GROUP(i2s0_1, 0x20, 0, 1),
+	CYGNUS_PIN_GROUP(smart_card1_fcb, 0x20, 4, 0),
+	CYGNUS_PIN_GROUP(i2s1_1, 0x20, 4, 1),
+	CYGNUS_PIN_GROUP(gpio0_3p3, 0x28, 0, 0),
+	CYGNUS_PIN_GROUP(usb0_oc, 0x28, 0, 1),
+	CYGNUS_PIN_GROUP(gpio1_3p3, 0x28, 4, 0),
+	CYGNUS_PIN_GROUP(usb1_oc, 0x28, 4, 1),
+	CYGNUS_PIN_GROUP(gpio2_3p3, 0x28, 8, 0),
+	CYGNUS_PIN_GROUP(usb2_oc, 0x28, 8, 1),
+};
+
+/*
+ * List of groups supported by functions
+ */
+static const char * const i2s0_grps[] = { "i2s0_0_grp", "i2s0_1_grp" };
+static const char * const i2s1_grps[] = { "i2s1_0_grp", "i2s1_1_grp" };
+static const char * const i2s2_grps[] = { "i2s2_0_grp", "i2s2_1_grp",
+	"i2s2_2_grp", "i2s2_3_grp", "i2s2_4_grp" };
+static const char * const spdif_grps[] = { "spdif_grp" };
+static const char * const pwm0_grps[] = { "pwm0_grp" };
+static const char * const pwm1_grps[] = { "pwm1_grp" };
+static const char * const pwm2_grps[] = { "pwm2_grp" };
+static const char * const pwm3_grps[] = { "pwm3_grp" };
+static const char * const pwm4_grps[] = { "pwm4_grp" };
+static const char * const pwm5_grps[] = { "pwm5_grp" };
+static const char * const key_grps[] = { "key0_grp", "key1_grp", "key2_grp",
+	"key3_grp", "key4_grp", "key5_grp", "key6_grp", "key7_grp", "key8_grp",
+	"key9_grp", "key10_grp", "key11_grp", "key12_grp", "key13_grp",
+	"key14_grp", "key15_grp" };
+static const char * const audio_dte_grps[] = { "audio_dte0_grp",
+	"audio_dte1_grp", "audio_dte2_grp", "audio_dte3_grp" };
+static const char * const smart_card0_grps[] = { "smart_card0_grp",
+	"smart_card0_fcb_grp" };
+static const char * const smart_card1_grps[] = { "smart_card1_grp",
+	"smart_card1_fcb_grp" };
+static const char * const spi0_grps[] = { "spi0_grp" };
+static const char * const spi1_grps[] = { "spi1_grp" };
+static const char * const spi2_grps[] = { "spi2_grp" };
+static const char * const spi3_grps[] = { "spi3_grp" };
+static const char * const spi4_grps[] = { "spi4_0_grp", "spi4_1_grp" };
+static const char * const spi5_grps[] = { "spi5_grp" };
+
+static const char * const sw_led0_grps[] = { "sw_led0_0_grp",
+	"sw_led0_1_grp" };
+static const char * const sw_led1_grps[] = { "sw_led1_grp" };
+static const char * const sw_led2_grps[] = { "sw_led2_0_grp",
+	"sw_led2_1_grp" };
+static const char * const d1w_grps[] = { "d1w_grp" };
+static const char * const lcd_grps[] = { "lcd_grp" };
+static const char * const sram_grps[] = { "sram_0_grp", "sram_1_grp" };
+
+static const char * const uart0_grps[] = { "uart0_grp" };
+static const char * const uart1_grps[] = { "uart1_grp", "uart1_dte_grp" };
+static const char * const uart2_grps[] = { "uart2_grp" };
+static const char * const uart3_grps[] = { "uart3_grp" };
+static const char * const uart4_grps[] = { "uart4_grp" };
+static const char * const qspi_grps[] = { "qspi_0_grp", "qspi_1_grp" };
+static const char * const nand_grps[] = { "nand_grp" };
+static const char * const sdio0_grps[] = { "sdio0_grp", "sdio0_cd_grp",
+	"sdio0_mmc_grp" };
+static const char * const sdio1_grps[] = { "sdio1_data_0_grp",
+	"sdio1_data_1_grp", "sdio1_cd_grp", "sdio1_led_grp", "sdio1_mmc_grp" };
+static const char * const can0_grps[] = { "can0_grp" };
+static const char * const can1_grps[] = { "can1_grp" };
+static const char * const cam_grps[] = { "cam_led_grp", "cam_0_grp",
+	"cam_1_grp" };
+static const char * const bsc1_grps[] = { "bsc1_grp" };
+static const char * const pcie_clkreq_grps[] = { "pcie_clkreq_grp" };
+static const char * const usb0_oc_grps[] = { "usb0_oc_grp" };
+static const char * const usb1_oc_grps[] = { "usb1_oc_grp" };
+static const char * const usb2_oc_grps[] = { "usb2_oc_grp" };
+
+#define CYGNUS_PIN_FUNCTION(func)				\
+{								\
+	.name = #func,						\
+	.groups = func ## _grps,				\
+	.num_groups = ARRAY_SIZE(func ## _grps),		\
+}
+
+/*
+ * List of supported functions in Cygnus
+ */
+static const struct cygnus_pin_function cygnus_pin_functions[] = {
+	CYGNUS_PIN_FUNCTION(i2s0),
+	CYGNUS_PIN_FUNCTION(i2s1),
+	CYGNUS_PIN_FUNCTION(i2s2),
+	CYGNUS_PIN_FUNCTION(spdif),
+	CYGNUS_PIN_FUNCTION(pwm0),
+	CYGNUS_PIN_FUNCTION(pwm1),
+	CYGNUS_PIN_FUNCTION(pwm2),
+	CYGNUS_PIN_FUNCTION(pwm3),
+	CYGNUS_PIN_FUNCTION(pwm4),
+	CYGNUS_PIN_FUNCTION(pwm5),
+	CYGNUS_PIN_FUNCTION(key),
+	CYGNUS_PIN_FUNCTION(audio_dte),
+	CYGNUS_PIN_FUNCTION(smart_card0),
+	CYGNUS_PIN_FUNCTION(smart_card1),
+	CYGNUS_PIN_FUNCTION(spi0),
+	CYGNUS_PIN_FUNCTION(spi1),
+	CYGNUS_PIN_FUNCTION(spi2),
+	CYGNUS_PIN_FUNCTION(spi3),
+	CYGNUS_PIN_FUNCTION(spi4),
+	CYGNUS_PIN_FUNCTION(spi5),
+	CYGNUS_PIN_FUNCTION(sw_led0),
+	CYGNUS_PIN_FUNCTION(sw_led1),
+	CYGNUS_PIN_FUNCTION(sw_led2),
+	CYGNUS_PIN_FUNCTION(d1w),
+	CYGNUS_PIN_FUNCTION(lcd),
+	CYGNUS_PIN_FUNCTION(sram),
+	CYGNUS_PIN_FUNCTION(uart0),
+	CYGNUS_PIN_FUNCTION(uart1),
+	CYGNUS_PIN_FUNCTION(uart2),
+	CYGNUS_PIN_FUNCTION(uart3),
+	CYGNUS_PIN_FUNCTION(uart4),
+	CYGNUS_PIN_FUNCTION(qspi),
+	CYGNUS_PIN_FUNCTION(nand),
+	CYGNUS_PIN_FUNCTION(sdio0),
+	CYGNUS_PIN_FUNCTION(sdio1),
+	CYGNUS_PIN_FUNCTION(can0),
+	CYGNUS_PIN_FUNCTION(can1),
+	CYGNUS_PIN_FUNCTION(cam),
+	CYGNUS_PIN_FUNCTION(bsc1),
+	CYGNUS_PIN_FUNCTION(pcie_clkreq),
+	CYGNUS_PIN_FUNCTION(usb0_oc),
+	CYGNUS_PIN_FUNCTION(usb1_oc),
+	CYGNUS_PIN_FUNCTION(usb2_oc),
+};
+
+static int cygnus_get_groups_count(struct pinctrl_dev *pctrl_dev)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pinctrl->num_groups;
+}
+
+static const char *cygnus_get_group_name(struct pinctrl_dev *pctrl_dev,
+					 unsigned selector)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pinctrl->groups[selector].name;
+}
+
+static int cygnus_get_group_pins(struct pinctrl_dev *pctrl_dev,
+				 unsigned selector, const unsigned **pins,
+				 unsigned *num_pins)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	*pins = pinctrl->groups[selector].pins;
+	*num_pins = pinctrl->groups[selector].num_pins;
+
+	return 0;
+}
+
+static void cygnus_pin_dbg_show(struct pinctrl_dev *pctrl_dev,
+				struct seq_file *s, unsigned offset)
+{
+	seq_printf(s, " %s", dev_name(pctrl_dev->dev));
+}
+
+static const struct pinctrl_ops cygnus_pinctrl_ops = {
+	.get_groups_count = cygnus_get_groups_count,
+	.get_group_name = cygnus_get_group_name,
+	.get_group_pins = cygnus_get_group_pins,
+	.pin_dbg_show = cygnus_pin_dbg_show,
+	.dt_node_to_map = pinconf_generic_dt_node_to_map_group,
+	.dt_free_map = pinctrl_utils_dt_free_map,
+};
+
+static int cygnus_get_functions_count(struct pinctrl_dev *pctrl_dev)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pinctrl->num_functions;
+}
+
+static const char *cygnus_get_function_name(struct pinctrl_dev *pctrl_dev,
+					    unsigned selector)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pinctrl->functions[selector].name;
+}
+
+static int cygnus_get_function_groups(struct pinctrl_dev *pctrl_dev,
+				      unsigned selector,
+				      const char * const **groups,
+				      unsigned * const num_groups)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	*groups = pinctrl->functions[selector].groups;
+	*num_groups = pinctrl->functions[selector].num_groups;
+
+	return 0;
+}
+
+static int cygnus_pinmux_set(struct cygnus_pinctrl *pinctrl,
+			     const struct cygnus_pin_function *func,
+			     const struct cygnus_pin_group *grp,
+			     struct cygnus_mux_log *mux_log)
+{
+	const struct cygnus_mux *mux = &grp->mux;
+	int i;
+	u32 val, mask = 0x7;
+	unsigned long flags;
+
+	for (i = 0; i < CYGNUS_NUM_IOMUX; i++) {
+		if (mux->offset != mux_log[i].mux.offset ||
+		    mux->shift != mux_log[i].mux.shift)
+			continue;
+
+		/* match found if we reach here */
+
+		/* if this is a new configuration, just do it! */
+		if (!mux_log[i].is_configured)
+			break;
+
+		/*
+		 * IOMUX has been configured previously and one is trying to
+		 * configure it to a different function
+		 */
+		if (mux_log[i].mux.alt != mux->alt) {
+			dev_err(pinctrl->dev,
+				"double configuration error detected!\n");
+			dev_err(pinctrl->dev, "func:%s grp:%s\n",
+				func->name, grp->name);
+			return -EINVAL;
+		} else {
+			/*
+			 * One tries to configure it to the same function.
+			 * Just quit and don't bother
+			 */
+			return 0;
+		}
+	}
+
+	mux_log[i].mux.alt = mux->alt;
+	mux_log[i].is_configured = true;
+
+	spin_lock_irqsave(&pinctrl->lock, flags);
+
+	val = readl(pinctrl->base0 + grp->mux.offset);
+	val &= ~(mask << grp->mux.shift);
+	val |= grp->mux.alt << grp->mux.shift;
+	writel(val, pinctrl->base0 + grp->mux.offset);
+
+	spin_unlock_irqrestore(&pinctrl->lock, flags);
+
+	return 0;
+}
+
+static int cygnus_pinmux_set_mux(struct pinctrl_dev *pctrl_dev,
+				 unsigned func_select, unsigned grp_select)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+	const struct cygnus_pin_function *func =
+		&pinctrl->functions[func_select];
+	const struct cygnus_pin_group *grp = &pinctrl->groups[grp_select];
+
+	dev_dbg(pctrl_dev->dev, "func:%u name:%s grp:%u name:%s\n",
+		func_select, func->name, grp_select, grp->name);
+
+	dev_dbg(pctrl_dev->dev, "offset:0x%08x shift:%u alt:%u\n",
+		grp->mux.offset, grp->mux.shift, grp->mux.alt);
+
+	return cygnus_pinmux_set(pinctrl, func, grp, pinctrl->mux_log);
+}
+
+static int cygnus_gpio_request_enable(struct pinctrl_dev *pctrl_dev,
+				      struct pinctrl_gpio_range *range,
+				      unsigned pin)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+	const struct cygnus_gpio_mux *mux = pctrl_dev->desc->pins[pin].drv_data;
+	u32 val;
+	unsigned long flags;
+
+	/* not all pins support GPIO pinmux override */
+	if (!mux->is_supported)
+		return -ENOTSUPP;
+
+	spin_lock_irqsave(&pinctrl->lock, flags);
+
+	val = readl(pinctrl->base1 + mux->offset);
+	val |= 0x3 << mux->shift;
+	writel(val, pinctrl->base1 + mux->offset);
+
+	spin_unlock_irqrestore(&pinctrl->lock, flags);
+
+	dev_dbg(pctrl_dev->dev,
+		"gpio request enable pin=%u offset=0x%x shift=%u\n",
+		pin, mux->offset, mux->shift);
+
+	return 0;
+}
+
+static void cygnus_gpio_disable_free(struct pinctrl_dev *pctrl_dev,
+				     struct pinctrl_gpio_range *range,
+				     unsigned pin)
+{
+	struct cygnus_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+	struct cygnus_gpio_mux *mux = pctrl_dev->desc->pins[pin].drv_data;
+	u32 val;
+	unsigned long flags;
+
+	if (!mux->is_supported)
+		return;
+
+	spin_lock_irqsave(&pinctrl->lock, flags);
+
+	val = readl(pinctrl->base1 + mux->offset);
+	val &= ~(0x3 << mux->shift);
+	writel(val, pinctrl->base1 + mux->offset);
+
+	spin_unlock_irqrestore(&pinctrl->lock, flags);
+
+	dev_err(pctrl_dev->dev,
+		"gpio disable free pin=%u offset=0x%x shift=%u\n",
+		pin, mux->offset, mux->shift);
+}
+
+static const struct pinmux_ops cygnus_pinmux_ops = {
+	.get_functions_count = cygnus_get_functions_count,
+	.get_function_name = cygnus_get_function_name,
+	.get_function_groups = cygnus_get_function_groups,
+	.set_mux = cygnus_pinmux_set_mux,
+	.gpio_request_enable = cygnus_gpio_request_enable,
+	.gpio_disable_free = cygnus_gpio_disable_free,
+};
+
+static struct pinctrl_desc cygnus_pinctrl_desc = {
+	.name = "cygnus-pinmux",
+	.pctlops = &cygnus_pinctrl_ops,
+	.pmxops = &cygnus_pinmux_ops,
+};
+
+static int cygnus_mux_log_init(struct cygnus_pinctrl *pinctrl)
+{
+	struct cygnus_mux_log *log;
+	unsigned int i, j;
+
+	pinctrl->mux_log = devm_kcalloc(pinctrl->dev, CYGNUS_NUM_IOMUX,
+					sizeof(struct cygnus_mux_log),
+					GFP_KERNEL);
+	if (!pinctrl->mux_log)
+		return -ENOMEM;
+
+	log = pinctrl->mux_log;
+	for (i = 0; i < CYGNUS_NUM_IOMUX_REGS; i++) {
+		for (j = 0; j < CYGNUS_NUM_MUX_PER_REG; j++) {
+			log = &pinctrl->mux_log[i * CYGNUS_NUM_MUX_PER_REG
+				+ j];
+			log->mux.offset = i * 4;
+			log->mux.shift = j * 4;
+			log->mux.alt = 0;
+			log->is_configured = false;
+		}
+	}
+
+	return 0;
+}
+
+static int cygnus_pinmux_probe(struct platform_device *pdev)
+{
+	struct cygnus_pinctrl *pinctrl;
+	struct resource *res;
+	int i, ret;
+	struct pinctrl_pin_desc *pins;
+	unsigned num_pins = ARRAY_SIZE(cygnus_pins);
+
+	pinctrl = devm_kzalloc(&pdev->dev, sizeof(*pinctrl), GFP_KERNEL);
+	if (!pinctrl)
+		return -ENOMEM;
+
+	pinctrl->dev = &pdev->dev;
+	platform_set_drvdata(pdev, pinctrl);
+	spin_lock_init(&pinctrl->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pinctrl->base0 = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pinctrl->base0)) {
+		dev_err(&pdev->dev, "unable to map I/O space\n");
+		return PTR_ERR(pinctrl->base0);
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	pinctrl->base1 = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pinctrl->base1)) {
+		dev_err(&pdev->dev, "unable to map I/O space\n");
+		return PTR_ERR(pinctrl->base1);
+	}
+
+	ret = cygnus_mux_log_init(pinctrl);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to initialize IOMUX log\n");
+		return ret;
+	}
+
+	pins = devm_kcalloc(&pdev->dev, num_pins, sizeof(*pins), GFP_KERNEL);
+	if (!pins)
+		return -ENOMEM;
+
+	for (i = 0; i < num_pins; i++) {
+		pins[i].number = cygnus_pins[i].pin;
+		pins[i].name = cygnus_pins[i].name;
+		pins[i].drv_data = &cygnus_pins[i].gpio_mux;
+	}
+
+	pinctrl->groups = cygnus_pin_groups;
+	pinctrl->num_groups = ARRAY_SIZE(cygnus_pin_groups);
+	pinctrl->functions = cygnus_pin_functions;
+	pinctrl->num_functions = ARRAY_SIZE(cygnus_pin_functions);
+	cygnus_pinctrl_desc.pins = pins;
+	cygnus_pinctrl_desc.npins = num_pins;
+
+	pinctrl->pctl = pinctrl_register(&cygnus_pinctrl_desc, &pdev->dev,
+			pinctrl);
+	if (!pinctrl->pctl) {
+		dev_err(&pdev->dev, "unable to register Cygnus IOMUX pinctrl\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id cygnus_pinmux_of_match[] = {
+	{ .compatible = "brcm,cygnus-pinmux" },
+	{ }
+};
+
+static struct platform_driver cygnus_pinmux_driver = {
+	.driver = {
+		.name = "cygnus-pinmux",
+		.of_match_table = cygnus_pinmux_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe = cygnus_pinmux_probe,
+};
+
+static int __init cygnus_pinmux_init(void)
+{
+	return platform_driver_register(&cygnus_pinmux_driver);
+}
+arch_initcall(cygnus_pinmux_init);
+
+MODULE_AUTHOR("Ray Jui <rjui@broadcom.com>");
+MODULE_DESCRIPTION("Broadcom Cygnus IOMUX driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/freescale/pinctrl-imx.c b/drivers/pinctrl/freescale/pinctrl-imx.c
index 448f10986c28..e261f1cf85c6 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx.c
@@ -542,10 +542,13 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 		struct imx_pin_reg *pin_reg;
 		struct imx_pin *pin = &grp->pins[i];
 
-		if (info->flags & SHARE_MUX_CONF_REG)
+		if (info->flags & SHARE_MUX_CONF_REG) {
 			conf_reg = mux_reg;
-		else
+		} else {
 			conf_reg = be32_to_cpu(*list++);
+			if (!conf_reg)
+				conf_reg = -1;
+		}
 
 		pin_id = mux_reg ? mux_reg / 4 : conf_reg / 4;
 		pin_reg = &info->pin_regs[pin_id];
@@ -645,7 +648,7 @@ int imx_pinctrl_probe(struct platform_device *pdev,
 {
 	struct imx_pinctrl *ipctl;
 	struct resource *res;
-	int ret;
+	int ret, i;
 
 	if (!info || !info->pins || !info->npins) {
 		dev_err(&pdev->dev, "wrong pinctrl info\n");
@@ -662,7 +665,11 @@ int imx_pinctrl_probe(struct platform_device *pdev,
 				      info->npins, GFP_KERNEL);
 	if (!info->pin_regs)
 		return -ENOMEM;
-	memset(info->pin_regs, 0xff, sizeof(*info->pin_regs) * info->npins);
+
+	for (i = 0; i < info->npins; i++) {
+		info->pin_regs[i].mux_reg = -1;
+		info->pin_regs[i].conf_reg = -1;
+	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	ipctl->base = devm_ioremap_resource(&pdev->dev, res);
diff --git a/drivers/pinctrl/freescale/pinctrl-vf610.c b/drivers/pinctrl/freescale/pinctrl-vf610.c
index fc86276892fd..37a037543d29 100644
--- a/drivers/pinctrl/freescale/pinctrl-vf610.c
+++ b/drivers/pinctrl/freescale/pinctrl-vf610.c
@@ -302,7 +302,7 @@ static struct imx_pinctrl_soc_info vf610_pinctrl_info = {
 	.flags = SHARE_MUX_CONF_REG,
 };
 
-static struct of_device_id vf610_pinctrl_of_match[] = {
+static const struct of_device_id vf610_pinctrl_of_match[] = {
 	{ .compatible = "fsl,vf610-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/intel/Kconfig b/drivers/pinctrl/intel/Kconfig
index b801d869e91c..fe5e07db0a95 100644
--- a/drivers/pinctrl/intel/Kconfig
+++ b/drivers/pinctrl/intel/Kconfig
@@ -25,3 +25,20 @@ config PINCTRL_CHERRYVIEW
 	help
 	  Cherryview/Braswell pinctrl driver provides an interface that
 	  allows configuring of SoC pins and using them as GPIOs.
+
+config PINCTRL_INTEL
+	tristate
+	select PINMUX
+	select PINCONF
+	select GENERIC_PINCONF
+	select GPIOLIB
+	select GPIOLIB_IRQCHIP
+
+config PINCTRL_SUNRISEPOINT
+	tristate "Intel Sunrisepoint pinctrl and GPIO driver"
+	depends on ACPI
+	select PINCTRL_INTEL
+	help
+	  Sunrisepoint is the PCH of Intel Skylake. This pinctrl driver
+	  provides an interface that allows configuring of PCH pins and
+	  using them as GPIOs.
diff --git a/drivers/pinctrl/intel/Makefile b/drivers/pinctrl/intel/Makefile
index 4c210e4139e2..fee756e1255b 100644
--- a/drivers/pinctrl/intel/Makefile
+++ b/drivers/pinctrl/intel/Makefile
@@ -2,3 +2,5 @@
 
 obj-$(CONFIG_PINCTRL_BAYTRAIL)		+= pinctrl-baytrail.o
 obj-$(CONFIG_PINCTRL_CHERRYVIEW)	+= pinctrl-cherryview.o
+obj-$(CONFIG_PINCTRL_INTEL)		+= pinctrl-intel.o
+obj-$(CONFIG_PINCTRL_SUNRISEPOINT)	+= pinctrl-sunrisepoint.o
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/intel/pinctrl-intel.c b/drivers/pinctrl/intel/pinctrl-intel.c
new file mode 100644
index 000000000000..00768e53deec
--- /dev/null
+++ b/drivers/pinctrl/intel/pinctrl-intel.c
@@ -0,0 +1,1149 @@
+/*
+ * Intel pinctrl/GPIO core driver.
+ *
+ * Copyright (C) 2015, Intel Corporation
+ * Authors: Mathias Nyman <mathias.nyman@linux.intel.com>
+ *          Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/acpi.h>
+#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+
+#include "pinctrl-intel.h"
+
+/* Maximum number of pads in each group */
+#define NPADS_IN_GPP			24
+
+/* Offset from regs */
+#define PADBAR				0x00c
+#define GPI_IS				0x100
+#define GPI_GPE_STS			0x140
+#define GPI_GPE_EN			0x160
+
+#define PADOWN_BITS			4
+#define PADOWN_SHIFT(p)			((p) % 8 * PADOWN_BITS)
+#define PADOWN_MASK(p)			(0xf << PADOWN_SHIFT(p))
+
+/* Offset from pad_regs */
+#define PADCFG0				0x000
+#define PADCFG0_RXEVCFG_SHIFT		25
+#define PADCFG0_RXEVCFG_MASK		(3 << PADCFG0_RXEVCFG_SHIFT)
+#define PADCFG0_RXEVCFG_LEVEL		0
+#define PADCFG0_RXEVCFG_EDGE		1
+#define PADCFG0_RXEVCFG_DISABLED	2
+#define PADCFG0_RXEVCFG_EDGE_BOTH	3
+#define PADCFG0_RXINV			BIT(23)
+#define PADCFG0_GPIROUTIOXAPIC		BIT(20)
+#define PADCFG0_GPIROUTSCI		BIT(19)
+#define PADCFG0_GPIROUTSMI		BIT(18)
+#define PADCFG0_GPIROUTNMI		BIT(17)
+#define PADCFG0_PMODE_SHIFT		10
+#define PADCFG0_PMODE_MASK		(0xf << PADCFG0_PMODE_SHIFT)
+#define PADCFG0_GPIORXDIS		BIT(9)
+#define PADCFG0_GPIOTXDIS		BIT(8)
+#define PADCFG0_GPIORXSTATE		BIT(1)
+#define PADCFG0_GPIOTXSTATE		BIT(0)
+
+#define PADCFG1				0x004
+#define PADCFG1_TERM_UP			BIT(13)
+#define PADCFG1_TERM_SHIFT		10
+#define PADCFG1_TERM_MASK		(7 << PADCFG1_TERM_SHIFT)
+#define PADCFG1_TERM_20K		4
+#define PADCFG1_TERM_2K			3
+#define PADCFG1_TERM_5K			2
+#define PADCFG1_TERM_1K			1
+
+struct intel_pad_context {
+	u32 padcfg0;
+	u32 padcfg1;
+};
+
+struct intel_community_context {
+	u32 *intmask;
+};
+
+struct intel_pinctrl_context {
+	struct intel_pad_context *pads;
+	struct intel_community_context *communities;
+};
+
+/**
+ * struct intel_pinctrl - Intel pinctrl private structure
+ * @dev: Pointer to the device structure
+ * @lock: Lock to serialize register access
+ * @pctldesc: Pin controller description
+ * @pctldev: Pointer to the pin controller device
+ * @chip: GPIO chip in this pin controller
+ * @soc: SoC/PCH specific pin configuration data
+ * @communities: All communities in this pin controller
+ * @ncommunities: Number of communities in this pin controller
+ * @context: Configuration saved over system sleep
+ */
+struct intel_pinctrl {
+	struct device *dev;
+	spinlock_t lock;
+	struct pinctrl_desc pctldesc;
+	struct pinctrl_dev *pctldev;
+	struct gpio_chip chip;
+	const struct intel_pinctrl_soc_data *soc;
+	struct intel_community *communities;
+	size_t ncommunities;
+	struct intel_pinctrl_context context;
+};
+
+#define gpiochip_to_pinctrl(c)	container_of(c, struct intel_pinctrl, chip)
+#define pin_to_padno(c, p)	((p) - (c)->pin_base)
+
+static struct intel_community *intel_get_community(struct intel_pinctrl *pctrl,
+						   unsigned pin)
+{
+	struct intel_community *community;
+	int i;
+
+	for (i = 0; i < pctrl->ncommunities; i++) {
+		community = &pctrl->communities[i];
+		if (pin >= community->pin_base &&
+		    pin < community->pin_base + community->npins)
+			return community;
+	}
+
+	dev_warn(pctrl->dev, "failed to find community for pin %u\n", pin);
+	return NULL;
+}
+
+static void __iomem *intel_get_padcfg(struct intel_pinctrl *pctrl, unsigned pin,
+				      unsigned reg)
+{
+	const struct intel_community *community;
+	unsigned padno;
+
+	community = intel_get_community(pctrl, pin);
+	if (!community)
+		return NULL;
+
+	padno = pin_to_padno(community, pin);
+	return community->pad_regs + reg + padno * 8;
+}
+
+static bool intel_pad_owned_by_host(struct intel_pinctrl *pctrl, unsigned pin)
+{
+	const struct intel_community *community;
+	unsigned padno, gpp, gpp_offset, offset;
+	void __iomem *padown;
+
+	community = intel_get_community(pctrl, pin);
+	if (!community)
+		return false;
+	if (!community->padown_offset)
+		return true;
+
+	padno = pin_to_padno(community, pin);
+	gpp = padno / NPADS_IN_GPP;
+	gpp_offset = padno % NPADS_IN_GPP;
+	offset = community->padown_offset + gpp * 16 + (gpp_offset / 8) * 4;
+	padown = community->regs + offset;
+
+	return !(readl(padown) & PADOWN_MASK(padno));
+}
+
+static bool intel_pad_reserved_for_acpi(struct intel_pinctrl *pctrl,
+					unsigned pin)
+{
+	const struct intel_community *community;
+	unsigned padno, gpp, offset;
+	void __iomem *hostown;
+
+	community = intel_get_community(pctrl, pin);
+	if (!community)
+		return true;
+	if (!community->hostown_offset)
+		return false;
+
+	padno = pin_to_padno(community, pin);
+	gpp = padno / NPADS_IN_GPP;
+	offset = community->hostown_offset + gpp * 4;
+	hostown = community->regs + offset;
+
+	return !(readl(hostown) & BIT(padno % NPADS_IN_GPP));
+}
+
+static bool intel_pad_locked(struct intel_pinctrl *pctrl, unsigned pin)
+{
+	struct intel_community *community;
+	unsigned padno, gpp, offset;
+	u32 value;
+
+	community = intel_get_community(pctrl, pin);
+	if (!community)
+		return true;
+	if (!community->padcfglock_offset)
+		return false;
+
+	padno = pin_to_padno(community, pin);
+	gpp = padno / NPADS_IN_GPP;
+
+	/*
+	 * If PADCFGLOCK and PADCFGLOCKTX bits are both clear for this pad,
+	 * the pad is considered unlocked. Any other case means that it is
+	 * either fully or partially locked and we don't touch it.
+	 */
+	offset = community->padcfglock_offset + gpp * 8;
+	value = readl(community->regs + offset);
+	if (value & BIT(pin % NPADS_IN_GPP))
+		return true;
+
+	offset = community->padcfglock_offset + 4 + gpp * 8;
+	value = readl(community->regs + offset);
+	if (value & BIT(pin % NPADS_IN_GPP))
+		return true;
+
+	return false;
+}
+
+static bool intel_pad_usable(struct intel_pinctrl *pctrl, unsigned pin)
+{
+	return intel_pad_owned_by_host(pctrl, pin) &&
+		!intel_pad_reserved_for_acpi(pctrl, pin) &&
+		!intel_pad_locked(pctrl, pin);
+}
+
+static int intel_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctrl->soc->ngroups;
+}
+
+static const char *intel_get_group_name(struct pinctrl_dev *pctldev,
+				      unsigned group)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctrl->soc->groups[group].name;
+}
+
+static int intel_get_group_pins(struct pinctrl_dev *pctldev, unsigned group,
+			      const unsigned **pins, unsigned *npins)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = pctrl->soc->groups[group].pins;
+	*npins = pctrl->soc->groups[group].npins;
+	return 0;
+}
+
+static void intel_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
+			       unsigned pin)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+	u32 cfg0, cfg1, mode;
+	bool locked, acpi;
+
+	if (!intel_pad_owned_by_host(pctrl, pin)) {
+		seq_puts(s, "not available");
+		return;
+	}
+
+	cfg0 = readl(intel_get_padcfg(pctrl, pin, PADCFG0));
+	cfg1 = readl(intel_get_padcfg(pctrl, pin, PADCFG1));
+
+	mode = (cfg0 & PADCFG0_PMODE_MASK) >> PADCFG0_PMODE_SHIFT;
+	if (!mode)
+		seq_puts(s, "GPIO ");
+	else
+		seq_printf(s, "mode %d ", mode);
+
+	seq_printf(s, "0x%08x 0x%08x", cfg0, cfg1);
+
+	locked = intel_pad_locked(pctrl, pin);
+	acpi = intel_pad_reserved_for_acpi(pctrl, pin);
+
+	if (locked || acpi) {
+		seq_puts(s, " [");
+		if (locked) {
+			seq_puts(s, "LOCKED");
+			if (acpi)
+				seq_puts(s, ", ");
+		}
+		if (acpi)
+			seq_puts(s, "ACPI");
+		seq_puts(s, "]");
+	}
+}
+
+static const struct pinctrl_ops intel_pinctrl_ops = {
+	.get_groups_count = intel_get_groups_count,
+	.get_group_name = intel_get_group_name,
+	.get_group_pins = intel_get_group_pins,
+	.pin_dbg_show = intel_pin_dbg_show,
+};
+
+static int intel_get_functions_count(struct pinctrl_dev *pctldev)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctrl->soc->nfunctions;
+}
+
+static const char *intel_get_function_name(struct pinctrl_dev *pctldev,
+					   unsigned function)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctrl->soc->functions[function].name;
+}
+
+static int intel_get_function_groups(struct pinctrl_dev *pctldev,
+				     unsigned function,
+				     const char * const **groups,
+				     unsigned * const ngroups)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = pctrl->soc->functions[function].groups;
+	*ngroups = pctrl->soc->functions[function].ngroups;
+	return 0;
+}
+
+static int intel_pinmux_set_mux(struct pinctrl_dev *pctldev, unsigned function,
+				unsigned group)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+	const struct intel_pingroup *grp = &pctrl->soc->groups[group];
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&pctrl->lock, flags);
+
+	/*
+	 * All pins in the groups needs to be accessible and writable
+	 * before we can enable the mux for this group.
+	 */
+	for (i = 0; i < grp->npins; i++) {
+		if (!intel_pad_usable(pctrl, grp->pins[i])) {
+			spin_unlock_irqrestore(&pctrl->lock, flags);
+			return -EBUSY;
+		}
+	}
+
+	/* Now enable the mux setting for each pin in the group */
+	for (i = 0; i < grp->npins; i++) {
+		void __iomem *padcfg0;
+		u32 value;
+
+		padcfg0 = intel_get_padcfg(pctrl, grp->pins[i], PADCFG0);
+		value = readl(padcfg0);
+
+		value &= ~PADCFG0_PMODE_MASK;
+		value |= grp->mode << PADCFG0_PMODE_SHIFT;
+
+		writel(value, padcfg0);
+	}
+
+	spin_unlock_irqrestore(&pctrl->lock, flags);
+
+	return 0;
+}
+
+static int intel_gpio_request_enable(struct pinctrl_dev *pctldev,
+				     struct pinctrl_gpio_range *range,
+				     unsigned pin)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+	void __iomem *padcfg0;
+	unsigned long flags;
+	u32 value;
+
+	spin_lock_irqsave(&pctrl->lock, flags);
+
+	if (!intel_pad_usable(pctrl, pin)) {
+		spin_unlock_irqrestore(&pctrl->lock, flags);
+		return -EBUSY;
+	}
+
+	padcfg0 = intel_get_padcfg(pctrl, pin, PADCFG0);
+	/* Put the pad into GPIO mode */
+	value = readl(padcfg0) & ~PADCFG0_PMODE_MASK;
+	/* Disable SCI/SMI/NMI generation */
+	value &= ~(PADCFG0_GPIROUTIOXAPIC | PADCFG0_GPIROUTSCI);
+	value &= ~(PADCFG0_GPIROUTSMI | PADCFG0_GPIROUTNMI);
+	/* Disable TX buffer and enable RX (this will be input) */
+	value &= ~PADCFG0_GPIORXDIS;
+	value |= PADCFG0_GPIOTXDIS;
+	writel(value, padcfg0);
+
+	spin_unlock_irqrestore(&pctrl->lock, flags);
+
+	return 0;
+}
+
+static int intel_gpio_set_direction(struct pinctrl_dev *pctldev,
+				    struct pinctrl_gpio_range *range,
+				    unsigned pin, bool input)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+	void __iomem *padcfg0;
+	unsigned long flags;
+	u32 value;
+
+	spin_lock_irqsave(&pctrl->lock, flags);
+
+	padcfg0 = intel_get_padcfg(pctrl, pin, PADCFG0);
+
+	value = readl(padcfg0);
+	if (input)
+		value |= PADCFG0_GPIOTXDIS;
+	else
+		value &= ~PADCFG0_GPIOTXDIS;
+	writel(value, padcfg0);
+
+	spin_unlock_irqrestore(&pctrl->lock, flags);
+
+	return 0;
+}
+
+static const struct pinmux_ops intel_pinmux_ops = {
+	.get_functions_count = intel_get_functions_count,
+	.get_function_name = intel_get_function_name,
+	.get_function_groups = intel_get_function_groups,
+	.set_mux = intel_pinmux_set_mux,
+	.gpio_request_enable = intel_gpio_request_enable,
+	.gpio_set_direction = intel_gpio_set_direction,
+};
+
+static int intel_config_get(struct pinctrl_dev *pctldev, unsigned pin,
+			    unsigned long *config)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	u32 value, term;
+	u16 arg = 0;
+
+	if (!intel_pad_owned_by_host(pctrl, pin))
+		return -ENOTSUPP;
+
+	value = readl(intel_get_padcfg(pctrl, pin, PADCFG1));
+	term = (value & PADCFG1_TERM_MASK) >> PADCFG1_TERM_SHIFT;
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		if (term)
+			return -EINVAL;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		if (!term || !(value & PADCFG1_TERM_UP))
+			return -EINVAL;
+
+		switch (term) {
+		case PADCFG1_TERM_1K:
+			arg = 1000;
+			break;
+		case PADCFG1_TERM_2K:
+			arg = 2000;
+			break;
+		case PADCFG1_TERM_5K:
+			arg = 5000;
+			break;
+		case PADCFG1_TERM_20K:
+			arg = 20000;
+			break;
+		}
+
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		if (!term || value & PADCFG1_TERM_UP)
+			return -EINVAL;
+
+		switch (term) {
+		case PADCFG1_TERM_5K:
+			arg = 5000;
+			break;
+		case PADCFG1_TERM_20K:
+			arg = 20000;
+			break;
+		}
+
+		break;
+
+	default:
+		return -ENOTSUPP;
+	}
+
+	*config = pinconf_to_config_packed(param, arg);
+	return 0;
+}
+
+static int intel_config_set_pull(struct intel_pinctrl *pctrl, unsigned pin,
+				 unsigned long config)
+{
+	unsigned param = pinconf_to_config_param(config);
+	unsigned arg = pinconf_to_config_argument(config);
+	void __iomem *padcfg1;
+	unsigned long flags;
+	int ret = 0;
+	u32 value;
+
+	spin_lock_irqsave(&pctrl->lock, flags);
+
+	padcfg1 = intel_get_padcfg(pctrl, pin, PADCFG1);
+	value = readl(padcfg1);
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		value &= ~(PADCFG1_TERM_MASK | PADCFG1_TERM_UP);
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		value &= ~PADCFG1_TERM_MASK;
+
+		value |= PADCFG1_TERM_UP;
+
+		switch (arg) {
+		case 20000:
+			value |= PADCFG1_TERM_20K << PADCFG1_TERM_SHIFT;
+			break;
+		case 5000:
+			value |= PADCFG1_TERM_5K << PADCFG1_TERM_SHIFT;
+			break;
+		case 2000:
+			value |= PADCFG1_TERM_2K << PADCFG1_TERM_SHIFT;
+			break;
+		case 1000:
+			value |= PADCFG1_TERM_1K << PADCFG1_TERM_SHIFT;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		value &= ~(PADCFG1_TERM_UP | PADCFG1_TERM_MASK);
+
+		switch (arg) {
+		case 20000:
+			value |= PADCFG1_TERM_20K << PADCFG1_TERM_SHIFT;
+			break;
+		case 5000:
+			value |= PADCFG1_TERM_5K << PADCFG1_TERM_SHIFT;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+
+		break;
+	}
+
+	if (!ret)
+		writel(value, padcfg1);
+
+	spin_unlock_irqrestore(&pctrl->lock, flags);
+
+	return ret;
+}
+
+static int intel_config_set(struct pinctrl_dev *pctldev, unsigned pin,
+			  unsigned long *configs, unsigned nconfigs)
+{
+	struct intel_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
+	int i, ret;
+
+	if (!intel_pad_usable(pctrl, pin))
+		return -ENOTSUPP;
+
+	for (i = 0; i < nconfigs; i++) {
+		switch (pinconf_to_config_param(configs[i])) {
+		case PIN_CONFIG_BIAS_DISABLE:
+		case PIN_CONFIG_BIAS_PULL_UP:
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			ret = intel_config_set_pull(pctrl, pin, configs[i]);
+			if (ret)
+				return ret;
+			break;
+
+		default:
+			return -ENOTSUPP;
+		}
+	}
+
+	return 0;
+}
+
+static const struct pinconf_ops intel_pinconf_ops = {
+	.is_generic = true,
+	.pin_config_get = intel_config_get,
+	.pin_config_set = intel_config_set,
+};
+
+static const struct pinctrl_desc intel_pinctrl_desc = {
+	.pctlops = &intel_pinctrl_ops,
+	.pmxops = &intel_pinmux_ops,
+	.confops = &intel_pinconf_ops,
+	.owner = THIS_MODULE,
+};
+
+static int intel_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(chip->base + offset);
+}
+
+static void intel_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+}
+
+static int intel_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(chip);
+	void __iomem *reg;
+
+	reg = intel_get_padcfg(pctrl, offset, PADCFG0);
+	if (!reg)
+		return -EINVAL;
+
+	return !!(readl(reg) & PADCFG0_GPIORXSTATE);
+}
+
+static void intel_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(chip);
+	void __iomem *reg;
+
+	reg = intel_get_padcfg(pctrl, offset, PADCFG0);
+	if (reg) {
+		unsigned long flags;
+		u32 padcfg0;
+
+		spin_lock_irqsave(&pctrl->lock, flags);
+		padcfg0 = readl(reg);
+		if (value)
+			padcfg0 |= PADCFG0_GPIOTXSTATE;
+		else
+			padcfg0 &= ~PADCFG0_GPIOTXSTATE;
+		writel(padcfg0, reg);
+		spin_unlock_irqrestore(&pctrl->lock, flags);
+	}
+}
+
+static int intel_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_gpio_direction_input(chip->base + offset);
+}
+
+static int intel_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
+				       int value)
+{
+	intel_gpio_set(chip, offset, value);
+	return pinctrl_gpio_direction_output(chip->base + offset);
+}
+
+static const struct gpio_chip intel_gpio_chip = {
+	.owner = THIS_MODULE,
+	.request = intel_gpio_request,
+	.free = intel_gpio_free,
+	.direction_input = intel_gpio_direction_input,
+	.direction_output = intel_gpio_direction_output,
+	.get = intel_gpio_get,
+	.set = intel_gpio_set,
+};
+
+static void intel_gpio_irq_ack(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	const struct intel_community *community;
+	unsigned pin = irqd_to_hwirq(d);
+
+	spin_lock(&pctrl->lock);
+
+	community = intel_get_community(pctrl, pin);
+	if (community) {
+		unsigned padno = pin_to_padno(community, pin);
+		unsigned gpp_offset = padno % NPADS_IN_GPP;
+		unsigned gpp = padno / NPADS_IN_GPP;
+
+		writel(BIT(gpp_offset), community->regs + GPI_IS + gpp * 4);
+	}
+
+	spin_unlock(&pctrl->lock);
+}
+
+static void intel_gpio_irq_mask_unmask(struct irq_data *d, bool mask)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	const struct intel_community *community;
+	unsigned pin = irqd_to_hwirq(d);
+	unsigned long flags;
+
+	spin_lock_irqsave(&pctrl->lock, flags);
+
+	community = intel_get_community(pctrl, pin);
+	if (community) {
+		unsigned padno = pin_to_padno(community, pin);
+		unsigned gpp_offset = padno % NPADS_IN_GPP;
+		unsigned gpp = padno / NPADS_IN_GPP;
+		void __iomem *reg;
+		u32 value;
+
+		reg = community->regs + community->ie_offset + gpp * 4;
+		value = readl(reg);
+		if (mask)
+			value &= ~BIT(gpp_offset);
+		else
+			value |= BIT(gpp_offset);
+		writel(value, reg);
+	}
+
+	spin_unlock_irqrestore(&pctrl->lock, flags);
+}
+
+static void intel_gpio_irq_mask(struct irq_data *d)
+{
+	intel_gpio_irq_mask_unmask(d, true);
+}
+
+static void intel_gpio_irq_unmask(struct irq_data *d)
+{
+	intel_gpio_irq_mask_unmask(d, false);
+}
+
+static int intel_gpio_irq_type(struct irq_data *d, unsigned type)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	unsigned pin = irqd_to_hwirq(d);
+	unsigned long flags;
+	void __iomem *reg;
+	u32 value;
+
+	reg = intel_get_padcfg(pctrl, pin, PADCFG0);
+	if (!reg)
+		return -EINVAL;
+
+	spin_lock_irqsave(&pctrl->lock, flags);
+
+	value = readl(reg);
+
+	value &= ~(PADCFG0_RXEVCFG_MASK | PADCFG0_RXINV);
+
+	if ((type & IRQ_TYPE_EDGE_BOTH) == IRQ_TYPE_EDGE_BOTH) {
+		value |= PADCFG0_RXEVCFG_EDGE_BOTH << PADCFG0_RXEVCFG_SHIFT;
+	} else if (type & IRQ_TYPE_EDGE_FALLING) {
+		value |= PADCFG0_RXEVCFG_EDGE << PADCFG0_RXEVCFG_SHIFT;
+		value |= PADCFG0_RXINV;
+	} else if (type & IRQ_TYPE_EDGE_RISING) {
+		value |= PADCFG0_RXEVCFG_EDGE << PADCFG0_RXEVCFG_SHIFT;
+	} else if (type & IRQ_TYPE_LEVEL_LOW) {
+		value |= PADCFG0_RXINV;
+	} else {
+		value |= PADCFG0_RXEVCFG_DISABLED << PADCFG0_RXEVCFG_SHIFT;
+	}
+
+	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(&pctrl->lock, flags);
+
+	return 0;
+}
+
+static int intel_gpio_irq_wake(struct irq_data *d, unsigned int on)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	const struct intel_community *community;
+	unsigned pin = irqd_to_hwirq(d);
+	unsigned padno, gpp, gpp_offset;
+	u32 gpe_en;
+
+	community = intel_get_community(pctrl, pin);
+	if (!community)
+		return -EINVAL;
+
+	padno = pin_to_padno(community, pin);
+	gpp = padno / NPADS_IN_GPP;
+	gpp_offset = padno % NPADS_IN_GPP;
+
+	/* Clear the existing wake status */
+	writel(BIT(gpp_offset), community->regs + GPI_GPE_STS + gpp * 4);
+
+	/*
+	 * The controller will generate wake when GPE of the corresponding
+	 * pad is enabled and it is not routed to SCI (GPIROUTSCI is not
+	 * set).
+	 */
+	gpe_en = readl(community->regs + GPI_GPE_EN + gpp * 4);
+	if (on)
+		gpe_en |= BIT(gpp_offset);
+	else
+		gpe_en &= ~BIT(gpp_offset);
+	writel(gpe_en, community->regs + GPI_GPE_EN + gpp * 4);
+
+	dev_dbg(pctrl->dev, "%sable wake for pin %u\n", on ? "en" : "dis", pin);
+	return 0;
+}
+
+static void intel_gpio_community_irq_handler(struct gpio_chip *gc,
+	const struct intel_community *community)
+{
+	int gpp;
+
+	for (gpp = 0; gpp < community->ngpps; gpp++) {
+		unsigned long pending, enabled, gpp_offset;
+
+		pending = readl(community->regs + GPI_IS + gpp * 4);
+		enabled = readl(community->regs + community->ie_offset +
+				gpp * 4);
+
+		/* Only interrupts that are enabled */
+		pending &= enabled;
+
+		for_each_set_bit(gpp_offset, &pending, NPADS_IN_GPP) {
+			unsigned padno, irq;
+
+			/*
+			 * The last group in community can have less pins
+			 * than NPADS_IN_GPP.
+			 */
+			padno = gpp_offset + gpp * NPADS_IN_GPP;
+			if (padno >= community->npins)
+				break;
+
+			irq = irq_find_mapping(gc->irqdomain,
+					       community->pin_base + padno);
+			generic_handle_irq(irq);
+		}
+	}
+}
+
+static void intel_gpio_irq_handler(unsigned irq, struct irq_desc *desc)
+{
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct irq_chip *chip = irq_get_chip(irq);
+	int i;
+
+	chained_irq_enter(chip, desc);
+
+	/* Need to check all communities for pending interrupts */
+	for (i = 0; i < pctrl->ncommunities; i++)
+		intel_gpio_community_irq_handler(gc, &pctrl->communities[i]);
+
+	chained_irq_exit(chip, desc);
+}
+
+static struct irq_chip intel_gpio_irqchip = {
+	.name = "intel-gpio",
+	.irq_ack = intel_gpio_irq_ack,
+	.irq_mask = intel_gpio_irq_mask,
+	.irq_unmask = intel_gpio_irq_unmask,
+	.irq_set_type = intel_gpio_irq_type,
+	.irq_set_wake = intel_gpio_irq_wake,
+};
+
+static void intel_gpio_irq_init(struct intel_pinctrl *pctrl)
+{
+	size_t i;
+
+	for (i = 0; i < pctrl->ncommunities; i++) {
+		const struct intel_community *community;
+		void __iomem *base;
+		unsigned gpp;
+
+		community = &pctrl->communities[i];
+		base = community->regs;
+
+		for (gpp = 0; gpp < community->ngpps; gpp++) {
+			/* Mask and clear all interrupts */
+			writel(0, base + community->ie_offset + gpp * 4);
+			writel(0xffff, base + GPI_IS + gpp * 4);
+		}
+	}
+}
+
+static int intel_gpio_probe(struct intel_pinctrl *pctrl, int irq)
+{
+	int ret;
+
+	pctrl->chip = intel_gpio_chip;
+
+	pctrl->chip.ngpio = pctrl->soc->npins;
+	pctrl->chip.label = dev_name(pctrl->dev);
+	pctrl->chip.dev = pctrl->dev;
+	pctrl->chip.base = -1;
+
+	ret = gpiochip_add(&pctrl->chip);
+	if (ret) {
+		dev_err(pctrl->dev, "failed to register gpiochip\n");
+		return ret;
+	}
+
+	ret = gpiochip_add_pin_range(&pctrl->chip, dev_name(pctrl->dev),
+				     0, 0, pctrl->soc->npins);
+	if (ret) {
+		dev_err(pctrl->dev, "failed to add GPIO pin range\n");
+		gpiochip_remove(&pctrl->chip);
+		return ret;
+	}
+
+	ret = gpiochip_irqchip_add(&pctrl->chip, &intel_gpio_irqchip, 0,
+				   handle_simple_irq, IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(pctrl->dev, "failed to add irqchip\n");
+		gpiochip_remove(&pctrl->chip);
+		return ret;
+	}
+
+	gpiochip_set_chained_irqchip(&pctrl->chip, &intel_gpio_irqchip, irq,
+				     intel_gpio_irq_handler);
+	return 0;
+}
+
+static int intel_pinctrl_pm_init(struct intel_pinctrl *pctrl)
+{
+#ifdef CONFIG_PM_SLEEP
+	const struct intel_pinctrl_soc_data *soc = pctrl->soc;
+	struct intel_community_context *communities;
+	struct intel_pad_context *pads;
+	int i;
+
+	pads = devm_kcalloc(pctrl->dev, soc->npins, sizeof(*pads), GFP_KERNEL);
+	if (!pads)
+		return -ENOMEM;
+
+	communities = devm_kcalloc(pctrl->dev, pctrl->ncommunities,
+				   sizeof(*communities), GFP_KERNEL);
+	if (!communities)
+		return -ENOMEM;
+
+
+	for (i = 0; i < pctrl->ncommunities; i++) {
+		struct intel_community *community = &pctrl->communities[i];
+		u32 *intmask;
+
+		intmask = devm_kcalloc(pctrl->dev, community->ngpps,
+				       sizeof(*intmask), GFP_KERNEL);
+		if (!intmask)
+			return -ENOMEM;
+
+		communities[i].intmask = intmask;
+	}
+
+	pctrl->context.pads = pads;
+	pctrl->context.communities = communities;
+#endif
+
+	return 0;
+}
+
+int intel_pinctrl_probe(struct platform_device *pdev,
+			const struct intel_pinctrl_soc_data *soc_data)
+{
+	struct intel_pinctrl *pctrl;
+	int i, ret, irq;
+
+	if (!soc_data)
+		return -EINVAL;
+
+	pctrl = devm_kzalloc(&pdev->dev, sizeof(*pctrl), GFP_KERNEL);
+	if (!pctrl)
+		return -ENOMEM;
+
+	pctrl->dev = &pdev->dev;
+	pctrl->soc = soc_data;
+	spin_lock_init(&pctrl->lock);
+
+	/*
+	 * Make a copy of the communities which we can use to hold pointers
+	 * to the registers.
+	 */
+	pctrl->ncommunities = pctrl->soc->ncommunities;
+	pctrl->communities = devm_kcalloc(&pdev->dev, pctrl->ncommunities,
+				  sizeof(*pctrl->communities), GFP_KERNEL);
+	if (!pctrl->communities)
+		return -ENOMEM;
+
+	for (i = 0; i < pctrl->ncommunities; i++) {
+		struct intel_community *community = &pctrl->communities[i];
+		struct resource *res;
+		void __iomem *regs;
+		u32 padbar;
+
+		*community = pctrl->soc->communities[i];
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM,
+					    community->barno);
+		regs = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(regs))
+			return PTR_ERR(regs);
+
+		/* Read offset of the pad configuration registers */
+		padbar = readl(regs + PADBAR);
+
+		community->regs = regs;
+		community->pad_regs = regs + padbar;
+		community->ngpps = DIV_ROUND_UP(community->npins, NPADS_IN_GPP);
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get interrupt number\n");
+		return irq;
+	}
+
+	ret = intel_pinctrl_pm_init(pctrl);
+	if (ret)
+		return ret;
+
+	pctrl->pctldesc = intel_pinctrl_desc;
+	pctrl->pctldesc.name = dev_name(&pdev->dev);
+	pctrl->pctldesc.pins = pctrl->soc->pins;
+	pctrl->pctldesc.npins = pctrl->soc->npins;
+
+	pctrl->pctldev = pinctrl_register(&pctrl->pctldesc, &pdev->dev, pctrl);
+	if (!pctrl->pctldev) {
+		dev_err(&pdev->dev, "failed to register pinctrl driver\n");
+		return -ENODEV;
+	}
+
+	ret = intel_gpio_probe(pctrl, irq);
+	if (ret) {
+		pinctrl_unregister(pctrl->pctldev);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, pctrl);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(intel_pinctrl_probe);
+
+int intel_pinctrl_remove(struct platform_device *pdev)
+{
+	struct intel_pinctrl *pctrl = platform_get_drvdata(pdev);
+
+	gpiochip_remove(&pctrl->chip);
+	pinctrl_unregister(pctrl->pctldev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(intel_pinctrl_remove);
+
+#ifdef CONFIG_PM_SLEEP
+int intel_pinctrl_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct intel_pinctrl *pctrl = platform_get_drvdata(pdev);
+	struct intel_community_context *communities;
+	struct intel_pad_context *pads;
+	int i;
+
+	pads = pctrl->context.pads;
+	for (i = 0; i < pctrl->soc->npins; i++) {
+		const struct pinctrl_pin_desc *desc = &pctrl->soc->pins[i];
+		u32 val;
+
+		if (!intel_pad_usable(pctrl, desc->number))
+			continue;
+
+		val = readl(intel_get_padcfg(pctrl, desc->number, PADCFG0));
+		pads[i].padcfg0 = val & ~PADCFG0_GPIORXSTATE;
+		val = readl(intel_get_padcfg(pctrl, desc->number, PADCFG1));
+		pads[i].padcfg1 = val;
+	}
+
+	communities = pctrl->context.communities;
+	for (i = 0; i < pctrl->ncommunities; i++) {
+		struct intel_community *community = &pctrl->communities[i];
+		void __iomem *base;
+		unsigned gpp;
+
+		base = community->regs + community->ie_offset;
+		for (gpp = 0; gpp < community->ngpps; gpp++)
+			communities[i].intmask[gpp] = readl(base + gpp * 4);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(intel_pinctrl_suspend);
+
+int intel_pinctrl_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct intel_pinctrl *pctrl = platform_get_drvdata(pdev);
+	const struct intel_community_context *communities;
+	const struct intel_pad_context *pads;
+	int i;
+
+	/* Mask all interrupts */
+	intel_gpio_irq_init(pctrl);
+
+	pads = pctrl->context.pads;
+	for (i = 0; i < pctrl->soc->npins; i++) {
+		const struct pinctrl_pin_desc *desc = &pctrl->soc->pins[i];
+		void __iomem *padcfg;
+		u32 val;
+
+		if (!intel_pad_usable(pctrl, desc->number))
+			continue;
+
+		padcfg = intel_get_padcfg(pctrl, desc->number, PADCFG0);
+		val = readl(padcfg) & ~PADCFG0_GPIORXSTATE;
+		if (val != pads[i].padcfg0) {
+			writel(pads[i].padcfg0, padcfg);
+			dev_dbg(dev, "restored pin %u padcfg0 %#08x\n",
+				desc->number, readl(padcfg));
+		}
+
+		padcfg = intel_get_padcfg(pctrl, desc->number, PADCFG1);
+		val = readl(padcfg);
+		if (val != pads[i].padcfg1) {
+			writel(pads[i].padcfg1, padcfg);
+			dev_dbg(dev, "restored pin %u padcfg1 %#08x\n",
+				desc->number, readl(padcfg));
+		}
+	}
+
+	communities = pctrl->context.communities;
+	for (i = 0; i < pctrl->ncommunities; i++) {
+		struct intel_community *community = &pctrl->communities[i];
+		void __iomem *base;
+		unsigned gpp;
+
+		base = community->regs + community->ie_offset;
+		for (gpp = 0; gpp < community->ngpps; gpp++) {
+			writel(communities[i].intmask[gpp], base + gpp * 4);
+			dev_dbg(dev, "restored mask %d/%u %#08x\n", i, gpp,
+				readl(base + gpp * 4));
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(intel_pinctrl_resume);
+#endif
+
+MODULE_AUTHOR("Mathias Nyman <mathias.nyman@linux.intel.com>");
+MODULE_AUTHOR("Mika Westerberg <mika.westerberg@linux.intel.com>");
+MODULE_DESCRIPTION("Intel pinctrl/GPIO core driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/intel/pinctrl-intel.h b/drivers/pinctrl/intel/pinctrl-intel.h
new file mode 100644
index 000000000000..4ec8b572a288
--- /dev/null
+++ b/drivers/pinctrl/intel/pinctrl-intel.h
@@ -0,0 +1,128 @@
+/*
+ * Core pinctrl/GPIO driver for Intel GPIO controllers
+ *
+ * Copyright (C) 2015, Intel Corporation
+ * Authors: Mathias Nyman <mathias.nyman@linux.intel.com>
+ *          Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef PINCTRL_INTEL_H
+#define PINCTRL_INTEL_H
+
+struct pinctrl_pin_desc;
+struct platform_device;
+struct device;
+
+/**
+ * struct intel_pingroup - Description about group of pins
+ * @name: Name of the groups
+ * @pins: All pins in this group
+ * @npins: Number of pins in this groups
+ * @mode: Native mode in which the group is muxed out @pins
+ */
+struct intel_pingroup {
+	const char *name;
+	const unsigned *pins;
+	size_t npins;
+	unsigned short mode;
+};
+
+/**
+ * struct intel_function - Description about a function
+ * @name: Name of the function
+ * @groups: An array of groups for this function
+ * @ngroups: Number of groups in @groups
+ */
+struct intel_function {
+	const char *name;
+	const char * const *groups;
+	size_t ngroups;
+};
+
+/**
+ * struct intel_community - Intel pin community description
+ * @barno: MMIO BAR number where registers for this community reside
+ * @padown_offset: Register offset of PAD_OWN register from @regs. If %0
+ *                 then there is no support for owner.
+ * @padcfglock_offset: Register offset of PADCFGLOCK from @regs. If %0 then
+ *                     locking is not supported.
+ * @hostown_offset: Register offset of HOSTSW_OWN from @regs. If %0 then it
+ *                  is assumed that the host owns the pin (rather than
+ *                  ACPI).
+ * @ie_offset: Register offset of GPI_IE from @regs.
+ * @pin_base: Starting pin of pins in this community
+ * @npins: Number of pins in this community
+ * @regs: Community specific common registers (reserved for core driver)
+ * @pad_regs: Community specific pad registers (reserved for core driver)
+ * @ngpps: Number of groups (hw groups) in this community (reserved for
+ *         core driver)
+ */
+struct intel_community {
+	unsigned barno;
+	unsigned padown_offset;
+	unsigned padcfglock_offset;
+	unsigned hostown_offset;
+	unsigned ie_offset;
+	unsigned pin_base;
+	size_t npins;
+	void __iomem *regs;
+	void __iomem *pad_regs;
+	size_t ngpps;
+};
+
+#define PIN_GROUP(n, p, m)			\
+	{					\
+		.name = (n),			\
+		.pins = (p),			\
+		.npins = ARRAY_SIZE((p)),	\
+		.mode = (m),			\
+	}
+
+#define FUNCTION(n, g)				\
+	{					\
+		.name = (n),			\
+		.groups = (g),			\
+		.ngroups = ARRAY_SIZE((g)),	\
+	}
+
+/**
+ * struct intel_pinctrl_soc_data - Intel pin controller per-SoC configuration
+ * @uid: ACPI _UID for the probe driver use if needed
+ * @pins: Array if pins this pinctrl controls
+ * @npins: Number of pins in the array
+ * @groups: Array of pin groups
+ * @ngroups: Number of groups in the array
+ * @functions: Array of functions
+ * @nfunctions: Number of functions in the array
+ * @communities: Array of communities this pinctrl handles
+ * @ncommunities: Number of communities in the array
+ *
+ * The @communities is used as a template by the core driver. It will make
+ * copy of all communities and fill in rest of the information.
+ */
+struct intel_pinctrl_soc_data {
+	const char *uid;
+	const struct pinctrl_pin_desc *pins;
+	size_t npins;
+	const struct intel_pingroup *groups;
+	size_t ngroups;
+	const struct intel_function *functions;
+	size_t nfunctions;
+	const struct intel_community *communities;
+	size_t ncommunities;
+};
+
+int intel_pinctrl_probe(struct platform_device *pdev,
+			const struct intel_pinctrl_soc_data *soc_data);
+int intel_pinctrl_remove(struct platform_device *pdev);
+
+#ifdef CONFIG_PM_SLEEP
+int intel_pinctrl_suspend(struct device *dev);
+int intel_pinctrl_resume(struct device *dev);
+#endif
+
+#endif /* PINCTRL_INTEL_H */
diff --git a/drivers/pinctrl/intel/pinctrl-sunrisepoint.c b/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
new file mode 100644
index 000000000000..55d025dc89e8
--- /dev/null
+++ b/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
@@ -0,0 +1,336 @@
+/*
+ * Intel Sunrisepoint PCH pinctrl/GPIO driver
+ *
+ * Copyright (C) 2015, Intel Corporation
+ * Authors: Mathias Nyman <mathias.nyman@linux.intel.com>
+ *          Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/acpi.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-intel.h"
+
+#define SPT_PAD_OWN	0x020
+#define SPT_PADCFGLOCK	0x0a0
+#define SPT_HOSTSW_OWN	0x0d0
+#define SPT_GPI_IE	0x120
+
+#define SPT_COMMUNITY(b, s, e)				\
+	{						\
+		.barno = (b),				\
+		.padown_offset = SPT_PAD_OWN,		\
+		.padcfglock_offset = SPT_PADCFGLOCK,	\
+		.hostown_offset = SPT_HOSTSW_OWN,	\
+		.ie_offset = SPT_GPI_IE,		\
+		.pin_base = (s),			\
+		.npins = ((e) - (s) + 1),		\
+	}
+
+/* Sunrisepoint-LP */
+static const struct pinctrl_pin_desc sptlp_pins[] = {
+	/* GPP_A */
+	PINCTRL_PIN(0, "RCINB"),
+	PINCTRL_PIN(1, "LAD_0"),
+	PINCTRL_PIN(2, "LAD_1"),
+	PINCTRL_PIN(3, "LAD_2"),
+	PINCTRL_PIN(4, "LAD_3"),
+	PINCTRL_PIN(5, "LFRAMEB"),
+	PINCTRL_PIN(6, "SERIQ"),
+	PINCTRL_PIN(7, "PIRQAB"),
+	PINCTRL_PIN(8, "CLKRUNB"),
+	PINCTRL_PIN(9, "CLKOUT_LPC_0"),
+	PINCTRL_PIN(10, "CLKOUT_LPC_1"),
+	PINCTRL_PIN(11, "PMEB"),
+	PINCTRL_PIN(12, "BM_BUSYB"),
+	PINCTRL_PIN(13, "SUSWARNB_SUS_PWRDNACK"),
+	PINCTRL_PIN(14, "SUS_STATB"),
+	PINCTRL_PIN(15, "SUSACKB"),
+	PINCTRL_PIN(16, "SD_1P8_SEL"),
+	PINCTRL_PIN(17, "SD_PWR_EN_B"),
+	PINCTRL_PIN(18, "ISH_GP_0"),
+	PINCTRL_PIN(19, "ISH_GP_1"),
+	PINCTRL_PIN(20, "ISH_GP_2"),
+	PINCTRL_PIN(21, "ISH_GP_3"),
+	PINCTRL_PIN(22, "ISH_GP_4"),
+	PINCTRL_PIN(23, "ISH_GP_5"),
+	/* GPP_B */
+	PINCTRL_PIN(24, "CORE_VID_0"),
+	PINCTRL_PIN(25, "CORE_VID_1"),
+	PINCTRL_PIN(26, "VRALERTB"),
+	PINCTRL_PIN(27, "CPU_GP_2"),
+	PINCTRL_PIN(28, "CPU_GP_3"),
+	PINCTRL_PIN(29, "SRCCLKREQB_0"),
+	PINCTRL_PIN(30, "SRCCLKREQB_1"),
+	PINCTRL_PIN(31, "SRCCLKREQB_2"),
+	PINCTRL_PIN(32, "SRCCLKREQB_3"),
+	PINCTRL_PIN(33, "SRCCLKREQB_4"),
+	PINCTRL_PIN(34, "SRCCLKREQB_5"),
+	PINCTRL_PIN(35, "EXT_PWR_GATEB"),
+	PINCTRL_PIN(36, "SLP_S0B"),
+	PINCTRL_PIN(37, "PLTRSTB"),
+	PINCTRL_PIN(38, "SPKR"),
+	PINCTRL_PIN(39, "GSPI0_CSB"),
+	PINCTRL_PIN(40, "GSPI0_CLK"),
+	PINCTRL_PIN(41, "GSPI0_MISO"),
+	PINCTRL_PIN(42, "GSPI0_MOSI"),
+	PINCTRL_PIN(43, "GSPI1_CSB"),
+	PINCTRL_PIN(44, "GSPI1_CLK"),
+	PINCTRL_PIN(45, "GSPI1_MISO"),
+	PINCTRL_PIN(46, "GSPI1_MOSI"),
+	PINCTRL_PIN(47, "SML1ALERTB"),
+	/* GPP_C */
+	PINCTRL_PIN(48, "SMBCLK"),
+	PINCTRL_PIN(49, "SMBDATA"),
+	PINCTRL_PIN(50, "SMBALERTB"),
+	PINCTRL_PIN(51, "SML0CLK"),
+	PINCTRL_PIN(52, "SML0DATA"),
+	PINCTRL_PIN(53, "SML0ALERTB"),
+	PINCTRL_PIN(54, "SML1CLK"),
+	PINCTRL_PIN(55, "SML1DATA"),
+	PINCTRL_PIN(56, "UART0_RXD"),
+	PINCTRL_PIN(57, "UART0_TXD"),
+	PINCTRL_PIN(58, "UART0_RTSB"),
+	PINCTRL_PIN(59, "UART0_CTSB"),
+	PINCTRL_PIN(60, "UART1_RXD"),
+	PINCTRL_PIN(61, "UART1_TXD"),
+	PINCTRL_PIN(62, "UART1_RTSB"),
+	PINCTRL_PIN(63, "UART1_CTSB"),
+	PINCTRL_PIN(64, "I2C0_SDA"),
+	PINCTRL_PIN(65, "I2C0_SCL"),
+	PINCTRL_PIN(66, "I2C1_SDA"),
+	PINCTRL_PIN(67, "I2C1_SCL"),
+	PINCTRL_PIN(68, "UART2_RXD"),
+	PINCTRL_PIN(69, "UART2_TXD"),
+	PINCTRL_PIN(70, "UART2_RTSB"),
+	PINCTRL_PIN(71, "UART2_CTSB"),
+	/* GPP_D */
+	PINCTRL_PIN(72, "SPI1_CSB"),
+	PINCTRL_PIN(73, "SPI1_CLK"),
+	PINCTRL_PIN(74, "SPI1_MISO_IO_1"),
+	PINCTRL_PIN(75, "SPI1_MOSI_IO_0"),
+	PINCTRL_PIN(76, "FLASHTRIG"),
+	PINCTRL_PIN(77, "ISH_I2C0_SDA"),
+	PINCTRL_PIN(78, "ISH_I2C0_SCL"),
+	PINCTRL_PIN(79, "ISH_I2C1_SDA"),
+	PINCTRL_PIN(80, "ISH_I2C1_SCL"),
+	PINCTRL_PIN(81, "ISH_SPI_CSB"),
+	PINCTRL_PIN(82, "ISH_SPI_CLK"),
+	PINCTRL_PIN(83, "ISH_SPI_MISO"),
+	PINCTRL_PIN(84, "ISH_SPI_MOSI"),
+	PINCTRL_PIN(85, "ISH_UART0_RXD"),
+	PINCTRL_PIN(86, "ISH_UART0_TXD"),
+	PINCTRL_PIN(87, "ISH_UART0_RTSB"),
+	PINCTRL_PIN(88, "ISH_UART0_CTSB"),
+	PINCTRL_PIN(89, "DMIC_CLK_1"),
+	PINCTRL_PIN(90, "DMIC_DATA_1"),
+	PINCTRL_PIN(91, "DMIC_CLK_0"),
+	PINCTRL_PIN(92, "DMIC_DATA_0"),
+	PINCTRL_PIN(93, "SPI1_IO_2"),
+	PINCTRL_PIN(94, "SPI1_IO_3"),
+	PINCTRL_PIN(95, "SSP_MCLK"),
+	/* GPP_E */
+	PINCTRL_PIN(96, "SATAXPCIE_0"),
+	PINCTRL_PIN(97, "SATAXPCIE_1"),
+	PINCTRL_PIN(98, "SATAXPCIE_2"),
+	PINCTRL_PIN(99, "CPU_GP_0"),
+	PINCTRL_PIN(100, "SATA_DEVSLP_0"),
+	PINCTRL_PIN(101, "SATA_DEVSLP_1"),
+	PINCTRL_PIN(102, "SATA_DEVSLP_2"),
+	PINCTRL_PIN(103, "CPU_GP_1"),
+	PINCTRL_PIN(104, "SATA_LEDB"),
+	PINCTRL_PIN(105, "USB2_OCB_0"),
+	PINCTRL_PIN(106, "USB2_OCB_1"),
+	PINCTRL_PIN(107, "USB2_OCB_2"),
+	PINCTRL_PIN(108, "USB2_OCB_3"),
+	PINCTRL_PIN(109, "DDSP_HPD_0"),
+	PINCTRL_PIN(110, "DDSP_HPD_1"),
+	PINCTRL_PIN(111, "DDSP_HPD_2"),
+	PINCTRL_PIN(112, "DDSP_HPD_3"),
+	PINCTRL_PIN(113, "EDP_HPD"),
+	PINCTRL_PIN(114, "DDPB_CTRLCLK"),
+	PINCTRL_PIN(115, "DDPB_CTRLDATA"),
+	PINCTRL_PIN(116, "DDPC_CTRLCLK"),
+	PINCTRL_PIN(117, "DDPC_CTRLDATA"),
+	PINCTRL_PIN(118, "DDPD_CTRLCLK"),
+	PINCTRL_PIN(119, "DDPD_CTRLDATA"),
+	/* GPP_F */
+	PINCTRL_PIN(120, "SSP2_SCLK"),
+	PINCTRL_PIN(121, "SSP2_SFRM"),
+	PINCTRL_PIN(122, "SSP2_TXD"),
+	PINCTRL_PIN(123, "SSP2_RXD"),
+	PINCTRL_PIN(124, "I2C2_SDA"),
+	PINCTRL_PIN(125, "I2C2_SCL"),
+	PINCTRL_PIN(126, "I2C3_SDA"),
+	PINCTRL_PIN(127, "I2C3_SCL"),
+	PINCTRL_PIN(128, "I2C4_SDA"),
+	PINCTRL_PIN(129, "I2C4_SCL"),
+	PINCTRL_PIN(130, "I2C5_SDA"),
+	PINCTRL_PIN(131, "I2C5_SCL"),
+	PINCTRL_PIN(132, "EMMC_CMD"),
+	PINCTRL_PIN(133, "EMMC_DATA_0"),
+	PINCTRL_PIN(134, "EMMC_DATA_1"),
+	PINCTRL_PIN(135, "EMMC_DATA_2"),
+	PINCTRL_PIN(136, "EMMC_DATA_3"),
+	PINCTRL_PIN(137, "EMMC_DATA_4"),
+	PINCTRL_PIN(138, "EMMC_DATA_5"),
+	PINCTRL_PIN(139, "EMMC_DATA_6"),
+	PINCTRL_PIN(140, "EMMC_DATA_7"),
+	PINCTRL_PIN(141, "EMMC_RCLK"),
+	PINCTRL_PIN(142, "EMMC_CLK"),
+	PINCTRL_PIN(143, "GPP_F_23"),
+	/* GPP_G */
+	PINCTRL_PIN(144, "SD_CMD"),
+	PINCTRL_PIN(145, "SD_DATA_0"),
+	PINCTRL_PIN(146, "SD_DATA_1"),
+	PINCTRL_PIN(147, "SD_DATA_2"),
+	PINCTRL_PIN(148, "SD_DATA_3"),
+	PINCTRL_PIN(149, "SD_CDB"),
+	PINCTRL_PIN(150, "SD_CLK"),
+	PINCTRL_PIN(151, "SD_WP"),
+};
+
+static const unsigned sptlp_spi0_pins[] = { 39, 40, 41, 42 };
+static const unsigned sptlp_spi1_pins[] = { 43, 44, 45, 46 };
+static const unsigned sptlp_uart0_pins[] = { 56, 57, 58, 59 };
+static const unsigned sptlp_uart1_pins[] = { 60, 61, 62, 63 };
+static const unsigned sptlp_uart2_pins[] = { 68, 69, 71, 71 };
+static const unsigned sptlp_i2c0_pins[] = { 64, 65 };
+static const unsigned sptlp_i2c1_pins[] = { 66, 67 };
+static const unsigned sptlp_i2c2_pins[] = { 124, 125 };
+static const unsigned sptlp_i2c3_pins[] = { 126, 127 };
+static const unsigned sptlp_i2c4_pins[] = { 128, 129 };
+static const unsigned sptlp_i2c4b_pins[] = { 85, 86 };
+static const unsigned sptlp_i2c5_pins[] = { 130, 131 };
+static const unsigned sptlp_ssp2_pins[] = { 120, 121, 122, 123 };
+static const unsigned sptlp_emmc_pins[] = {
+	132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142,
+};
+static const unsigned sptlp_sd_pins[] = {
+	144, 145, 146, 147, 148, 149, 150, 151,
+};
+
+static const struct intel_pingroup sptlp_groups[] = {
+	PIN_GROUP("spi0_grp", sptlp_spi0_pins, 1),
+	PIN_GROUP("spi1_grp", sptlp_spi1_pins, 1),
+	PIN_GROUP("uart0_grp", sptlp_uart0_pins, 1),
+	PIN_GROUP("uart1_grp", sptlp_uart1_pins, 1),
+	PIN_GROUP("uart2_grp", sptlp_uart2_pins, 1),
+	PIN_GROUP("i2c0_grp", sptlp_i2c0_pins, 1),
+	PIN_GROUP("i2c1_grp", sptlp_i2c1_pins, 1),
+	PIN_GROUP("i2c2_grp", sptlp_i2c2_pins, 1),
+	PIN_GROUP("i2c3_grp", sptlp_i2c3_pins, 1),
+	PIN_GROUP("i2c4_grp", sptlp_i2c4_pins, 1),
+	PIN_GROUP("i2c4b_grp", sptlp_i2c4b_pins, 3),
+	PIN_GROUP("i2c5_grp", sptlp_i2c5_pins, 1),
+	PIN_GROUP("ssp2_grp", sptlp_ssp2_pins, 1),
+	PIN_GROUP("emmc_grp", sptlp_emmc_pins, 1),
+	PIN_GROUP("sd_grp", sptlp_sd_pins, 1),
+};
+
+static const char * const sptlp_spi0_groups[] = { "spi0_grp" };
+static const char * const sptlp_spi1_groups[] = { "spi0_grp" };
+static const char * const sptlp_uart0_groups[] = { "uart0_grp" };
+static const char * const sptlp_uart1_groups[] = { "uart1_grp" };
+static const char * const sptlp_uart2_groups[] = { "uart2_grp" };
+static const char * const sptlp_i2c0_groups[] = { "i2c0_grp" };
+static const char * const sptlp_i2c1_groups[] = { "i2c1_grp" };
+static const char * const sptlp_i2c2_groups[] = { "i2c2_grp" };
+static const char * const sptlp_i2c3_groups[] = { "i2c3_grp" };
+static const char * const sptlp_i2c4_groups[] = { "i2c4_grp", "i2c4b_grp" };
+static const char * const sptlp_i2c5_groups[] = { "i2c5_grp" };
+static const char * const sptlp_ssp2_groups[] = { "ssp2_grp" };
+static const char * const sptlp_emmc_groups[] = { "emmc_grp" };
+static const char * const sptlp_sd_groups[] = { "sd_grp" };
+
+static const struct intel_function sptlp_functions[] = {
+	FUNCTION("spi0", sptlp_spi0_groups),
+	FUNCTION("spi1", sptlp_spi1_groups),
+	FUNCTION("uart0", sptlp_uart0_groups),
+	FUNCTION("uart1", sptlp_uart1_groups),
+	FUNCTION("uart2", sptlp_uart2_groups),
+	FUNCTION("i2c0", sptlp_i2c0_groups),
+	FUNCTION("i2c1", sptlp_i2c1_groups),
+	FUNCTION("i2c2", sptlp_i2c2_groups),
+	FUNCTION("i2c3", sptlp_i2c3_groups),
+	FUNCTION("i2c4", sptlp_i2c4_groups),
+	FUNCTION("i2c5", sptlp_i2c5_groups),
+	FUNCTION("ssp2", sptlp_ssp2_groups),
+	FUNCTION("emmc", sptlp_emmc_groups),
+	FUNCTION("sd", sptlp_sd_groups),
+};
+
+static const struct intel_community sptlp_communities[] = {
+	SPT_COMMUNITY(0, 0, 47),
+	SPT_COMMUNITY(1, 48, 119),
+	SPT_COMMUNITY(2, 120, 151),
+};
+
+static const struct intel_pinctrl_soc_data sptlp_soc_data = {
+	.pins = sptlp_pins,
+	.npins = ARRAY_SIZE(sptlp_pins),
+	.groups = sptlp_groups,
+	.ngroups = ARRAY_SIZE(sptlp_groups),
+	.functions = sptlp_functions,
+	.nfunctions = ARRAY_SIZE(sptlp_functions),
+	.communities = sptlp_communities,
+	.ncommunities = ARRAY_SIZE(sptlp_communities),
+};
+
+static const struct acpi_device_id spt_pinctrl_acpi_match[] = {
+	{ "INT344B", (kernel_ulong_t)&sptlp_soc_data },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, spt_pinctrl_acpi_match);
+
+static int spt_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct intel_pinctrl_soc_data *soc_data;
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(spt_pinctrl_acpi_match, &pdev->dev);
+	if (!id || !id->driver_data)
+		return -ENODEV;
+
+	soc_data = (const struct intel_pinctrl_soc_data *)id->driver_data;
+	return intel_pinctrl_probe(pdev, soc_data);
+}
+
+static const struct dev_pm_ops spt_pinctrl_pm_ops = {
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(intel_pinctrl_suspend,
+				     intel_pinctrl_resume)
+};
+
+static struct platform_driver spt_pinctrl_driver = {
+	.probe = spt_pinctrl_probe,
+	.remove = intel_pinctrl_remove,
+	.driver = {
+		.name = "sunrisepoint-pinctrl",
+		.acpi_match_table = spt_pinctrl_acpi_match,
+		.pm = &spt_pinctrl_pm_ops,
+	},
+};
+
+static int __init spt_pinctrl_init(void)
+{
+	return platform_driver_register(&spt_pinctrl_driver);
+}
+subsys_initcall(spt_pinctrl_init);
+
+static void __exit spt_pinctrl_exit(void)
+{
+	platform_driver_unregister(&spt_pinctrl_driver);
+}
+module_exit(spt_pinctrl_exit);
+
+MODULE_AUTHOR("Mathias Nyman <mathias.nyman@linux.intel.com>");
+MODULE_AUTHOR("Mika Westerberg <mika.westerberg@linux.intel.com>");
+MODULE_DESCRIPTION("Intel Sunrisepoint PCH pinctrl/GPIO driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/mediatek/Kconfig b/drivers/pinctrl/mediatek/Kconfig
new file mode 100644
index 000000000000..5983cf5b2c46
--- /dev/null
+++ b/drivers/pinctrl/mediatek/Kconfig
@@ -0,0 +1,23 @@
+if ARCH_MEDIATEK || COMPILE_TEST
+
+config PINCTRL_MTK_COMMON
+	bool
+	select PINMUX
+	select GENERIC_PINCONF
+	select GPIOLIB
+	select OF_GPIO
+
+# For ARMv7 SoCs
+config PINCTRL_MT8135
+	bool "Mediatek MT8135 pin control" if COMPILE_TEST && !MACH_MT8135
+	default MACH_MT8135
+	select PINCTRL_MTK_COMMON
+
+# For ARMv8 SoCs
+config PINCTRL_MT8173
+	bool "Mediatek MT8173 pin control"
+	depends on ARM64 || COMPILE_TEST
+	default ARM64 && ARCH_MEDIATEK
+	select PINCTRL_MTK_COMMON
+
+endif
diff --git a/drivers/pinctrl/mediatek/Makefile b/drivers/pinctrl/mediatek/Makefile
new file mode 100644
index 000000000000..d8606a2179cf
--- /dev/null
+++ b/drivers/pinctrl/mediatek/Makefile
@@ -0,0 +1,6 @@
+# Core
+obj-$(CONFIG_PINCTRL_MTK_COMMON)	+= pinctrl-mtk-common.o
+
+# SoC Drivers
+obj-$(CONFIG_PINCTRL_MT8135)		+= pinctrl-mt8135.o
+obj-$(CONFIG_PINCTRL_MT8173)		+= pinctrl-mt8173.o
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8135.c b/drivers/pinctrl/mediatek/pinctrl-mt8135.c
new file mode 100644
index 000000000000..f1e1e187ce96
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8135.c
@@ -0,0 +1,376 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/regmap.h>
+#include <dt-bindings/pinctrl/mt65xx.h>
+
+#include "pinctrl-mtk-common.h"
+#include "pinctrl-mtk-mt8135.h"
+
+#define DRV_BASE1				0x500
+#define DRV_BASE2				0x510
+#define PUPD_BASE1				0x400
+#define PUPD_BASE2				0x450
+#define R0_BASE1				0x4d0
+#define R1_BASE1				0x200
+#define R1_BASE2				0x250
+
+struct mtk_spec_pull_set {
+	unsigned int pin;
+	unsigned int pupd_offset;
+	unsigned char pupd_bit;
+	unsigned int r0_offset;
+	unsigned char r0_bit;
+	unsigned int r1_offset;
+	unsigned char r1_bit;
+};
+
+#define SPEC_PULL(_pin, _pupd_offset, _pupd_bit, _r0_offset, \
+	_r0_bit, _r1_offset, _r1_bit)	\
+	{	\
+		.pin = _pin,	\
+		.pupd_offset = _pupd_offset,	\
+		.pupd_bit = _pupd_bit,	\
+		.r0_offset = _r0_offset, \
+		.r0_bit = _r0_bit, \
+		.r1_offset = _r1_offset, \
+		.r1_bit = _r1_bit, \
+	}
+
+static const struct mtk_drv_group_desc mt8135_drv_grp[] =  {
+	/* E8E4E2 2/4/6/8/10/12/14/16 */
+	MTK_DRV_GRP(2, 16, 0, 2, 2),
+	/* E8E4  4/8/12/16 */
+	MTK_DRV_GRP(4, 16, 1, 2, 4),
+	/* E4E2  2/4/6/8 */
+	MTK_DRV_GRP(2, 8, 0, 1, 2),
+	/* E16E8E4 4/8/12/16/20/24/28/32 */
+	MTK_DRV_GRP(4, 32, 0, 2, 4)
+};
+
+static const struct mtk_pin_drv_grp mt8135_pin_drv[] = {
+	MTK_PIN_DRV_GRP(0, DRV_BASE1, 0, 0),
+	MTK_PIN_DRV_GRP(1, DRV_BASE1, 0, 0),
+	MTK_PIN_DRV_GRP(2, DRV_BASE1, 0, 0),
+	MTK_PIN_DRV_GRP(3, DRV_BASE1, 0, 0),
+	MTK_PIN_DRV_GRP(4, DRV_BASE1, 4, 0),
+	MTK_PIN_DRV_GRP(5, DRV_BASE1, 8, 0),
+	MTK_PIN_DRV_GRP(6, DRV_BASE1, 0, 0),
+	MTK_PIN_DRV_GRP(7, DRV_BASE1, 0, 0),
+	MTK_PIN_DRV_GRP(8, DRV_BASE1, 0, 0),
+	MTK_PIN_DRV_GRP(9, DRV_BASE1, 0, 0),
+
+	MTK_PIN_DRV_GRP(10, DRV_BASE1, 12, 1),
+	MTK_PIN_DRV_GRP(11, DRV_BASE1, 12, 1),
+	MTK_PIN_DRV_GRP(12, DRV_BASE1, 12, 1),
+	MTK_PIN_DRV_GRP(13, DRV_BASE1, 12, 1),
+	MTK_PIN_DRV_GRP(14, DRV_BASE1, 12, 1),
+	MTK_PIN_DRV_GRP(15, DRV_BASE1, 12, 1),
+	MTK_PIN_DRV_GRP(16, DRV_BASE1, 12, 1),
+	MTK_PIN_DRV_GRP(17, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(18, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(19, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(20, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(21, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(22, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(23, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(24, DRV_BASE1, 16, 1),
+	MTK_PIN_DRV_GRP(33, DRV_BASE1, 24, 1),
+	MTK_PIN_DRV_GRP(34, DRV_BASE2, 12, 2),
+	MTK_PIN_DRV_GRP(37, DRV_BASE2, 20, 1),
+	MTK_PIN_DRV_GRP(38, DRV_BASE2, 20, 1),
+	MTK_PIN_DRV_GRP(39, DRV_BASE2, 20, 1),
+	MTK_PIN_DRV_GRP(40, DRV_BASE2, 24, 1),
+	MTK_PIN_DRV_GRP(41, DRV_BASE2, 24, 1),
+	MTK_PIN_DRV_GRP(42, DRV_BASE2, 24, 1),
+	MTK_PIN_DRV_GRP(43, DRV_BASE2, 28, 1),
+	MTK_PIN_DRV_GRP(44, DRV_BASE2, 28, 1),
+	MTK_PIN_DRV_GRP(45, DRV_BASE2, 28, 1),
+	MTK_PIN_DRV_GRP(46, DRV_BASE2, 28, 1),
+	MTK_PIN_DRV_GRP(47, DRV_BASE2, 28, 1),
+
+	MTK_PIN_DRV_GRP(49, DRV_BASE2+0x10, 0, 1),
+	MTK_PIN_DRV_GRP(50, DRV_BASE2+0x10, 4, 1),
+	MTK_PIN_DRV_GRP(51, DRV_BASE2+0x10, 8, 1),
+	MTK_PIN_DRV_GRP(52, DRV_BASE2+0x10, 12, 2),
+	MTK_PIN_DRV_GRP(53, DRV_BASE2+0x10, 16, 1),
+	MTK_PIN_DRV_GRP(54, DRV_BASE2+0x10, 20, 1),
+	MTK_PIN_DRV_GRP(55, DRV_BASE2+0x10, 24, 1),
+	MTK_PIN_DRV_GRP(56, DRV_BASE2+0x10, 28, 1),
+
+	MTK_PIN_DRV_GRP(57, DRV_BASE2+0x20, 0, 1),
+	MTK_PIN_DRV_GRP(58, DRV_BASE2+0x20, 0, 1),
+	MTK_PIN_DRV_GRP(59, DRV_BASE2+0x20, 0, 1),
+	MTK_PIN_DRV_GRP(60, DRV_BASE2+0x20, 0, 1),
+	MTK_PIN_DRV_GRP(61, DRV_BASE2+0x20, 0, 1),
+	MTK_PIN_DRV_GRP(62, DRV_BASE2+0x20, 0, 1),
+	MTK_PIN_DRV_GRP(63, DRV_BASE2+0x20, 4, 1),
+	MTK_PIN_DRV_GRP(64, DRV_BASE2+0x20, 8, 1),
+	MTK_PIN_DRV_GRP(65, DRV_BASE2+0x20, 12, 1),
+	MTK_PIN_DRV_GRP(66, DRV_BASE2+0x20, 16, 1),
+	MTK_PIN_DRV_GRP(67, DRV_BASE2+0x20, 20, 1),
+	MTK_PIN_DRV_GRP(68, DRV_BASE2+0x20, 24, 1),
+	MTK_PIN_DRV_GRP(69, DRV_BASE2+0x20, 28, 1),
+
+	MTK_PIN_DRV_GRP(70, DRV_BASE2+0x30, 0, 1),
+	MTK_PIN_DRV_GRP(71, DRV_BASE2+0x30, 4, 1),
+	MTK_PIN_DRV_GRP(72, DRV_BASE2+0x30, 8, 1),
+	MTK_PIN_DRV_GRP(73, DRV_BASE2+0x30, 12, 1),
+	MTK_PIN_DRV_GRP(74, DRV_BASE2+0x30, 16, 1),
+	MTK_PIN_DRV_GRP(75, DRV_BASE2+0x30, 20, 1),
+	MTK_PIN_DRV_GRP(76, DRV_BASE2+0x30, 24, 1),
+	MTK_PIN_DRV_GRP(77, DRV_BASE2+0x30, 28, 3),
+	MTK_PIN_DRV_GRP(78, DRV_BASE2+0x30, 28, 3),
+
+	MTK_PIN_DRV_GRP(79, DRV_BASE2+0x40, 0, 3),
+	MTK_PIN_DRV_GRP(80, DRV_BASE2+0x40, 4, 3),
+
+	MTK_PIN_DRV_GRP(81, DRV_BASE2+0x30, 28, 3),
+	MTK_PIN_DRV_GRP(82, DRV_BASE2+0x30, 28, 3),
+
+	MTK_PIN_DRV_GRP(83, DRV_BASE2+0x40, 8, 3),
+	MTK_PIN_DRV_GRP(84, DRV_BASE2+0x40, 8, 3),
+	MTK_PIN_DRV_GRP(85, DRV_BASE2+0x40, 12, 3),
+	MTK_PIN_DRV_GRP(86, DRV_BASE2+0x40, 16, 3),
+	MTK_PIN_DRV_GRP(87, DRV_BASE2+0x40, 8, 3),
+	MTK_PIN_DRV_GRP(88, DRV_BASE2+0x40, 8, 3),
+
+	MTK_PIN_DRV_GRP(89, DRV_BASE2+0x50, 12, 0),
+	MTK_PIN_DRV_GRP(90, DRV_BASE2+0x50, 12, 0),
+	MTK_PIN_DRV_GRP(91, DRV_BASE2+0x50, 12, 0),
+	MTK_PIN_DRV_GRP(92, DRV_BASE2+0x50, 12, 0),
+	MTK_PIN_DRV_GRP(93, DRV_BASE2+0x50, 12, 0),
+	MTK_PIN_DRV_GRP(94, DRV_BASE2+0x50, 12, 0),
+	MTK_PIN_DRV_GRP(95, DRV_BASE2+0x50, 12, 0),
+
+	MTK_PIN_DRV_GRP(96, DRV_BASE1+0xb0, 28, 0),
+
+	MTK_PIN_DRV_GRP(97, DRV_BASE2+0x50, 12, 0),
+	MTK_PIN_DRV_GRP(98, DRV_BASE2+0x50, 16, 0),
+	MTK_PIN_DRV_GRP(99, DRV_BASE2+0x50, 20, 1),
+	MTK_PIN_DRV_GRP(102, DRV_BASE2+0x50, 24, 1),
+	MTK_PIN_DRV_GRP(103, DRV_BASE2+0x50, 28, 1),
+
+
+	MTK_PIN_DRV_GRP(104, DRV_BASE2+0x60, 0, 1),
+	MTK_PIN_DRV_GRP(105, DRV_BASE2+0x60, 4, 1),
+	MTK_PIN_DRV_GRP(106, DRV_BASE2+0x60, 4, 1),
+	MTK_PIN_DRV_GRP(107, DRV_BASE2+0x60, 4, 1),
+	MTK_PIN_DRV_GRP(108, DRV_BASE2+0x60, 4, 1),
+	MTK_PIN_DRV_GRP(109, DRV_BASE2+0x60, 8, 2),
+	MTK_PIN_DRV_GRP(110, DRV_BASE2+0x60, 12, 2),
+	MTK_PIN_DRV_GRP(111, DRV_BASE2+0x60, 16, 2),
+	MTK_PIN_DRV_GRP(112, DRV_BASE2+0x60, 20, 2),
+	MTK_PIN_DRV_GRP(113, DRV_BASE2+0x60, 24, 2),
+	MTK_PIN_DRV_GRP(114, DRV_BASE2+0x60, 28, 2),
+
+	MTK_PIN_DRV_GRP(115, DRV_BASE2+0x70, 0, 2),
+	MTK_PIN_DRV_GRP(116, DRV_BASE2+0x70, 4, 2),
+	MTK_PIN_DRV_GRP(117, DRV_BASE2+0x70, 8, 2),
+	MTK_PIN_DRV_GRP(118, DRV_BASE2+0x70, 12, 2),
+	MTK_PIN_DRV_GRP(119, DRV_BASE2+0x70, 16, 2),
+	MTK_PIN_DRV_GRP(120, DRV_BASE2+0x70, 20, 2),
+
+	MTK_PIN_DRV_GRP(181, DRV_BASE1+0xa0, 12, 1),
+	MTK_PIN_DRV_GRP(182, DRV_BASE1+0xa0, 16, 1),
+	MTK_PIN_DRV_GRP(183, DRV_BASE1+0xa0, 20, 1),
+	MTK_PIN_DRV_GRP(184, DRV_BASE1+0xa0, 24, 1),
+	MTK_PIN_DRV_GRP(185, DRV_BASE1+0xa0, 28, 1),
+
+	MTK_PIN_DRV_GRP(186, DRV_BASE1+0xb0, 0, 2),
+	MTK_PIN_DRV_GRP(187, DRV_BASE1+0xb0, 0, 2),
+	MTK_PIN_DRV_GRP(188, DRV_BASE1+0xb0, 0, 2),
+	MTK_PIN_DRV_GRP(189, DRV_BASE1+0xb0, 0, 2),
+	MTK_PIN_DRV_GRP(190, DRV_BASE1+0xb0, 4, 1),
+	MTK_PIN_DRV_GRP(191, DRV_BASE1+0xb0, 8, 1),
+	MTK_PIN_DRV_GRP(192, DRV_BASE1+0xb0, 12, 1),
+
+	MTK_PIN_DRV_GRP(197, DRV_BASE1+0xb0, 16, 0),
+	MTK_PIN_DRV_GRP(198, DRV_BASE1+0xb0, 16, 0),
+	MTK_PIN_DRV_GRP(199, DRV_BASE1+0xb0, 20, 0),
+	MTK_PIN_DRV_GRP(200, DRV_BASE1+0xb0, 24, 0),
+	MTK_PIN_DRV_GRP(201, DRV_BASE1+0xb0, 16, 0),
+	MTK_PIN_DRV_GRP(202, DRV_BASE1+0xb0, 16, 0)
+};
+
+static const struct mtk_spec_pull_set spec_pupd[] = {
+	SPEC_PULL(0, PUPD_BASE1, 0, R0_BASE1, 9, R1_BASE1, 0),
+	SPEC_PULL(1, PUPD_BASE1, 1, R0_BASE1, 8, R1_BASE1, 1),
+	SPEC_PULL(2, PUPD_BASE1, 2, R0_BASE1, 7, R1_BASE1, 2),
+	SPEC_PULL(3, PUPD_BASE1, 3, R0_BASE1, 6, R1_BASE1, 3),
+	SPEC_PULL(4, PUPD_BASE1, 4, R0_BASE1, 1, R1_BASE1, 4),
+	SPEC_PULL(5, PUPD_BASE1, 5, R0_BASE1, 0, R1_BASE1, 5),
+	SPEC_PULL(6, PUPD_BASE1, 6, R0_BASE1, 5, R1_BASE1, 6),
+	SPEC_PULL(7, PUPD_BASE1, 7, R0_BASE1, 4, R1_BASE1, 7),
+	SPEC_PULL(8, PUPD_BASE1, 8, R0_BASE1, 3, R1_BASE1, 8),
+	SPEC_PULL(9, PUPD_BASE1, 9, R0_BASE1, 2, R1_BASE1, 9),
+	SPEC_PULL(89, PUPD_BASE2, 9, R0_BASE1, 18, R1_BASE2, 9),
+	SPEC_PULL(90, PUPD_BASE2, 10, R0_BASE1, 19, R1_BASE2, 10),
+	SPEC_PULL(91, PUPD_BASE2, 11, R0_BASE1, 23, R1_BASE2, 11),
+	SPEC_PULL(92, PUPD_BASE2, 12, R0_BASE1, 24, R1_BASE2, 12),
+	SPEC_PULL(93, PUPD_BASE2, 13, R0_BASE1, 25, R1_BASE2, 13),
+	SPEC_PULL(94, PUPD_BASE2, 14, R0_BASE1, 22, R1_BASE2, 14),
+	SPEC_PULL(95, PUPD_BASE2, 15, R0_BASE1, 20, R1_BASE2, 15),
+	SPEC_PULL(96, PUPD_BASE2+0x10, 0, R0_BASE1, 16, R1_BASE2+0x10, 0),
+	SPEC_PULL(97, PUPD_BASE2+0x10, 1, R0_BASE1, 21, R1_BASE2+0x10, 1),
+	SPEC_PULL(98, PUPD_BASE2+0x10, 2, R0_BASE1, 17, R1_BASE2+0x10, 2),
+	SPEC_PULL(197, PUPD_BASE1+0xc0, 5, R0_BASE1, 13, R1_BASE2+0xc0, 5),
+	SPEC_PULL(198, PUPD_BASE2+0xc0, 6, R0_BASE1, 14, R1_BASE2+0xc0, 6),
+	SPEC_PULL(199, PUPD_BASE2+0xc0, 7, R0_BASE1, 11, R1_BASE2+0xc0, 7),
+	SPEC_PULL(200, PUPD_BASE2+0xc0, 8, R0_BASE1, 10, R1_BASE2+0xc0, 8),
+	SPEC_PULL(201, PUPD_BASE2+0xc0, 9, R0_BASE1, 13, R1_BASE2+0xc0, 9),
+	SPEC_PULL(202, PUPD_BASE2+0xc0, 10, R0_BASE1, 12, R1_BASE2+0xc0, 10)
+};
+
+static int spec_pull_set(struct regmap *regmap, unsigned int pin,
+		unsigned char align, bool isup, unsigned int r1r0)
+{
+	unsigned int i;
+	unsigned int reg_pupd, reg_set_r0, reg_set_r1;
+	unsigned int reg_rst_r0, reg_rst_r1;
+	bool find = false;
+
+	for (i = 0; i < ARRAY_SIZE(spec_pupd); i++) {
+		if (pin == spec_pupd[i].pin) {
+			find = true;
+			break;
+		}
+	}
+
+	if (!find)
+		return -EINVAL;
+
+	if (isup)
+		reg_pupd = spec_pupd[i].pupd_offset + align;
+	else
+		reg_pupd = spec_pupd[i].pupd_offset + (align << 1);
+
+	regmap_write(regmap, reg_pupd, spec_pupd[i].pupd_bit);
+
+	reg_set_r0 = spec_pupd[i].r0_offset + align;
+	reg_rst_r0 = spec_pupd[i].r0_offset + (align << 1);
+	reg_set_r1 = spec_pupd[i].r1_offset + align;
+	reg_rst_r1 = spec_pupd[i].r1_offset + (align << 1);
+
+	switch (r1r0) {
+	case MTK_PUPD_SET_R1R0_00:
+		regmap_write(regmap, reg_rst_r0, spec_pupd[i].r0_bit);
+		regmap_write(regmap, reg_rst_r1, spec_pupd[i].r1_bit);
+		break;
+	case MTK_PUPD_SET_R1R0_01:
+		regmap_write(regmap, reg_set_r0, spec_pupd[i].r0_bit);
+		regmap_write(regmap, reg_rst_r1, spec_pupd[i].r1_bit);
+		break;
+	case MTK_PUPD_SET_R1R0_10:
+		regmap_write(regmap, reg_rst_r0, spec_pupd[i].r0_bit);
+		regmap_write(regmap, reg_set_r1, spec_pupd[i].r1_bit);
+		break;
+	case MTK_PUPD_SET_R1R0_11:
+		regmap_write(regmap, reg_set_r0, spec_pupd[i].r0_bit);
+		regmap_write(regmap, reg_set_r1, spec_pupd[i].r1_bit);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct mtk_pinctrl_devdata mt8135_pinctrl_data = {
+	.pins = mtk_pins_mt8135,
+	.npins = ARRAY_SIZE(mtk_pins_mt8135),
+	.grp_desc = mt8135_drv_grp,
+	.n_grp_cls = ARRAY_SIZE(mt8135_drv_grp),
+	.pin_drv_grp = mt8135_pin_drv,
+	.n_pin_drv_grps = ARRAY_SIZE(mt8135_pin_drv),
+	.spec_pull_set = spec_pull_set,
+	.dir_offset = 0x0000,
+	.ies_offset = 0x0100,
+	.pullen_offset = 0x0200,
+	.smt_offset = 0x0300,
+	.pullsel_offset = 0x0400,
+	.invser_offset = 0x0600,
+	.dout_offset = 0x0800,
+	.din_offset = 0x0A00,
+	.pinmux_offset = 0x0C00,
+	.type1_start = 34,
+	.type1_end = 149,
+	.port_shf = 4,
+	.port_mask = 0xf,
+	.port_align = 4,
+	.chip_type = MTK_CHIP_TYPE_BASE,
+	.eint_offsets = {
+		.name = "mt8135_eint",
+		.stat      = 0x000,
+		.ack       = 0x040,
+		.mask      = 0x080,
+		.mask_set  = 0x0c0,
+		.mask_clr  = 0x100,
+		.sens      = 0x140,
+		.sens_set  = 0x180,
+		.sens_clr  = 0x1c0,
+		.soft      = 0x200,
+		.soft_set  = 0x240,
+		.soft_clr  = 0x280,
+		.pol       = 0x300,
+		.pol_set   = 0x340,
+		.pol_clr   = 0x380,
+		.dom_en    = 0x400,
+		.dbnc_ctrl = 0x500,
+		.dbnc_set  = 0x600,
+		.dbnc_clr  = 0x700,
+		.port_mask = 7,
+		.ports     = 6,
+	},
+	.ap_num = 192,
+	.db_cnt = 16,
+};
+
+static int mt8135_pinctrl_probe(struct platform_device *pdev)
+{
+	return mtk_pctrl_init(pdev, &mt8135_pinctrl_data);
+}
+
+static const struct of_device_id mt8135_pctrl_match[] = {
+	{
+		.compatible = "mediatek,mt8135-pinctrl",
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mt8135_pctrl_match);
+
+static struct platform_driver mtk_pinctrl_driver = {
+	.probe = mt8135_pinctrl_probe,
+	.driver = {
+		.name = "mediatek-mt8135-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = mt8135_pctrl_match,
+	},
+};
+
+static int __init mtk_pinctrl_init(void)
+{
+	return platform_driver_register(&mtk_pinctrl_driver);
+}
+
+module_init(mtk_pinctrl_init);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MediaTek Pinctrl Driver");
+MODULE_AUTHOR("Hongzhou Yang <hongzhou.yang@mediatek.com>");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8173.c b/drivers/pinctrl/mediatek/pinctrl-mt8173.c
new file mode 100644
index 000000000000..412ea84836a1
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8173.c
@@ -0,0 +1,455 @@
+/*
+ * Copyright (c) 2014-2015 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/regmap.h>
+#include <dt-bindings/pinctrl/mt65xx.h>
+
+#include "pinctrl-mtk-common.h"
+#include "pinctrl-mtk-mt8173.h"
+
+#define DRV_BASE				0xb00
+
+/**
+ * struct mtk_pin_ies_smt_set - For special pins' ies and smt setting.
+ * @start: The start pin number of those special pins.
+ * @end: The end pin number of those special pins.
+ * @offset: The offset of special setting register.
+ * @bit: The bit of special setting register.
+ */
+struct mtk_pin_ies_smt_set {
+	unsigned int start;
+	unsigned int end;
+	unsigned int offset;
+	unsigned char bit;
+};
+
+#define MTK_PIN_IES_SMT_SET(_start, _end, _offset, _bit)	\
+	{	\
+		.start = _start,	\
+		.end = _end,	\
+		.bit = _bit,	\
+		.offset = _offset,	\
+	}
+
+/**
+ * struct mtk_pin_spec_pupd_set - For special pins' pull up/down setting.
+ * @pin: The pin number.
+ * @offset: The offset of special pull up/down setting register.
+ * @pupd_bit: The pull up/down bit in this register.
+ * @r0_bit: The r0 bit of pull resistor.
+ * @r1_bit: The r1 bit of pull resistor.
+ */
+struct mtk_pin_spec_pupd_set {
+	unsigned int pin;
+	unsigned int offset;
+	unsigned char pupd_bit;
+	unsigned char r1_bit;
+	unsigned char r0_bit;
+};
+
+#define MTK_PIN_PUPD_SPEC(_pin, _offset, _pupd, _r1, _r0)	\
+	{	\
+		.pin = _pin,	\
+		.offset = _offset,	\
+		.pupd_bit = _pupd,	\
+		.r1_bit = _r1,		\
+		.r0_bit = _r0,		\
+	}
+
+static const struct mtk_pin_spec_pupd_set mt8173_spec_pupd[] = {
+	MTK_PIN_PUPD_SPEC(119, 0xe00, 2, 1, 0),  /* KROW0 */
+	MTK_PIN_PUPD_SPEC(120, 0xe00, 6, 5, 4),  /* KROW1 */
+	MTK_PIN_PUPD_SPEC(121, 0xe00, 10, 9, 8), /* KROW2 */
+	MTK_PIN_PUPD_SPEC(122, 0xe10, 2, 1, 0),  /* KCOL0 */
+	MTK_PIN_PUPD_SPEC(123, 0xe10, 6, 5, 4),  /* KCOL1 */
+	MTK_PIN_PUPD_SPEC(124, 0xe10, 10, 9, 8), /* KCOL2 */
+
+	MTK_PIN_PUPD_SPEC(67, 0xd10, 2, 1, 0),   /* ms0 DS */
+	MTK_PIN_PUPD_SPEC(68, 0xd00, 2, 1, 0),   /* ms0 RST */
+	MTK_PIN_PUPD_SPEC(66, 0xc10, 2, 1, 0),   /* ms0 cmd */
+	MTK_PIN_PUPD_SPEC(65, 0xc00, 2, 1, 0),   /* ms0 clk */
+	MTK_PIN_PUPD_SPEC(57, 0xc20, 2, 1, 0),   /* ms0 data0 */
+	MTK_PIN_PUPD_SPEC(58, 0xc20, 2, 1, 0),   /* ms0 data1 */
+	MTK_PIN_PUPD_SPEC(59, 0xc20, 2, 1, 0),   /* ms0 data2 */
+	MTK_PIN_PUPD_SPEC(60, 0xc20, 2, 1, 0),   /* ms0 data3 */
+	MTK_PIN_PUPD_SPEC(61, 0xc20, 2, 1, 0),   /* ms0 data4 */
+	MTK_PIN_PUPD_SPEC(62, 0xc20, 2, 1, 0),   /* ms0 data5 */
+	MTK_PIN_PUPD_SPEC(63, 0xc20, 2, 1, 0),   /* ms0 data6 */
+	MTK_PIN_PUPD_SPEC(64, 0xc20, 2, 1, 0),   /* ms0 data7 */
+
+	MTK_PIN_PUPD_SPEC(78, 0xc50, 2, 1, 0),    /* ms1 cmd */
+	MTK_PIN_PUPD_SPEC(73, 0xd20, 2, 1, 0),    /* ms1 dat0 */
+	MTK_PIN_PUPD_SPEC(74, 0xd20, 6, 5, 4),    /* ms1 dat1 */
+	MTK_PIN_PUPD_SPEC(75, 0xd20, 10, 9, 8),   /* ms1 dat2 */
+	MTK_PIN_PUPD_SPEC(76, 0xd20, 14, 13, 12), /* ms1 dat3 */
+	MTK_PIN_PUPD_SPEC(77, 0xc40, 2, 1, 0),    /* ms1 clk */
+
+	MTK_PIN_PUPD_SPEC(100, 0xd40, 2, 1, 0),    /* ms2 dat0 */
+	MTK_PIN_PUPD_SPEC(101, 0xd40, 6, 5, 4),    /* ms2 dat1 */
+	MTK_PIN_PUPD_SPEC(102, 0xd40, 10, 9, 8),   /* ms2 dat2 */
+	MTK_PIN_PUPD_SPEC(103, 0xd40, 14, 13, 12), /* ms2 dat3 */
+	MTK_PIN_PUPD_SPEC(104, 0xc80, 2, 1, 0),    /* ms2 clk */
+	MTK_PIN_PUPD_SPEC(105, 0xc90, 2, 1, 0),    /* ms2 cmd */
+
+	MTK_PIN_PUPD_SPEC(22, 0xd60, 2, 1, 0),    /* ms3 dat0 */
+	MTK_PIN_PUPD_SPEC(23, 0xd60, 6, 5, 4),    /* ms3 dat1 */
+	MTK_PIN_PUPD_SPEC(24, 0xd60, 10, 9, 8),   /* ms3 dat2 */
+	MTK_PIN_PUPD_SPEC(25, 0xd60, 14, 13, 12), /* ms3 dat3 */
+	MTK_PIN_PUPD_SPEC(26, 0xcc0, 2, 1, 0),    /* ms3 clk */
+	MTK_PIN_PUPD_SPEC(27, 0xcd0, 2, 1, 0)     /* ms3 cmd */
+};
+
+static int spec_pull_set(struct regmap *regmap, unsigned int pin,
+		unsigned char align, bool isup, unsigned int r1r0)
+{
+	unsigned int i;
+	unsigned int reg_pupd, reg_set, reg_rst;
+	unsigned int bit_pupd, bit_r0, bit_r1;
+	const struct mtk_pin_spec_pupd_set *spec_pupd_pin;
+	bool find = false;
+
+	for (i = 0; i < ARRAY_SIZE(mt8173_spec_pupd); i++) {
+		if (pin == mt8173_spec_pupd[i].pin) {
+			find = true;
+			break;
+		}
+	}
+
+	if (!find)
+		return -EINVAL;
+
+	spec_pupd_pin = mt8173_spec_pupd + i;
+	reg_set = spec_pupd_pin->offset + align;
+	reg_rst = spec_pupd_pin->offset + (align << 1);
+
+	if (isup)
+		reg_pupd = reg_rst;
+	else
+		reg_pupd = reg_set;
+
+	bit_pupd = BIT(spec_pupd_pin->pupd_bit);
+	regmap_write(regmap, reg_pupd, bit_pupd);
+
+	bit_r0 = BIT(spec_pupd_pin->r0_bit);
+	bit_r1 = BIT(spec_pupd_pin->r1_bit);
+
+	switch (r1r0) {
+	case MTK_PUPD_SET_R1R0_00:
+		regmap_write(regmap, reg_rst, bit_r0);
+		regmap_write(regmap, reg_rst, bit_r1);
+		break;
+	case MTK_PUPD_SET_R1R0_01:
+		regmap_write(regmap, reg_set, bit_r0);
+		regmap_write(regmap, reg_rst, bit_r1);
+		break;
+	case MTK_PUPD_SET_R1R0_10:
+		regmap_write(regmap, reg_rst, bit_r0);
+		regmap_write(regmap, reg_set, bit_r1);
+		break;
+	case MTK_PUPD_SET_R1R0_11:
+		regmap_write(regmap, reg_set, bit_r0);
+		regmap_write(regmap, reg_set, bit_r1);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct mtk_pin_ies_smt_set mt8173_ies_smt_set[] = {
+	MTK_PIN_IES_SMT_SET(0, 4, 0x930, 1),
+	MTK_PIN_IES_SMT_SET(5, 9, 0x930, 2),
+	MTK_PIN_IES_SMT_SET(10, 13, 0x930, 10),
+	MTK_PIN_IES_SMT_SET(14, 15, 0x940, 10),
+	MTK_PIN_IES_SMT_SET(16, 16, 0x930, 0),
+	MTK_PIN_IES_SMT_SET(17, 17, 0x950, 2),
+	MTK_PIN_IES_SMT_SET(18, 21, 0x940, 3),
+	MTK_PIN_IES_SMT_SET(29, 32, 0x930, 3),
+	MTK_PIN_IES_SMT_SET(33, 33, 0x930, 4),
+	MTK_PIN_IES_SMT_SET(34, 36, 0x930, 5),
+	MTK_PIN_IES_SMT_SET(37, 38, 0x930, 6),
+	MTK_PIN_IES_SMT_SET(39, 39, 0x930, 7),
+	MTK_PIN_IES_SMT_SET(40, 41, 0x930, 9),
+	MTK_PIN_IES_SMT_SET(42, 42, 0x940, 0),
+	MTK_PIN_IES_SMT_SET(43, 44, 0x930, 11),
+	MTK_PIN_IES_SMT_SET(45, 46, 0x930, 12),
+	MTK_PIN_IES_SMT_SET(57, 64, 0xc20, 13),
+	MTK_PIN_IES_SMT_SET(65, 65, 0xc10, 13),
+	MTK_PIN_IES_SMT_SET(66, 66, 0xc00, 13),
+	MTK_PIN_IES_SMT_SET(67, 67, 0xd10, 13),
+	MTK_PIN_IES_SMT_SET(68, 68, 0xd00, 13),
+	MTK_PIN_IES_SMT_SET(69, 72, 0x940, 14),
+	MTK_PIN_IES_SMT_SET(73, 76, 0xc60, 13),
+	MTK_PIN_IES_SMT_SET(77, 77, 0xc40, 13),
+	MTK_PIN_IES_SMT_SET(78, 78, 0xc50, 13),
+	MTK_PIN_IES_SMT_SET(79, 82, 0x940, 15),
+	MTK_PIN_IES_SMT_SET(83, 83, 0x950, 0),
+	MTK_PIN_IES_SMT_SET(84, 85, 0x950, 1),
+	MTK_PIN_IES_SMT_SET(86, 91, 0x950, 2),
+	MTK_PIN_IES_SMT_SET(92, 92, 0x930, 13),
+	MTK_PIN_IES_SMT_SET(93, 95, 0x930, 14),
+	MTK_PIN_IES_SMT_SET(96, 99, 0x930, 15),
+	MTK_PIN_IES_SMT_SET(100, 103, 0xca0, 13),
+	MTK_PIN_IES_SMT_SET(104, 104, 0xc80, 13),
+	MTK_PIN_IES_SMT_SET(105, 105, 0xc90, 13),
+	MTK_PIN_IES_SMT_SET(106, 107, 0x940, 4),
+	MTK_PIN_IES_SMT_SET(108, 112, 0x940, 1),
+	MTK_PIN_IES_SMT_SET(113, 116, 0x940, 2),
+	MTK_PIN_IES_SMT_SET(117, 118, 0x940, 5),
+	MTK_PIN_IES_SMT_SET(119, 124, 0x940, 6),
+	MTK_PIN_IES_SMT_SET(125, 126, 0x940, 7),
+	MTK_PIN_IES_SMT_SET(127, 127, 0x940, 0),
+	MTK_PIN_IES_SMT_SET(128, 128, 0x950, 8),
+	MTK_PIN_IES_SMT_SET(129, 130, 0x950, 9),
+	MTK_PIN_IES_SMT_SET(131, 132, 0x950, 8),
+	MTK_PIN_IES_SMT_SET(133, 134, 0x910, 8)
+};
+
+static int spec_ies_smt_set(struct regmap *regmap, unsigned int pin,
+		unsigned char align, int value)
+{
+	unsigned int i, reg_addr, bit;
+	bool find = false;
+
+	for (i = 0; i < ARRAY_SIZE(mt8173_ies_smt_set); i++) {
+		if (pin >= mt8173_ies_smt_set[i].start &&
+				pin <= mt8173_ies_smt_set[i].end) {
+			find = true;
+			break;
+		}
+	}
+
+	if (!find)
+		return -EINVAL;
+
+	if (value)
+		reg_addr = mt8173_ies_smt_set[i].offset + align;
+	else
+		reg_addr = mt8173_ies_smt_set[i].offset + (align << 1);
+
+	bit = BIT(mt8173_ies_smt_set[i].bit);
+	regmap_write(regmap, reg_addr, bit);
+	return 0;
+}
+
+static const struct mtk_drv_group_desc mt8173_drv_grp[] =  {
+	/* 0E4E8SR 4/8/12/16 */
+	MTK_DRV_GRP(4, 16, 1, 2, 4),
+	/* 0E2E4SR  2/4/6/8 */
+	MTK_DRV_GRP(2, 8, 1, 2, 2),
+	/* E8E4E2  2/4/6/8/10/12/14/16 */
+	MTK_DRV_GRP(2, 16, 0, 2, 2)
+};
+
+static const struct mtk_pin_drv_grp mt8173_pin_drv[] = {
+	MTK_PIN_DRV_GRP(0, DRV_BASE+0x20, 12, 0),
+	MTK_PIN_DRV_GRP(1, DRV_BASE+0x20, 12, 0),
+	MTK_PIN_DRV_GRP(2, DRV_BASE+0x20, 12, 0),
+	MTK_PIN_DRV_GRP(3, DRV_BASE+0x20, 12, 0),
+	MTK_PIN_DRV_GRP(4, DRV_BASE+0x20, 12, 0),
+	MTK_PIN_DRV_GRP(5, DRV_BASE+0x30, 0, 0),
+	MTK_PIN_DRV_GRP(6, DRV_BASE+0x30, 0, 0),
+	MTK_PIN_DRV_GRP(7, DRV_BASE+0x30, 0, 0),
+	MTK_PIN_DRV_GRP(8, DRV_BASE+0x30, 0, 0),
+	MTK_PIN_DRV_GRP(9, DRV_BASE+0x30, 0, 0),
+	MTK_PIN_DRV_GRP(10, DRV_BASE+0x30, 4, 1),
+	MTK_PIN_DRV_GRP(11, DRV_BASE+0x30, 4, 1),
+	MTK_PIN_DRV_GRP(12, DRV_BASE+0x30, 4, 1),
+	MTK_PIN_DRV_GRP(13, DRV_BASE+0x30, 4, 1),
+	MTK_PIN_DRV_GRP(14, DRV_BASE+0x40, 8, 1),
+	MTK_PIN_DRV_GRP(15, DRV_BASE+0x40, 8, 1),
+	MTK_PIN_DRV_GRP(16, DRV_BASE, 8, 1),
+	MTK_PIN_DRV_GRP(17, 0xce0, 8, 2),
+	MTK_PIN_DRV_GRP(22, 0xce0, 8, 2),
+	MTK_PIN_DRV_GRP(23, 0xce0, 8, 2),
+	MTK_PIN_DRV_GRP(24, 0xce0, 8, 2),
+	MTK_PIN_DRV_GRP(25, 0xce0, 8, 2),
+	MTK_PIN_DRV_GRP(26, 0xcc0, 8, 2),
+	MTK_PIN_DRV_GRP(27, 0xcd0, 8, 2),
+	MTK_PIN_DRV_GRP(28, 0xd70, 8, 2),
+	MTK_PIN_DRV_GRP(29, DRV_BASE+0x80, 12, 1),
+	MTK_PIN_DRV_GRP(30, DRV_BASE+0x80, 12, 1),
+	MTK_PIN_DRV_GRP(31, DRV_BASE+0x80, 12, 1),
+	MTK_PIN_DRV_GRP(32, DRV_BASE+0x80, 12, 1),
+	MTK_PIN_DRV_GRP(33, DRV_BASE+0x10, 12, 1),
+	MTK_PIN_DRV_GRP(34, DRV_BASE+0x10, 8, 1),
+	MTK_PIN_DRV_GRP(35, DRV_BASE+0x10, 8, 1),
+	MTK_PIN_DRV_GRP(36, DRV_BASE+0x10, 8, 1),
+	MTK_PIN_DRV_GRP(37, DRV_BASE+0x10, 4, 1),
+	MTK_PIN_DRV_GRP(38, DRV_BASE+0x10, 4, 1),
+	MTK_PIN_DRV_GRP(39, DRV_BASE+0x20, 0, 0),
+	MTK_PIN_DRV_GRP(40, DRV_BASE+0x20, 8, 0),
+	MTK_PIN_DRV_GRP(41, DRV_BASE+0x20, 8, 0),
+	MTK_PIN_DRV_GRP(42, DRV_BASE+0x50, 8, 1),
+	MTK_PIN_DRV_GRP(57, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(58, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(59, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(60, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(61, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(62, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(63, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(64, 0xc20, 8, 2),
+	MTK_PIN_DRV_GRP(65, 0xc00, 8, 2),
+	MTK_PIN_DRV_GRP(66, 0xc10, 8, 2),
+	MTK_PIN_DRV_GRP(67, 0xd10, 8, 2),
+	MTK_PIN_DRV_GRP(68, 0xd00, 8, 2),
+	MTK_PIN_DRV_GRP(69, DRV_BASE+0x80, 0, 1),
+	MTK_PIN_DRV_GRP(70, DRV_BASE+0x80, 0, 1),
+	MTK_PIN_DRV_GRP(71, DRV_BASE+0x80, 0, 1),
+	MTK_PIN_DRV_GRP(72, DRV_BASE+0x80, 0, 1),
+	MTK_PIN_DRV_GRP(73, 0xc60, 8, 2),
+	MTK_PIN_DRV_GRP(74, 0xc60, 8, 2),
+	MTK_PIN_DRV_GRP(75, 0xc60, 8, 2),
+	MTK_PIN_DRV_GRP(76, 0xc60, 8, 2),
+	MTK_PIN_DRV_GRP(77, 0xc40, 8, 2),
+	MTK_PIN_DRV_GRP(78, 0xc50, 8, 2),
+	MTK_PIN_DRV_GRP(79, DRV_BASE+0x70, 12, 1),
+	MTK_PIN_DRV_GRP(80, DRV_BASE+0x70, 12, 1),
+	MTK_PIN_DRV_GRP(81, DRV_BASE+0x70, 12, 1),
+	MTK_PIN_DRV_GRP(82, DRV_BASE+0x70, 12, 1),
+	MTK_PIN_DRV_GRP(83, DRV_BASE, 4, 1),
+	MTK_PIN_DRV_GRP(84, DRV_BASE, 0, 1),
+	MTK_PIN_DRV_GRP(85, DRV_BASE, 0, 1),
+	MTK_PIN_DRV_GRP(85, DRV_BASE+0x60, 8, 1),
+	MTK_PIN_DRV_GRP(86, DRV_BASE+0x60, 8, 1),
+	MTK_PIN_DRV_GRP(87, DRV_BASE+0x60, 8, 1),
+	MTK_PIN_DRV_GRP(88, DRV_BASE+0x60, 8, 1),
+	MTK_PIN_DRV_GRP(89, DRV_BASE+0x60, 8, 1),
+	MTK_PIN_DRV_GRP(90, DRV_BASE+0x60, 8, 1),
+	MTK_PIN_DRV_GRP(91, DRV_BASE+0x60, 8, 1),
+	MTK_PIN_DRV_GRP(92, DRV_BASE+0x60, 4, 0),
+	MTK_PIN_DRV_GRP(93, DRV_BASE+0x60, 0, 0),
+	MTK_PIN_DRV_GRP(94, DRV_BASE+0x60, 0, 0),
+	MTK_PIN_DRV_GRP(95, DRV_BASE+0x60, 0, 0),
+	MTK_PIN_DRV_GRP(96, DRV_BASE+0x80, 8, 1),
+	MTK_PIN_DRV_GRP(97, DRV_BASE+0x80, 8, 1),
+	MTK_PIN_DRV_GRP(98, DRV_BASE+0x80, 8, 1),
+	MTK_PIN_DRV_GRP(99, DRV_BASE+0x80, 8, 1),
+	MTK_PIN_DRV_GRP(100, 0xca0, 8, 2),
+	MTK_PIN_DRV_GRP(101, 0xca0, 8, 2),
+	MTK_PIN_DRV_GRP(102, 0xca0, 8, 2),
+	MTK_PIN_DRV_GRP(103, 0xca0, 8, 2),
+	MTK_PIN_DRV_GRP(104, 0xc80, 8, 2),
+	MTK_PIN_DRV_GRP(105, 0xc90, 8, 2),
+	MTK_PIN_DRV_GRP(108, DRV_BASE+0x50, 0, 1),
+	MTK_PIN_DRV_GRP(109, DRV_BASE+0x50, 0, 1),
+	MTK_PIN_DRV_GRP(110, DRV_BASE+0x50, 0, 1),
+	MTK_PIN_DRV_GRP(111, DRV_BASE+0x50, 0, 1),
+	MTK_PIN_DRV_GRP(112, DRV_BASE+0x50, 0, 1),
+	MTK_PIN_DRV_GRP(113, DRV_BASE+0x80, 4, 1),
+	MTK_PIN_DRV_GRP(114, DRV_BASE+0x80, 4, 1),
+	MTK_PIN_DRV_GRP(115, DRV_BASE+0x80, 4, 1),
+	MTK_PIN_DRV_GRP(116, DRV_BASE+0x80, 4, 1),
+	MTK_PIN_DRV_GRP(117, DRV_BASE+0x90, 0, 1),
+	MTK_PIN_DRV_GRP(118, DRV_BASE+0x90, 0, 1),
+	MTK_PIN_DRV_GRP(119, DRV_BASE+0x50, 4, 1),
+	MTK_PIN_DRV_GRP(120, DRV_BASE+0x50, 4, 1),
+	MTK_PIN_DRV_GRP(121, DRV_BASE+0x50, 4, 1),
+	MTK_PIN_DRV_GRP(122, DRV_BASE+0x50, 4, 1),
+	MTK_PIN_DRV_GRP(123, DRV_BASE+0x50, 4, 1),
+	MTK_PIN_DRV_GRP(124, DRV_BASE+0x50, 4, 1),
+	MTK_PIN_DRV_GRP(125, DRV_BASE+0x30, 12, 1),
+	MTK_PIN_DRV_GRP(126, DRV_BASE+0x30, 12, 1),
+	MTK_PIN_DRV_GRP(127, DRV_BASE+0x50, 8, 1),
+	MTK_PIN_DRV_GRP(128, DRV_BASE+0x40, 0, 1),
+	MTK_PIN_DRV_GRP(129, DRV_BASE+0x40, 0, 1),
+	MTK_PIN_DRV_GRP(130, DRV_BASE+0x40, 0, 1),
+	MTK_PIN_DRV_GRP(131, DRV_BASE+0x40, 0, 1),
+	MTK_PIN_DRV_GRP(132, DRV_BASE+0x40, 0, 1)
+};
+
+static const struct mtk_pinctrl_devdata mt8173_pinctrl_data = {
+	.pins = mtk_pins_mt8173,
+	.npins = ARRAY_SIZE(mtk_pins_mt8173),
+	.grp_desc = mt8173_drv_grp,
+	.n_grp_cls = ARRAY_SIZE(mt8173_drv_grp),
+	.pin_drv_grp = mt8173_pin_drv,
+	.n_pin_drv_grps = ARRAY_SIZE(mt8173_pin_drv),
+	.spec_pull_set = spec_pull_set,
+	.spec_ies_smt_set = spec_ies_smt_set,
+	.dir_offset = 0x0000,
+	.pullen_offset = 0x0100,
+	.pullsel_offset = 0x0200,
+	.dout_offset = 0x0400,
+	.din_offset = 0x0500,
+	.pinmux_offset = 0x0600,
+	.type1_start = 135,
+	.type1_end = 135,
+	.port_shf = 4,
+	.port_mask = 0xf,
+	.port_align = 4,
+	.eint_offsets = {
+		.name = "mt8173_eint",
+		.stat      = 0x000,
+		.ack       = 0x040,
+		.mask      = 0x080,
+		.mask_set  = 0x0c0,
+		.mask_clr  = 0x100,
+		.sens      = 0x140,
+		.sens_set  = 0x180,
+		.sens_clr  = 0x1c0,
+		.soft      = 0x200,
+		.soft_set  = 0x240,
+		.soft_clr  = 0x280,
+		.pol       = 0x300,
+		.pol_set   = 0x340,
+		.pol_clr   = 0x380,
+		.dom_en    = 0x400,
+		.dbnc_ctrl = 0x500,
+		.dbnc_set  = 0x600,
+		.dbnc_clr  = 0x700,
+		.port_mask = 7,
+		.ports     = 6,
+	},
+	.ap_num = 224,
+	.db_cnt = 16,
+};
+
+static int mt8173_pinctrl_probe(struct platform_device *pdev)
+{
+	return mtk_pctrl_init(pdev, &mt8173_pinctrl_data);
+}
+
+static const struct of_device_id mt8173_pctrl_match[] = {
+	{
+		.compatible = "mediatek,mt8173-pinctrl",
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mt8173_pctrl_match);
+
+static struct platform_driver mtk_pinctrl_driver = {
+	.probe = mt8173_pinctrl_probe,
+	.driver = {
+		.name = "mediatek-mt8173-pinctrl",
+		.of_match_table = mt8173_pctrl_match,
+	},
+};
+
+static int __init mtk_pinctrl_init(void)
+{
+	return platform_driver_register(&mtk_pinctrl_driver);
+}
+
+module_init(mtk_pinctrl_init);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MediaTek Pinctrl Driver");
+MODULE_AUTHOR("Hongzhou Yang <hongzhou.yang@mediatek.com>");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-common.c b/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
new file mode 100644
index 000000000000..493294c0ebe6
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
@@ -0,0 +1,1257 @@
+/*
+ * mt65xx pinctrl driver based on Allwinner A1X pinctrl driver.
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/pinctrl/machine.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/bitops.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <dt-bindings/pinctrl/mt65xx.h>
+
+#include "../core.h"
+#include "../pinconf.h"
+#include "../pinctrl-utils.h"
+#include "pinctrl-mtk-common.h"
+
+#define MAX_GPIO_MODE_PER_REG 5
+#define GPIO_MODE_BITS        3
+
+static const char * const mtk_gpio_functions[] = {
+	"func0", "func1", "func2", "func3",
+	"func4", "func5", "func6", "func7",
+};
+
+/*
+ * There are two base address for pull related configuration
+ * in mt8135, and different GPIO pins use different base address.
+ * When pin number greater than type1_start and less than type1_end,
+ * should use the second base address.
+ */
+static struct regmap *mtk_get_regmap(struct mtk_pinctrl *pctl,
+		unsigned long pin)
+{
+	if (pin >= pctl->devdata->type1_start && pin < pctl->devdata->type1_end)
+		return pctl->regmap2;
+	return pctl->regmap1;
+}
+
+static unsigned int mtk_get_port(struct mtk_pinctrl *pctl, unsigned long pin)
+{
+	/* Different SoC has different mask and port shift. */
+	return ((pin >> 4) & pctl->devdata->port_mask)
+			<< pctl->devdata->port_shf;
+}
+
+static int mtk_pmx_gpio_set_direction(struct pinctrl_dev *pctldev,
+			struct pinctrl_gpio_range *range, unsigned offset,
+			bool input)
+{
+	unsigned int reg_addr;
+	unsigned int bit;
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	reg_addr = mtk_get_port(pctl, offset) + pctl->devdata->dir_offset;
+	bit = BIT(offset & 0xf);
+
+	if (input)
+		/* Different SoC has different alignment offset. */
+		reg_addr = CLR_ADDR(reg_addr, pctl);
+	else
+		reg_addr = SET_ADDR(reg_addr, pctl);
+
+	regmap_write(mtk_get_regmap(pctl, offset), reg_addr, bit);
+	return 0;
+}
+
+static void mtk_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	unsigned int reg_addr;
+	unsigned int bit;
+	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+
+	reg_addr = mtk_get_port(pctl, offset) + pctl->devdata->dout_offset;
+	bit = BIT(offset & 0xf);
+
+	if (value)
+		reg_addr = SET_ADDR(reg_addr, pctl);
+	else
+		reg_addr = CLR_ADDR(reg_addr, pctl);
+
+	regmap_write(mtk_get_regmap(pctl, offset), reg_addr, bit);
+}
+
+static void mtk_pconf_set_ies_smt(struct mtk_pinctrl *pctl, unsigned pin,
+		int value, enum pin_config_param param)
+{
+	unsigned int reg_addr, offset;
+	unsigned int bit;
+	int ret;
+
+	/*
+	 * Due to some pins are irregular, their input enable and smt
+	 * control register are discontinuous, but they are mapping together.
+	 * So we need this special handle.
+	 */
+	if (pctl->devdata->spec_ies_smt_set) {
+		ret = pctl->devdata->spec_ies_smt_set(mtk_get_regmap(pctl, pin),
+			pin, pctl->devdata->port_align, value);
+		if (!ret)
+			return;
+	}
+
+	bit = BIT(pin & 0xf);
+
+	if (param == PIN_CONFIG_INPUT_ENABLE)
+		offset = pctl->devdata->ies_offset;
+	else
+		offset = pctl->devdata->smt_offset;
+
+	if (value)
+		reg_addr = SET_ADDR(mtk_get_port(pctl, pin) + offset, pctl);
+	else
+		reg_addr = CLR_ADDR(mtk_get_port(pctl, pin) + offset, pctl);
+
+	regmap_write(mtk_get_regmap(pctl, pin), reg_addr, bit);
+}
+
+static const struct mtk_pin_drv_grp *mtk_find_pin_drv_grp_by_pin(
+		struct mtk_pinctrl *pctl,  unsigned long pin) {
+	int i;
+
+	for (i = 0; i < pctl->devdata->n_pin_drv_grps; i++) {
+		const struct mtk_pin_drv_grp *pin_drv =
+				pctl->devdata->pin_drv_grp + i;
+		if (pin == pin_drv->pin)
+			return pin_drv;
+	}
+
+	return NULL;
+}
+
+static int mtk_pconf_set_driving(struct mtk_pinctrl *pctl,
+		unsigned int pin, unsigned char driving)
+{
+	const struct mtk_pin_drv_grp *pin_drv;
+	unsigned int val;
+	unsigned int bits, mask, shift;
+	const struct mtk_drv_group_desc *drv_grp;
+
+	if (pin >= pctl->devdata->npins)
+		return -EINVAL;
+
+	pin_drv = mtk_find_pin_drv_grp_by_pin(pctl, pin);
+	if (!pin_drv || pin_drv->grp > pctl->devdata->n_grp_cls)
+		return -EINVAL;
+
+	drv_grp = pctl->devdata->grp_desc + pin_drv->grp;
+	if (driving >= drv_grp->min_drv && driving <= drv_grp->max_drv
+		&& !(driving % drv_grp->step)) {
+		val = driving / drv_grp->step - 1;
+		bits = drv_grp->high_bit - drv_grp->low_bit + 1;
+		mask = BIT(bits) - 1;
+		shift = pin_drv->bit + drv_grp->low_bit;
+		mask <<= shift;
+		val <<= shift;
+		return regmap_update_bits(mtk_get_regmap(pctl, pin),
+				pin_drv->offset, mask, val);
+	}
+
+	return -EINVAL;
+}
+
+static int mtk_pconf_set_pull_select(struct mtk_pinctrl *pctl,
+		unsigned int pin, bool enable, bool isup, unsigned int arg)
+{
+	unsigned int bit;
+	unsigned int reg_pullen, reg_pullsel;
+	int ret;
+
+	/* Some pins' pull setting are very different,
+	 * they have separate pull up/down bit, R0 and R1
+	 * resistor bit, so we need this special handle.
+	 */
+	if (pctl->devdata->spec_pull_set) {
+		ret = pctl->devdata->spec_pull_set(mtk_get_regmap(pctl, pin),
+			pin, pctl->devdata->port_align, isup, arg);
+		if (!ret)
+			return 0;
+	}
+
+	/* For generic pull config, default arg value should be 0 or 1. */
+	if (arg != 0 && arg != 1) {
+		dev_err(pctl->dev, "invalid pull-up argument %d on pin %d .\n",
+			arg, pin);
+		return -EINVAL;
+	}
+
+	bit = BIT(pin & 0xf);
+	if (enable)
+		reg_pullen = SET_ADDR(mtk_get_port(pctl, pin) +
+			pctl->devdata->pullen_offset, pctl);
+	else
+		reg_pullen = CLR_ADDR(mtk_get_port(pctl, pin) +
+			pctl->devdata->pullen_offset, pctl);
+
+	if (isup)
+		reg_pullsel = SET_ADDR(mtk_get_port(pctl, pin) +
+			pctl->devdata->pullsel_offset, pctl);
+	else
+		reg_pullsel = CLR_ADDR(mtk_get_port(pctl, pin) +
+			pctl->devdata->pullsel_offset, pctl);
+
+	regmap_write(mtk_get_regmap(pctl, pin), reg_pullen, bit);
+	regmap_write(mtk_get_regmap(pctl, pin), reg_pullsel, bit);
+	return 0;
+}
+
+static int mtk_pconf_parse_conf(struct pinctrl_dev *pctldev,
+		unsigned int pin, enum pin_config_param param,
+		enum pin_config_param arg)
+{
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		mtk_pconf_set_pull_select(pctl, pin, false, false, arg);
+		break;
+	case PIN_CONFIG_BIAS_PULL_UP:
+		mtk_pconf_set_pull_select(pctl, pin, true, true, arg);
+		break;
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		mtk_pconf_set_pull_select(pctl, pin, true, false, arg);
+		break;
+	case PIN_CONFIG_INPUT_ENABLE:
+		mtk_pconf_set_ies_smt(pctl, pin, arg, param);
+		break;
+	case PIN_CONFIG_OUTPUT:
+		mtk_gpio_set(pctl->chip, pin, arg);
+		mtk_pmx_gpio_set_direction(pctldev, NULL, pin, false);
+		break;
+	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		mtk_pconf_set_ies_smt(pctl, pin, arg, param);
+		break;
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		mtk_pconf_set_driving(pctl, pin, arg);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mtk_pconf_group_get(struct pinctrl_dev *pctldev,
+				 unsigned group,
+				 unsigned long *config)
+{
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	*config = pctl->groups[group].config;
+
+	return 0;
+}
+
+static int mtk_pconf_group_set(struct pinctrl_dev *pctldev, unsigned group,
+				 unsigned long *configs, unsigned num_configs)
+{
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct mtk_pinctrl_group *g = &pctl->groups[group];
+	int i;
+
+	for (i = 0; i < num_configs; i++) {
+		mtk_pconf_parse_conf(pctldev, g->pin,
+			pinconf_to_config_param(configs[i]),
+			pinconf_to_config_argument(configs[i]));
+
+		g->config = configs[i];
+	}
+
+	return 0;
+}
+
+static const struct pinconf_ops mtk_pconf_ops = {
+	.pin_config_group_get	= mtk_pconf_group_get,
+	.pin_config_group_set	= mtk_pconf_group_set,
+};
+
+static struct mtk_pinctrl_group *
+mtk_pctrl_find_group_by_pin(struct mtk_pinctrl *pctl, u32 pin)
+{
+	int i;
+
+	for (i = 0; i < pctl->ngroups; i++) {
+		struct mtk_pinctrl_group *grp = pctl->groups + i;
+
+		if (grp->pin == pin)
+			return grp;
+	}
+
+	return NULL;
+}
+
+static const struct mtk_desc_function *mtk_pctrl_find_function_by_pin(
+		struct mtk_pinctrl *pctl, u32 pin_num, u32 fnum)
+{
+	const struct mtk_desc_pin *pin = pctl->devdata->pins + pin_num;
+	const struct mtk_desc_function *func = pin->functions;
+
+	while (func && func->name) {
+		if (func->muxval == fnum)
+			return func;
+		func++;
+	}
+
+	return NULL;
+}
+
+static bool mtk_pctrl_is_function_valid(struct mtk_pinctrl *pctl,
+		u32 pin_num, u32 fnum)
+{
+	int i;
+
+	for (i = 0; i < pctl->devdata->npins; i++) {
+		const struct mtk_desc_pin *pin = pctl->devdata->pins + i;
+
+		if (pin->pin.number == pin_num) {
+			const struct mtk_desc_function *func =
+					pin->functions;
+
+			while (func && func->name) {
+				if (func->muxval == fnum)
+					return true;
+				func++;
+			}
+
+			break;
+		}
+	}
+
+	return false;
+}
+
+static int mtk_pctrl_dt_node_to_map_func(struct mtk_pinctrl *pctl,
+		u32 pin, u32 fnum, struct mtk_pinctrl_group *grp,
+		struct pinctrl_map **map, unsigned *reserved_maps,
+		unsigned *num_maps)
+{
+	bool ret;
+
+	if (*num_maps == *reserved_maps)
+		return -ENOSPC;
+
+	(*map)[*num_maps].type = PIN_MAP_TYPE_MUX_GROUP;
+	(*map)[*num_maps].data.mux.group = grp->name;
+
+	ret = mtk_pctrl_is_function_valid(pctl, pin, fnum);
+	if (!ret) {
+		dev_err(pctl->dev, "invalid function %d on pin %d .\n",
+				fnum, pin);
+		return -EINVAL;
+	}
+
+	(*map)[*num_maps].data.mux.function = mtk_gpio_functions[fnum];
+	(*num_maps)++;
+
+	return 0;
+}
+
+static int mtk_pctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
+				      struct device_node *node,
+				      struct pinctrl_map **map,
+				      unsigned *reserved_maps,
+				      unsigned *num_maps)
+{
+	struct property *pins;
+	u32 pinfunc, pin, func;
+	int num_pins, num_funcs, maps_per_pin;
+	unsigned long *configs;
+	unsigned int num_configs;
+	bool has_config = 0;
+	int i, err;
+	unsigned reserve = 0;
+	struct mtk_pinctrl_group *grp;
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	pins = of_find_property(node, "pinmux", NULL);
+	if (!pins) {
+		dev_err(pctl->dev, "missing pins property in node %s .\n",
+				node->name);
+		return -EINVAL;
+	}
+
+	err = pinconf_generic_parse_dt_config(node, pctldev, &configs,
+		&num_configs);
+	if (num_configs)
+		has_config = 1;
+
+	num_pins = pins->length / sizeof(u32);
+	num_funcs = num_pins;
+	maps_per_pin = 0;
+	if (num_funcs)
+		maps_per_pin++;
+	if (has_config && num_pins >= 1)
+		maps_per_pin++;
+
+	if (!num_pins || !maps_per_pin)
+		return -EINVAL;
+
+	reserve = num_pins * maps_per_pin;
+
+	err = pinctrl_utils_reserve_map(pctldev, map,
+			reserved_maps, num_maps, reserve);
+	if (err < 0)
+		goto fail;
+
+	for (i = 0; i < num_pins; i++) {
+		err = of_property_read_u32_index(node, "pinmux",
+				i, &pinfunc);
+		if (err)
+			goto fail;
+
+		pin = MTK_GET_PIN_NO(pinfunc);
+		func = MTK_GET_PIN_FUNC(pinfunc);
+
+		if (pin >= pctl->devdata->npins ||
+				func >= ARRAY_SIZE(mtk_gpio_functions)) {
+			dev_err(pctl->dev, "invalid pins value.\n");
+			err = -EINVAL;
+			goto fail;
+		}
+
+		grp = mtk_pctrl_find_group_by_pin(pctl, pin);
+		if (!grp) {
+			dev_err(pctl->dev, "unable to match pin %d to group\n",
+					pin);
+			return -EINVAL;
+		}
+
+		err = mtk_pctrl_dt_node_to_map_func(pctl, pin, func, grp, map,
+				reserved_maps, num_maps);
+		if (err < 0)
+			goto fail;
+
+		if (has_config) {
+			err = pinctrl_utils_add_map_configs(pctldev, map,
+					reserved_maps, num_maps, grp->name,
+					configs, num_configs,
+					PIN_MAP_TYPE_CONFIGS_GROUP);
+			if (err < 0)
+				goto fail;
+		}
+	}
+
+	return 0;
+
+fail:
+	return err;
+}
+
+static int mtk_pctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
+				 struct device_node *np_config,
+				 struct pinctrl_map **map, unsigned *num_maps)
+{
+	struct device_node *np;
+	unsigned reserved_maps;
+	int ret;
+
+	*map = NULL;
+	*num_maps = 0;
+	reserved_maps = 0;
+
+	for_each_child_of_node(np_config, np) {
+		ret = mtk_pctrl_dt_subnode_to_map(pctldev, np, map,
+				&reserved_maps, num_maps);
+		if (ret < 0) {
+			pinctrl_utils_dt_free_map(pctldev, *map, *num_maps);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int mtk_pctrl_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->ngroups;
+}
+
+static const char *mtk_pctrl_get_group_name(struct pinctrl_dev *pctldev,
+					      unsigned group)
+{
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->groups[group].name;
+}
+
+static int mtk_pctrl_get_group_pins(struct pinctrl_dev *pctldev,
+				      unsigned group,
+				      const unsigned **pins,
+				      unsigned *num_pins)
+{
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = (unsigned *)&pctl->groups[group].pin;
+	*num_pins = 1;
+
+	return 0;
+}
+
+static const struct pinctrl_ops mtk_pctrl_ops = {
+	.dt_node_to_map		= mtk_pctrl_dt_node_to_map,
+	.dt_free_map		= pinctrl_utils_dt_free_map,
+	.get_groups_count	= mtk_pctrl_get_groups_count,
+	.get_group_name		= mtk_pctrl_get_group_name,
+	.get_group_pins		= mtk_pctrl_get_group_pins,
+};
+
+static int mtk_pmx_get_funcs_cnt(struct pinctrl_dev *pctldev)
+{
+	return ARRAY_SIZE(mtk_gpio_functions);
+}
+
+static const char *mtk_pmx_get_func_name(struct pinctrl_dev *pctldev,
+					   unsigned selector)
+{
+	return mtk_gpio_functions[selector];
+}
+
+static int mtk_pmx_get_func_groups(struct pinctrl_dev *pctldev,
+				     unsigned function,
+				     const char * const **groups,
+				     unsigned * const num_groups)
+{
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = pctl->grp_names;
+	*num_groups = pctl->ngroups;
+
+	return 0;
+}
+
+static int mtk_pmx_set_mode(struct pinctrl_dev *pctldev,
+		unsigned long pin, unsigned long mode)
+{
+	unsigned int reg_addr;
+	unsigned char bit;
+	unsigned int val;
+	unsigned int mask = (1L << GPIO_MODE_BITS) - 1;
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	reg_addr = ((pin / MAX_GPIO_MODE_PER_REG) << pctl->devdata->port_shf)
+			+ pctl->devdata->pinmux_offset;
+
+	bit = pin % MAX_GPIO_MODE_PER_REG;
+	mask <<= (GPIO_MODE_BITS * bit);
+	val = (mode << (GPIO_MODE_BITS * bit));
+	return regmap_update_bits(mtk_get_regmap(pctl, pin),
+			reg_addr, mask, val);
+}
+
+static const struct mtk_desc_pin *
+mtk_find_pin_by_eint_num(struct mtk_pinctrl *pctl, unsigned int eint_num)
+{
+	int i;
+	const struct mtk_desc_pin *pin;
+
+	for (i = 0; i < pctl->devdata->npins; i++) {
+		pin = pctl->devdata->pins + i;
+		if (pin->eint.eintnum == eint_num)
+			return pin;
+	}
+
+	return NULL;
+}
+
+static int mtk_pmx_set_mux(struct pinctrl_dev *pctldev,
+			    unsigned function,
+			    unsigned group)
+{
+	bool ret;
+	const struct mtk_desc_function *desc;
+	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct mtk_pinctrl_group *g = pctl->groups + group;
+
+	ret = mtk_pctrl_is_function_valid(pctl, g->pin, function);
+	if (!ret) {
+		dev_err(pctl->dev, "invaild function %d on group %d .\n",
+				function, group);
+		return -EINVAL;
+	}
+
+	desc = mtk_pctrl_find_function_by_pin(pctl, g->pin, function);
+	if (!desc)
+		return -EINVAL;
+	mtk_pmx_set_mode(pctldev, g->pin, desc->muxval);
+	return 0;
+}
+
+static const struct pinmux_ops mtk_pmx_ops = {
+	.get_functions_count	= mtk_pmx_get_funcs_cnt,
+	.get_function_name	= mtk_pmx_get_func_name,
+	.get_function_groups	= mtk_pmx_get_func_groups,
+	.set_mux		= mtk_pmx_set_mux,
+	.gpio_set_direction	= mtk_pmx_gpio_set_direction,
+};
+
+static int mtk_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(chip->base + offset);
+}
+
+static void mtk_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+}
+
+static int mtk_gpio_direction_input(struct gpio_chip *chip,
+					unsigned offset)
+{
+	return pinctrl_gpio_direction_input(chip->base + offset);
+}
+
+static int mtk_gpio_direction_output(struct gpio_chip *chip,
+					unsigned offset, int value)
+{
+	mtk_gpio_set(chip, offset, value);
+	return pinctrl_gpio_direction_output(chip->base + offset);
+}
+
+static int mtk_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	unsigned int reg_addr;
+	unsigned int bit;
+	unsigned int read_val = 0;
+
+	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+
+	reg_addr =  mtk_get_port(pctl, offset) + pctl->devdata->dir_offset;
+	bit = BIT(offset & 0xf);
+	regmap_read(pctl->regmap1, reg_addr, &read_val);
+	return !!(read_val & bit);
+}
+
+static int mtk_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	unsigned int reg_addr;
+	unsigned int bit;
+	unsigned int read_val = 0;
+	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+
+	if (mtk_gpio_get_direction(chip, offset))
+		reg_addr = mtk_get_port(pctl, offset) +
+			pctl->devdata->dout_offset;
+	else
+		reg_addr = mtk_get_port(pctl, offset) +
+			pctl->devdata->din_offset;
+
+	bit = BIT(offset & 0xf);
+	regmap_read(pctl->regmap1, reg_addr, &read_val);
+	return !!(read_val & bit);
+}
+
+static int mtk_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	const struct mtk_desc_pin *pin;
+	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	int irq;
+
+	pin = pctl->devdata->pins + offset;
+	if (pin->eint.eintnum == NO_EINT_SUPPORT)
+		return -EINVAL;
+
+	irq = irq_find_mapping(pctl->domain, pin->eint.eintnum);
+	if (!irq)
+		return -EINVAL;
+
+	return irq;
+}
+
+static int mtk_pinctrl_irq_request_resources(struct irq_data *d)
+{
+	struct mtk_pinctrl *pctl = irq_data_get_irq_chip_data(d);
+	const struct mtk_desc_pin *pin;
+	int ret;
+
+	pin = mtk_find_pin_by_eint_num(pctl, d->hwirq);
+
+	if (!pin) {
+		dev_err(pctl->dev, "Can not find pin\n");
+		return -EINVAL;
+	}
+
+	ret = gpiochip_lock_as_irq(pctl->chip, pin->pin.number);
+	if (ret) {
+		dev_err(pctl->dev, "unable to lock HW IRQ %lu for IRQ\n",
+			irqd_to_hwirq(d));
+		return ret;
+	}
+
+	/* set mux to INT mode */
+	mtk_pmx_set_mode(pctl->pctl_dev, pin->pin.number, pin->eint.eintmux);
+
+	return 0;
+}
+
+static void mtk_pinctrl_irq_release_resources(struct irq_data *d)
+{
+	struct mtk_pinctrl *pctl = irq_data_get_irq_chip_data(d);
+	const struct mtk_desc_pin *pin;
+
+	pin = mtk_find_pin_by_eint_num(pctl, d->hwirq);
+
+	if (!pin) {
+		dev_err(pctl->dev, "Can not find pin\n");
+		return;
+	}
+
+	gpiochip_unlock_as_irq(pctl->chip, pin->pin.number);
+}
+
+static void __iomem *mtk_eint_get_offset(struct mtk_pinctrl *pctl,
+	unsigned int eint_num, unsigned int offset)
+{
+	unsigned int eint_base = 0;
+	void __iomem *reg;
+
+	if (eint_num >= pctl->devdata->ap_num)
+		eint_base = pctl->devdata->ap_num;
+
+	reg = pctl->eint_reg_base + offset + ((eint_num - eint_base) / 32) * 4;
+
+	return reg;
+}
+
+/*
+ * mtk_can_en_debounce: Check the EINT number is able to enable debounce or not
+ * @eint_num: the EINT number to setmtk_pinctrl
+ */
+static unsigned int mtk_eint_can_en_debounce(struct mtk_pinctrl *pctl,
+	unsigned int eint_num)
+{
+	unsigned int sens;
+	unsigned int bit = BIT(eint_num % 32);
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+
+	void __iomem *reg = mtk_eint_get_offset(pctl, eint_num,
+			eint_offsets->sens);
+
+	if (readl(reg) & bit)
+		sens = MT_LEVEL_SENSITIVE;
+	else
+		sens = MT_EDGE_SENSITIVE;
+
+	if ((eint_num < pctl->devdata->db_cnt) && (sens != MT_EDGE_SENSITIVE))
+		return 1;
+	else
+		return 0;
+}
+
+/*
+ * mtk_eint_get_mask: To get the eint mask
+ * @eint_num: the EINT number to get
+ */
+static unsigned int mtk_eint_get_mask(struct mtk_pinctrl *pctl,
+	unsigned int eint_num)
+{
+	unsigned int bit = BIT(eint_num % 32);
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+
+	void __iomem *reg = mtk_eint_get_offset(pctl, eint_num,
+			eint_offsets->mask);
+
+	return !!(readl(reg) & bit);
+}
+
+static int mtk_eint_flip_edge(struct mtk_pinctrl *pctl, int hwirq)
+{
+	int start_level, curr_level;
+	unsigned int reg_offset;
+	const struct mtk_eint_offsets *eint_offsets = &(pctl->devdata->eint_offsets);
+	u32 mask = 1 << (hwirq & 0x1f);
+	u32 port = (hwirq >> 5) & eint_offsets->port_mask;
+	void __iomem *reg = pctl->eint_reg_base + (port << 2);
+	const struct mtk_desc_pin *pin;
+
+	pin = mtk_find_pin_by_eint_num(pctl, hwirq);
+	curr_level = mtk_gpio_get(pctl->chip, pin->pin.number);
+	do {
+		start_level = curr_level;
+		if (start_level)
+			reg_offset = eint_offsets->pol_clr;
+		else
+			reg_offset = eint_offsets->pol_set;
+		writel(mask, reg + reg_offset);
+
+		curr_level = mtk_gpio_get(pctl->chip, pin->pin.number);
+	} while (start_level != curr_level);
+
+	return start_level;
+}
+
+static void mtk_eint_mask(struct irq_data *d)
+{
+	struct mtk_pinctrl *pctl = irq_data_get_irq_chip_data(d);
+	const struct mtk_eint_offsets *eint_offsets =
+			&pctl->devdata->eint_offsets;
+	u32 mask = BIT(d->hwirq & 0x1f);
+	void __iomem *reg = mtk_eint_get_offset(pctl, d->hwirq,
+			eint_offsets->mask_set);
+
+	writel(mask, reg);
+}
+
+static void mtk_eint_unmask(struct irq_data *d)
+{
+	struct mtk_pinctrl *pctl = irq_data_get_irq_chip_data(d);
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+	u32 mask = BIT(d->hwirq & 0x1f);
+	void __iomem *reg = mtk_eint_get_offset(pctl, d->hwirq,
+			eint_offsets->mask_clr);
+
+	writel(mask, reg);
+
+	if (pctl->eint_dual_edges[d->hwirq])
+		mtk_eint_flip_edge(pctl, d->hwirq);
+}
+
+static int mtk_gpio_set_debounce(struct gpio_chip *chip, unsigned offset,
+	unsigned debounce)
+{
+	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	int eint_num, virq, eint_offset;
+	unsigned int set_offset, bit, clr_bit, clr_offset, rst, i, unmask, dbnc;
+	static const unsigned int dbnc_arr[] = {0 , 1, 16, 32, 64, 128, 256};
+	const struct mtk_desc_pin *pin;
+	struct irq_data *d;
+
+	pin = pctl->devdata->pins + offset;
+	if (pin->eint.eintnum == NO_EINT_SUPPORT)
+		return -EINVAL;
+
+	eint_num = pin->eint.eintnum;
+	virq = irq_find_mapping(pctl->domain, eint_num);
+	eint_offset = (eint_num % 4) * 8;
+	d = irq_get_irq_data(virq);
+
+	set_offset = (eint_num / 4) * 4 + pctl->devdata->eint_offsets.dbnc_set;
+	clr_offset = (eint_num / 4) * 4 + pctl->devdata->eint_offsets.dbnc_clr;
+	if (!mtk_eint_can_en_debounce(pctl, eint_num))
+		return -ENOSYS;
+
+	dbnc = ARRAY_SIZE(dbnc_arr);
+	for (i = 0; i < ARRAY_SIZE(dbnc_arr); i++) {
+		if (debounce <= dbnc_arr[i]) {
+			dbnc = i;
+			break;
+		}
+	}
+
+	if (!mtk_eint_get_mask(pctl, eint_num)) {
+		mtk_eint_mask(d);
+		unmask = 1;
+	}
+
+	clr_bit = 0xff << eint_offset;
+	writel(clr_bit, pctl->eint_reg_base + clr_offset);
+
+	bit = ((dbnc << EINT_DBNC_SET_DBNC_BITS) | EINT_DBNC_SET_EN) <<
+		eint_offset;
+	rst = EINT_DBNC_RST_BIT << eint_offset;
+	writel(rst | bit, pctl->eint_reg_base + set_offset);
+
+	/* Delay a while (more than 2T) to wait for hw debounce counter reset
+	work correctly */
+	udelay(1);
+	if (unmask == 1)
+		mtk_eint_unmask(d);
+
+	return 0;
+}
+
+static struct gpio_chip mtk_gpio_chip = {
+	.owner			= THIS_MODULE,
+	.request		= mtk_gpio_request,
+	.free			= mtk_gpio_free,
+	.direction_input	= mtk_gpio_direction_input,
+	.direction_output	= mtk_gpio_direction_output,
+	.get			= mtk_gpio_get,
+	.set			= mtk_gpio_set,
+	.to_irq			= mtk_gpio_to_irq,
+	.set_debounce		= mtk_gpio_set_debounce,
+	.of_gpio_n_cells	= 2,
+};
+
+static int mtk_eint_set_type(struct irq_data *d,
+				      unsigned int type)
+{
+	struct mtk_pinctrl *pctl = irq_data_get_irq_chip_data(d);
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+	u32 mask = BIT(d->hwirq & 0x1f);
+	void __iomem *reg;
+
+	if (((type & IRQ_TYPE_EDGE_BOTH) && (type & IRQ_TYPE_LEVEL_MASK)) ||
+		((type & IRQ_TYPE_LEVEL_MASK) == IRQ_TYPE_LEVEL_MASK)) {
+		dev_err(pctl->dev, "Can't configure IRQ%d (EINT%lu) for type 0x%X\n",
+			d->irq, d->hwirq, type);
+		return -EINVAL;
+	}
+
+	if ((type & IRQ_TYPE_EDGE_BOTH) == IRQ_TYPE_EDGE_BOTH)
+		pctl->eint_dual_edges[d->hwirq] = 1;
+	else
+		pctl->eint_dual_edges[d->hwirq] = 0;
+
+	if (type & (IRQ_TYPE_LEVEL_LOW | IRQ_TYPE_EDGE_FALLING)) {
+		reg = mtk_eint_get_offset(pctl, d->hwirq,
+			eint_offsets->pol_clr);
+		writel(mask, reg);
+	} else {
+		reg = mtk_eint_get_offset(pctl, d->hwirq,
+			eint_offsets->pol_set);
+		writel(mask, reg);
+	}
+
+	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)) {
+		reg = mtk_eint_get_offset(pctl, d->hwirq,
+			eint_offsets->sens_clr);
+		writel(mask, reg);
+	} else {
+		reg = mtk_eint_get_offset(pctl, d->hwirq,
+			eint_offsets->sens_set);
+		writel(mask, reg);
+	}
+
+	if (pctl->eint_dual_edges[d->hwirq])
+		mtk_eint_flip_edge(pctl, d->hwirq);
+
+	return 0;
+}
+
+static void mtk_eint_ack(struct irq_data *d)
+{
+	struct mtk_pinctrl *pctl = irq_data_get_irq_chip_data(d);
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+	u32 mask = BIT(d->hwirq & 0x1f);
+	void __iomem *reg = mtk_eint_get_offset(pctl, d->hwirq,
+			eint_offsets->ack);
+
+	writel(mask, reg);
+}
+
+static struct irq_chip mtk_pinctrl_irq_chip = {
+	.name = "mt-eint",
+	.irq_mask = mtk_eint_mask,
+	.irq_unmask = mtk_eint_unmask,
+	.irq_ack = mtk_eint_ack,
+	.irq_set_type = mtk_eint_set_type,
+	.irq_request_resources = mtk_pinctrl_irq_request_resources,
+	.irq_release_resources = mtk_pinctrl_irq_release_resources,
+};
+
+static unsigned int mtk_eint_init(struct mtk_pinctrl *pctl)
+{
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+	void __iomem *reg = pctl->eint_reg_base + eint_offsets->dom_en;
+	unsigned int i;
+
+	for (i = 0; i < pctl->devdata->ap_num; i += 32) {
+		writel(0xffffffff, reg);
+		reg += 4;
+	}
+	return 0;
+}
+
+static inline void
+mtk_eint_debounce_process(struct mtk_pinctrl *pctl, int index)
+{
+	unsigned int rst, ctrl_offset;
+	unsigned int bit, dbnc;
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+
+	ctrl_offset = (index / 4) * 4 + eint_offsets->dbnc_ctrl;
+	dbnc = readl(pctl->eint_reg_base + ctrl_offset);
+	bit = EINT_DBNC_SET_EN << ((index % 4) * 8);
+	if ((bit & dbnc) > 0) {
+		ctrl_offset = (index / 4) * 4 + eint_offsets->dbnc_set;
+		rst = EINT_DBNC_RST_BIT << ((index % 4) * 8);
+		writel(rst, pctl->eint_reg_base + ctrl_offset);
+	}
+}
+
+static void mtk_eint_irq_handler(unsigned irq, struct irq_desc *desc)
+{
+	struct irq_chip *chip = irq_get_chip(irq);
+	struct mtk_pinctrl *pctl = irq_get_handler_data(irq);
+	unsigned int status, eint_num;
+	int offset, index, virq;
+	const struct mtk_eint_offsets *eint_offsets =
+		&pctl->devdata->eint_offsets;
+	void __iomem *reg =  mtk_eint_get_offset(pctl, 0, eint_offsets->stat);
+	int dual_edges, start_level, curr_level;
+	const struct mtk_desc_pin *pin;
+
+	chained_irq_enter(chip, desc);
+	for (eint_num = 0; eint_num < pctl->devdata->ap_num; eint_num += 32) {
+		status = readl(reg);
+		reg += 4;
+		while (status) {
+			offset = __ffs(status);
+			index = eint_num + offset;
+			virq = irq_find_mapping(pctl->domain, index);
+			status &= ~BIT(offset);
+
+			dual_edges = pctl->eint_dual_edges[index];
+			if (dual_edges) {
+				/* Clear soft-irq in case we raised it
+				   last time */
+				writel(BIT(offset), reg - eint_offsets->stat +
+					eint_offsets->soft_clr);
+
+				pin = mtk_find_pin_by_eint_num(pctl, index);
+				start_level = mtk_gpio_get(pctl->chip,
+							   pin->pin.number);
+			}
+
+			generic_handle_irq(virq);
+
+			if (dual_edges) {
+				curr_level = mtk_eint_flip_edge(pctl, index);
+
+				/* If level changed, we might lost one edge
+				   interrupt, raised it through soft-irq */
+				if (start_level != curr_level)
+					writel(BIT(offset), reg -
+						eint_offsets->stat +
+						eint_offsets->soft_set);
+			}
+
+			if (index < pctl->devdata->db_cnt)
+				mtk_eint_debounce_process(pctl , index);
+		}
+	}
+	chained_irq_exit(chip, desc);
+}
+
+static int mtk_pctrl_build_state(struct platform_device *pdev)
+{
+	struct mtk_pinctrl *pctl = platform_get_drvdata(pdev);
+	int i;
+
+	pctl->ngroups = pctl->devdata->npins;
+
+	/* Allocate groups */
+	pctl->groups = devm_kcalloc(&pdev->dev, pctl->ngroups,
+				    sizeof(*pctl->groups), GFP_KERNEL);
+	if (!pctl->groups)
+		return -ENOMEM;
+
+	/* We assume that one pin is one group, use pin name as group name. */
+	pctl->grp_names = devm_kcalloc(&pdev->dev, pctl->ngroups,
+				       sizeof(*pctl->grp_names), GFP_KERNEL);
+	if (!pctl->grp_names)
+		return -ENOMEM;
+
+	for (i = 0; i < pctl->devdata->npins; i++) {
+		const struct mtk_desc_pin *pin = pctl->devdata->pins + i;
+		struct mtk_pinctrl_group *group = pctl->groups + i;
+
+		group->name = pin->pin.name;
+		group->pin = pin->pin.number;
+
+		pctl->grp_names[i] = pin->pin.name;
+	}
+
+	return 0;
+}
+
+static struct pinctrl_desc mtk_pctrl_desc = {
+	.confops	= &mtk_pconf_ops,
+	.pctlops	= &mtk_pctrl_ops,
+	.pmxops		= &mtk_pmx_ops,
+};
+
+int mtk_pctrl_init(struct platform_device *pdev,
+		const struct mtk_pinctrl_devdata *data)
+{
+	struct pinctrl_pin_desc *pins;
+	struct mtk_pinctrl *pctl;
+	struct device_node *np = pdev->dev.of_node, *node;
+	struct property *prop;
+	struct resource *res;
+	int i, ret, irq;
+
+	pctl = devm_kzalloc(&pdev->dev, sizeof(*pctl), GFP_KERNEL);
+	if (!pctl)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, pctl);
+
+	prop = of_find_property(np, "pins-are-numbered", NULL);
+	if (!prop) {
+		dev_err(&pdev->dev, "only support pins-are-numbered format\n");
+		return -EINVAL;
+	}
+
+	node = of_parse_phandle(np, "mediatek,pctl-regmap", 0);
+	if (node) {
+		pctl->regmap1 = syscon_node_to_regmap(node);
+		if (IS_ERR(pctl->regmap1))
+			return PTR_ERR(pctl->regmap1);
+	}
+
+	/* Only 8135 has two base addr, other SoCs have only one. */
+	node = of_parse_phandle(np, "mediatek,pctl-regmap", 1);
+	if (node) {
+		pctl->regmap2 = syscon_node_to_regmap(node);
+		if (IS_ERR(pctl->regmap2))
+			return PTR_ERR(pctl->regmap2);
+	}
+
+	pctl->devdata = data;
+	ret = mtk_pctrl_build_state(pdev);
+	if (ret) {
+		dev_err(&pdev->dev, "build state failed: %d\n", ret);
+		return -EINVAL;
+	}
+
+	pins = devm_kcalloc(&pdev->dev, pctl->devdata->npins, sizeof(*pins),
+			    GFP_KERNEL);
+	if (!pins)
+		return -ENOMEM;
+
+	for (i = 0; i < pctl->devdata->npins; i++)
+		pins[i] = pctl->devdata->pins[i].pin;
+	mtk_pctrl_desc.name = dev_name(&pdev->dev);
+	mtk_pctrl_desc.owner = THIS_MODULE;
+	mtk_pctrl_desc.pins = pins;
+	mtk_pctrl_desc.npins = pctl->devdata->npins;
+	pctl->dev = &pdev->dev;
+	pctl->pctl_dev = pinctrl_register(&mtk_pctrl_desc, &pdev->dev, pctl);
+	if (!pctl->pctl_dev) {
+		dev_err(&pdev->dev, "couldn't register pinctrl driver\n");
+		return -EINVAL;
+	}
+
+	pctl->chip = devm_kzalloc(&pdev->dev, sizeof(*pctl->chip), GFP_KERNEL);
+	if (!pctl->chip) {
+		ret = -ENOMEM;
+		goto pctrl_error;
+	}
+
+	pctl->chip = &mtk_gpio_chip;
+	pctl->chip->ngpio = pctl->devdata->npins;
+	pctl->chip->label = dev_name(&pdev->dev);
+	pctl->chip->dev = &pdev->dev;
+	pctl->chip->base = 0;
+
+	ret = gpiochip_add(pctl->chip);
+	if (ret) {
+		ret = -EINVAL;
+		goto pctrl_error;
+	}
+
+	/* Register the GPIO to pin mappings. */
+	ret = gpiochip_add_pin_range(pctl->chip, dev_name(&pdev->dev),
+			0, 0, pctl->devdata->npins);
+	if (ret) {
+		ret = -EINVAL;
+		goto chip_error;
+	}
+
+	/* Get EINT register base from dts. */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Unable to get Pinctrl resource\n");
+		ret = -EINVAL;
+		goto chip_error;
+	}
+
+	pctl->eint_reg_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pctl->eint_reg_base)) {
+		ret = -EINVAL;
+		goto chip_error;
+	}
+
+	pctl->eint_dual_edges = devm_kcalloc(&pdev->dev, pctl->devdata->ap_num,
+					     sizeof(int), GFP_KERNEL);
+	if (!pctl->eint_dual_edges) {
+		ret = -ENOMEM;
+		goto chip_error;
+	}
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq) {
+		dev_err(&pdev->dev, "couldn't parse and map irq\n");
+		ret = -EINVAL;
+		goto chip_error;
+	}
+
+	pctl->domain = irq_domain_add_linear(np,
+		pctl->devdata->ap_num, &irq_domain_simple_ops, NULL);
+	if (!pctl->domain) {
+		dev_err(&pdev->dev, "Couldn't register IRQ domain\n");
+		ret = -ENOMEM;
+		goto chip_error;
+	}
+
+	mtk_eint_init(pctl);
+	for (i = 0; i < pctl->devdata->ap_num; i++) {
+		int virq = irq_create_mapping(pctl->domain, i);
+
+		irq_set_chip_and_handler(virq, &mtk_pinctrl_irq_chip,
+			handle_level_irq);
+		irq_set_chip_data(virq, pctl);
+		set_irq_flags(virq, IRQF_VALID);
+	};
+
+	irq_set_chained_handler(irq, mtk_eint_irq_handler);
+	irq_set_handler_data(irq, pctl);
+	set_irq_flags(irq, IRQF_VALID);
+	return 0;
+
+chip_error:
+	gpiochip_remove(pctl->chip);
+pctrl_error:
+	pinctrl_unregister(pctl->pctl_dev);
+	return ret;
+}
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MediaTek Pinctrl Driver");
+MODULE_AUTHOR("Hongzhou Yang <hongzhou.yang@mediatek.com>");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-common.h b/drivers/pinctrl/mediatek/pinctrl-mtk-common.h
new file mode 100644
index 000000000000..375771db9bd0
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-common.h
@@ -0,0 +1,229 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __PINCTRL_MTK_COMMON_H
+#define __PINCTRL_MTK_COMMON_H
+
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/regmap.h>
+
+#define NO_EINT_SUPPORT    255
+#define MTK_CHIP_TYPE_BASE     0
+#define MTK_CHIP_TYPE_PMIC     1
+#define MT_EDGE_SENSITIVE           0
+#define MT_LEVEL_SENSITIVE          1
+#define EINT_DBNC_SET_DBNC_BITS     4
+#define EINT_DBNC_RST_BIT           (0x1 << 1)
+#define EINT_DBNC_SET_EN            (0x1 << 0)
+
+struct mtk_desc_function {
+	const char *name;
+	unsigned char muxval;
+};
+
+struct mtk_desc_eint {
+	unsigned char eintmux;
+	unsigned char eintnum;
+};
+
+struct mtk_desc_pin {
+	struct pinctrl_pin_desc	pin;
+	const char *chip;
+	const struct mtk_desc_eint eint;
+	const struct mtk_desc_function	*functions;
+};
+
+#define MTK_PIN(_pin, _pad, _chip, _eint, ...)		\
+	{							\
+		.pin = _pin,					\
+		.chip = _chip,					\
+		.eint = _eint,					\
+		.functions = (struct mtk_desc_function[]){	\
+			__VA_ARGS__, { } },			\
+	}
+
+#define MTK_EINT_FUNCTION(_eintmux, _eintnum)				\
+	{							\
+		.eintmux = _eintmux,					\
+		.eintnum = _eintnum,					\
+	}
+
+#define MTK_FUNCTION(_val, _name)				\
+	{							\
+		.muxval = _val,					\
+		.name = _name,					\
+	}
+
+#define SET_ADDR(x, y)  (x + (y->devdata->port_align))
+#define CLR_ADDR(x, y)  (x + (y->devdata->port_align << 1))
+
+struct mtk_pinctrl_group {
+	const char	*name;
+	unsigned long	config;
+	unsigned	pin;
+};
+
+/**
+ * struct mtk_drv_group_desc - Provide driving group data.
+ * @max_drv: The maximum current of this group.
+ * @min_drv: The minimum current of this group.
+ * @low_bit: The lowest bit of this group.
+ * @high_bit: The highest bit of this group.
+ * @step: The step current of this group.
+ */
+struct mtk_drv_group_desc {
+	unsigned char min_drv;
+	unsigned char max_drv;
+	unsigned char low_bit;
+	unsigned char high_bit;
+	unsigned char step;
+};
+
+#define MTK_DRV_GRP(_min, _max, _low, _high, _step)	\
+	{	\
+		.min_drv = _min,	\
+		.max_drv = _max,	\
+		.low_bit = _low,	\
+		.high_bit = _high,	\
+		.step = _step,		\
+	}
+
+/**
+ * struct mtk_pin_drv_grp - Provide each pin driving info.
+ * @pin: The pin number.
+ * @offset: The offset of driving register for this pin.
+ * @bit: The bit of driving register for this pin.
+ * @grp: The group for this pin belongs to.
+ */
+struct mtk_pin_drv_grp {
+	unsigned int pin;
+	unsigned int offset;
+	unsigned char bit;
+	unsigned char grp;
+};
+
+#define MTK_PIN_DRV_GRP(_pin, _offset, _bit, _grp)	\
+	{	\
+		.pin = _pin,	\
+		.offset = _offset,	\
+		.bit = _bit,	\
+		.grp = _grp,	\
+	}
+
+struct mtk_eint_offsets {
+	const char *name;
+	unsigned int  stat;
+	unsigned int  ack;
+	unsigned int  mask;
+	unsigned int  mask_set;
+	unsigned int  mask_clr;
+	unsigned int  sens;
+	unsigned int  sens_set;
+	unsigned int  sens_clr;
+	unsigned int  soft;
+	unsigned int  soft_set;
+	unsigned int  soft_clr;
+	unsigned int  pol;
+	unsigned int  pol_set;
+	unsigned int  pol_clr;
+	unsigned int  dom_en;
+	unsigned int  dbnc_ctrl;
+	unsigned int  dbnc_set;
+	unsigned int  dbnc_clr;
+	u8  port_mask;
+	u8  ports;
+};
+
+/**
+ * struct mtk_pinctrl_devdata - Provide HW GPIO related data.
+ * @pins: An array describing all pins the pin controller affects.
+ * @npins: The number of entries in @pins.
+ *
+ * @grp_desc: The driving group info.
+ * @pin_drv_grp: The driving group for all pins.
+ * @spec_pull_set: Each SoC may have special pins for pull up/down setting,
+ *  these pins' pull setting are very different, they have separate pull
+ *  up/down bit, R0 and R1 resistor bit, so they need special pull setting.
+ *  If special setting is success, this should return 0, otherwise it should
+ *  return non-zero value.
+ * @spec_ies_smt_set: Some pins are irregular, their input enable and smt
+ * control register are discontinuous, but they are mapping together. That
+ * means when user set smt, input enable is set at the same time. So they
+ * also need special control. If special control is success, this should
+ * return 0, otherwise return non-zero value.
+ *
+ * @dir_offset: The direction register offset.
+ * @pullen_offset: The pull-up/pull-down enable register offset.
+ * @pinmux_offset: The pinmux register offset.
+ *
+ * @type1_start: Some chips have two base addresses for pull select register,
+ *  that means some pins use the first address and others use the second. This
+ *  member record the start of pin number to use the second address.
+ * @type1_end: The end of pin number to use the second address.
+ *
+ * @port_shf: The shift between two registers.
+ * @port_mask: The mask of register.
+ * @port_align: Provide clear register and set register step.
+ */
+struct mtk_pinctrl_devdata {
+	const struct mtk_desc_pin	*pins;
+	unsigned int				npins;
+	const struct mtk_drv_group_desc	*grp_desc;
+	unsigned int	n_grp_cls;
+	const struct mtk_pin_drv_grp	*pin_drv_grp;
+	unsigned int	n_pin_drv_grps;
+	int (*spec_pull_set)(struct regmap *reg, unsigned int pin,
+			unsigned char align, bool isup, unsigned int arg);
+	int (*spec_ies_smt_set)(struct regmap *reg, unsigned int pin,
+			unsigned char align, int value);
+	unsigned int dir_offset;
+	unsigned int ies_offset;
+	unsigned int smt_offset;
+	unsigned int pullen_offset;
+	unsigned int pullsel_offset;
+	unsigned int drv_offset;
+	unsigned int invser_offset;
+	unsigned int dout_offset;
+	unsigned int din_offset;
+	unsigned int pinmux_offset;
+	unsigned short type1_start;
+	unsigned short type1_end;
+	unsigned char  port_shf;
+	unsigned char  port_mask;
+	unsigned char  port_align;
+	unsigned char	chip_type;
+	struct mtk_eint_offsets eint_offsets;
+	unsigned int	ap_num;
+	unsigned int	db_cnt;
+};
+
+struct mtk_pinctrl {
+	struct regmap	*regmap1;
+	struct regmap	*regmap2;
+	struct device           *dev;
+	struct gpio_chip	*chip;
+	struct mtk_pinctrl_group	*groups;
+	unsigned			ngroups;
+	const char          **grp_names;
+	struct pinctrl_dev      *pctl_dev;
+	const struct mtk_pinctrl_devdata  *devdata;
+	void __iomem		*eint_reg_base;
+	struct irq_domain	*domain;
+	int			*eint_dual_edges;
+};
+
+int mtk_pctrl_init(struct platform_device *pdev,
+		const struct mtk_pinctrl_devdata *data);
+
+#endif /* __PINCTRL_MTK_COMMON_H */
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-mt8135.h b/drivers/pinctrl/mediatek/pinctrl-mtk-mt8135.h
new file mode 100644
index 000000000000..e17aedb73c8d
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-mt8135.h
@@ -0,0 +1,2114 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __PINCTRL_MTK_MT8135_H
+#define __PINCTRL_MTK_MT8135_H
+
+#include <linux/pinctrl/pinctrl.h>
+#include "pinctrl-mtk-common.h"
+
+static const struct mtk_desc_pin mtk_pins_mt8135[] = {
+	MTK_PIN(
+		PINCTRL_PIN(0, "MSDC0_DAT7"),
+		"D21", "mt8135",
+		MTK_EINT_FUNCTION(2, 49),
+		MTK_FUNCTION(0, "GPIO0"),
+		MTK_FUNCTION(1, "MSDC0_DAT7"),
+		MTK_FUNCTION(2, "EINT49"),
+		MTK_FUNCTION(3, "I2SOUT_DAT"),
+		MTK_FUNCTION(4, "DAC_DAT_OUT"),
+		MTK_FUNCTION(5, "PCM1_DO"),
+		MTK_FUNCTION(6, "SPI1_MO"),
+		MTK_FUNCTION(7, "NALE")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(1, "MSDC0_DAT6"),
+		"D22", "mt8135",
+		MTK_EINT_FUNCTION(2, 48),
+		MTK_FUNCTION(0, "GPIO1"),
+		MTK_FUNCTION(1, "MSDC0_DAT6"),
+		MTK_FUNCTION(2, "EINT48"),
+		MTK_FUNCTION(3, "I2SIN_WS"),
+		MTK_FUNCTION(4, "DAC_WS"),
+		MTK_FUNCTION(5, "PCM1_WS"),
+		MTK_FUNCTION(6, "SPI1_CSN"),
+		MTK_FUNCTION(7, "NCLE")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(2, "MSDC0_DAT5"),
+		"E22", "mt8135",
+		MTK_EINT_FUNCTION(2, 47),
+		MTK_FUNCTION(0, "GPIO2"),
+		MTK_FUNCTION(1, "MSDC0_DAT5"),
+		MTK_FUNCTION(2, "EINT47"),
+		MTK_FUNCTION(3, "I2SIN_CK"),
+		MTK_FUNCTION(4, "DAC_CK"),
+		MTK_FUNCTION(5, "PCM1_CK"),
+		MTK_FUNCTION(6, "SPI1_CLK"),
+		MTK_FUNCTION(7, "NLD4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(3, "MSDC0_DAT4"),
+		"F21", "mt8135",
+		MTK_EINT_FUNCTION(2, 46),
+		MTK_FUNCTION(0, "GPIO3"),
+		MTK_FUNCTION(1, "MSDC0_DAT4"),
+		MTK_FUNCTION(2, "EINT46"),
+		MTK_FUNCTION(3, "A_FUNC_CK"),
+		MTK_FUNCTION(6, "LSCE1B_2X"),
+		MTK_FUNCTION(7, "NLD5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(4, "MSDC0_CMD"),
+		"F20", "mt8135",
+		MTK_EINT_FUNCTION(2, 41),
+		MTK_FUNCTION(0, "GPIO4"),
+		MTK_FUNCTION(1, "MSDC0_CMD"),
+		MTK_FUNCTION(2, "EINT41"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[0]"),
+		MTK_FUNCTION(5, "USB_TEST_IO[0]"),
+		MTK_FUNCTION(6, "LRSTB_2X"),
+		MTK_FUNCTION(7, "NRNB")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(5, "MSDC0_CLK"),
+		"G18", "mt8135",
+		MTK_EINT_FUNCTION(2, 40),
+		MTK_FUNCTION(0, "GPIO5"),
+		MTK_FUNCTION(1, "MSDC0_CLK"),
+		MTK_FUNCTION(2, "EINT40"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[1]"),
+		MTK_FUNCTION(5, "USB_TEST_IO[1]"),
+		MTK_FUNCTION(6, "LPTE"),
+		MTK_FUNCTION(7, "NREB")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(6, "MSDC0_DAT3"),
+		"G21", "mt8135",
+		MTK_EINT_FUNCTION(2, 45),
+		MTK_FUNCTION(0, "GPIO6"),
+		MTK_FUNCTION(1, "MSDC0_DAT3"),
+		MTK_FUNCTION(2, "EINT45"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[2]"),
+		MTK_FUNCTION(5, "USB_TEST_IO[2]"),
+		MTK_FUNCTION(6, "LSCE0B_2X"),
+		MTK_FUNCTION(7, "NLD7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(7, "MSDC0_DAT2"),
+		"E21", "mt8135",
+		MTK_EINT_FUNCTION(2, 44),
+		MTK_FUNCTION(0, "GPIO7"),
+		MTK_FUNCTION(1, "MSDC0_DAT2"),
+		MTK_FUNCTION(2, "EINT44"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[3]"),
+		MTK_FUNCTION(5, "USB_TEST_IO[3]"),
+		MTK_FUNCTION(6, "LSA0_2X"),
+		MTK_FUNCTION(7, "NLD14")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(8, "MSDC0_DAT1"),
+		"E23", "mt8135",
+		MTK_EINT_FUNCTION(2, 43),
+		MTK_FUNCTION(0, "GPIO8"),
+		MTK_FUNCTION(1, "MSDC0_DAT1"),
+		MTK_FUNCTION(2, "EINT43"),
+		MTK_FUNCTION(5, "USB_TEST_IO[4]"),
+		MTK_FUNCTION(6, "LSCK_2X"),
+		MTK_FUNCTION(7, "NLD11")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(9, "MSDC0_DAT0"),
+		"F22", "mt8135",
+		MTK_EINT_FUNCTION(2, 42),
+		MTK_FUNCTION(0, "GPIO9"),
+		MTK_FUNCTION(1, "MSDC0_DAT0"),
+		MTK_FUNCTION(2, "EINT42"),
+		MTK_FUNCTION(5, "USB_TEST_IO[5]"),
+		MTK_FUNCTION(6, "LSDA_2X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(10, "NCEB0"),
+		"G20", "mt8135",
+		MTK_EINT_FUNCTION(2, 139),
+		MTK_FUNCTION(0, "GPIO10"),
+		MTK_FUNCTION(1, "NCEB0"),
+		MTK_FUNCTION(2, "EINT139"),
+		MTK_FUNCTION(7, "TESTA_OUT4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(11, "NCEB1"),
+		"L17", "mt8135",
+		MTK_EINT_FUNCTION(2, 140),
+		MTK_FUNCTION(0, "GPIO11"),
+		MTK_FUNCTION(1, "NCEB1"),
+		MTK_FUNCTION(2, "EINT140"),
+		MTK_FUNCTION(6, "USB_DRVVBUS"),
+		MTK_FUNCTION(7, "TESTA_OUT5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(12, "NRNB"),
+		"G19", "mt8135",
+		MTK_EINT_FUNCTION(2, 141),
+		MTK_FUNCTION(0, "GPIO12"),
+		MTK_FUNCTION(1, "NRNB"),
+		MTK_FUNCTION(2, "EINT141"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[4]"),
+		MTK_FUNCTION(7, "TESTA_OUT6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(13, "NCLE"),
+		"J18", "mt8135",
+		MTK_EINT_FUNCTION(2, 142),
+		MTK_FUNCTION(0, "GPIO13"),
+		MTK_FUNCTION(1, "NCLE"),
+		MTK_FUNCTION(2, "EINT142"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[5]"),
+		MTK_FUNCTION(4, "CM2PDN_1X"),
+		MTK_FUNCTION(6, "NALE"),
+		MTK_FUNCTION(7, "TESTA_OUT7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(14, "NALE"),
+		"J19", "mt8135",
+		MTK_EINT_FUNCTION(2, 143),
+		MTK_FUNCTION(0, "GPIO14"),
+		MTK_FUNCTION(1, "NALE"),
+		MTK_FUNCTION(2, "EINT143"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[6]"),
+		MTK_FUNCTION(4, "CM2MCLK_1X"),
+		MTK_FUNCTION(5, "IRDA_RXD"),
+		MTK_FUNCTION(6, "NCLE"),
+		MTK_FUNCTION(7, "TESTA_OUT8")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(15, "NREB"),
+		"L18", "mt8135",
+		MTK_EINT_FUNCTION(2, 144),
+		MTK_FUNCTION(0, "GPIO15"),
+		MTK_FUNCTION(1, "NREB"),
+		MTK_FUNCTION(2, "EINT144"),
+		MTK_FUNCTION(3, "A_FUNC_DOUT[7]"),
+		MTK_FUNCTION(4, "CM2RST_1X"),
+		MTK_FUNCTION(5, "IRDA_TXD"),
+		MTK_FUNCTION(7, "TESTA_OUT9")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(16, "NWEB"),
+		"J20", "mt8135",
+		MTK_EINT_FUNCTION(2, 145),
+		MTK_FUNCTION(0, "GPIO16"),
+		MTK_FUNCTION(1, "NWEB"),
+		MTK_FUNCTION(2, "EINT145"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[0]"),
+		MTK_FUNCTION(4, "CM2PCLK_1X"),
+		MTK_FUNCTION(5, "IRDA_PDN"),
+		MTK_FUNCTION(7, "TESTA_OUT10")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(17, "NLD0"),
+		"K21", "mt8135",
+		MTK_EINT_FUNCTION(2, 146),
+		MTK_FUNCTION(0, "GPIO17"),
+		MTK_FUNCTION(1, "NLD0"),
+		MTK_FUNCTION(2, "EINT146"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[1]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[0]"),
+		MTK_FUNCTION(5, "I2SIN_CK"),
+		MTK_FUNCTION(6, "DAC_CK"),
+		MTK_FUNCTION(7, "TESTA_OUT11")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(18, "NLD1"),
+		"K22", "mt8135",
+		MTK_EINT_FUNCTION(2, 147),
+		MTK_FUNCTION(0, "GPIO18"),
+		MTK_FUNCTION(1, "NLD1"),
+		MTK_FUNCTION(2, "EINT147"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[2]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[1]"),
+		MTK_FUNCTION(5, "I2SIN_WS"),
+		MTK_FUNCTION(6, "DAC_WS"),
+		MTK_FUNCTION(7, "TESTA_OUT12")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(19, "NLD2"),
+		"J21", "mt8135",
+		MTK_EINT_FUNCTION(2, 148),
+		MTK_FUNCTION(0, "GPIO19"),
+		MTK_FUNCTION(1, "NLD2"),
+		MTK_FUNCTION(2, "EINT148"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[3]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[2]"),
+		MTK_FUNCTION(5, "I2SOUT_DAT"),
+		MTK_FUNCTION(6, "DAC_DAT_OUT"),
+		MTK_FUNCTION(7, "TESTA_OUT13")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(20, "NLD3"),
+		"J23", "mt8135",
+		MTK_EINT_FUNCTION(2, 149),
+		MTK_FUNCTION(0, "GPIO20"),
+		MTK_FUNCTION(1, "NLD3"),
+		MTK_FUNCTION(2, "EINT149"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[4]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[3]"),
+		MTK_FUNCTION(7, "TESTA_OUT14")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(21, "NLD4"),
+		"J22", "mt8135",
+		MTK_EINT_FUNCTION(2, 150),
+		MTK_FUNCTION(0, "GPIO21"),
+		MTK_FUNCTION(1, "NLD4"),
+		MTK_FUNCTION(2, "EINT150"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[5]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[4]"),
+		MTK_FUNCTION(7, "TESTA_OUT15")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(22, "NLD5"),
+		"H21", "mt8135",
+		MTK_EINT_FUNCTION(2, 151),
+		MTK_FUNCTION(0, "GPIO22"),
+		MTK_FUNCTION(1, "NLD5"),
+		MTK_FUNCTION(2, "EINT151"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[6]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[5]"),
+		MTK_FUNCTION(7, "TESTA_OUT16")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(23, "NLD6"),
+		"H22", "mt8135",
+		MTK_EINT_FUNCTION(2, 152),
+		MTK_FUNCTION(0, "GPIO23"),
+		MTK_FUNCTION(1, "NLD6"),
+		MTK_FUNCTION(2, "EINT152"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[7]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[6]"),
+		MTK_FUNCTION(7, "TESTA_OUT17")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(24, "NLD7"),
+		"H20", "mt8135",
+		MTK_EINT_FUNCTION(2, 153),
+		MTK_FUNCTION(0, "GPIO24"),
+		MTK_FUNCTION(1, "NLD7"),
+		MTK_FUNCTION(2, "EINT153"),
+		MTK_FUNCTION(3, "A_FUNC_DIN[8]"),
+		MTK_FUNCTION(4, "CM2DAT_1X[7]"),
+		MTK_FUNCTION(7, "TESTA_OUT18")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(25, "NLD8"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 154),
+		MTK_FUNCTION(0, "GPIO25"),
+		MTK_FUNCTION(1, "NLD8"),
+		MTK_FUNCTION(2, "EINT154"),
+		MTK_FUNCTION(4, "CM2DAT_1X[8]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(26, "NLD9"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 155),
+		MTK_FUNCTION(0, "GPIO26"),
+		MTK_FUNCTION(1, "NLD9"),
+		MTK_FUNCTION(2, "EINT155"),
+		MTK_FUNCTION(4, "CM2DAT_1X[9]"),
+		MTK_FUNCTION(5, "PWM1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(27, "NLD10"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 156),
+		MTK_FUNCTION(0, "GPIO27"),
+		MTK_FUNCTION(1, "NLD10"),
+		MTK_FUNCTION(2, "EINT156"),
+		MTK_FUNCTION(4, "CM2VSYNC_1X"),
+		MTK_FUNCTION(5, "PWM2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(28, "NLD11"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 157),
+		MTK_FUNCTION(0, "GPIO28"),
+		MTK_FUNCTION(1, "NLD11"),
+		MTK_FUNCTION(2, "EINT157"),
+		MTK_FUNCTION(4, "CM2HSYNC_1X"),
+		MTK_FUNCTION(5, "PWM3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(29, "NLD12"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 158),
+		MTK_FUNCTION(0, "GPIO29"),
+		MTK_FUNCTION(1, "NLD12"),
+		MTK_FUNCTION(2, "EINT158"),
+		MTK_FUNCTION(3, "I2SIN_CK"),
+		MTK_FUNCTION(4, "DAC_CK"),
+		MTK_FUNCTION(5, "PCM1_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(30, "NLD13"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 159),
+		MTK_FUNCTION(0, "GPIO30"),
+		MTK_FUNCTION(1, "NLD13"),
+		MTK_FUNCTION(2, "EINT159"),
+		MTK_FUNCTION(3, "I2SIN_WS"),
+		MTK_FUNCTION(4, "DAC_WS"),
+		MTK_FUNCTION(5, "PCM1_WS")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(31, "NLD14"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 160),
+		MTK_FUNCTION(0, "GPIO31"),
+		MTK_FUNCTION(1, "NLD14"),
+		MTK_FUNCTION(2, "EINT160"),
+		MTK_FUNCTION(3, "I2SOUT_DAT"),
+		MTK_FUNCTION(4, "DAC_DAT_OUT"),
+		MTK_FUNCTION(5, "PCM1_DO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(32, "NLD15"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 161),
+		MTK_FUNCTION(0, "GPIO32"),
+		MTK_FUNCTION(1, "NLD15"),
+		MTK_FUNCTION(2, "EINT161"),
+		MTK_FUNCTION(3, "DISP_PWM"),
+		MTK_FUNCTION(4, "PWM4"),
+		MTK_FUNCTION(5, "PCM1_DI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(33, "MSDC0_RSTB"),
+		"G22", "mt8135",
+		MTK_EINT_FUNCTION(2, 50),
+		MTK_FUNCTION(0, "GPIO33"),
+		MTK_FUNCTION(1, "MSDC0_RSTB"),
+		MTK_FUNCTION(2, "EINT50"),
+		MTK_FUNCTION(3, "I2SIN_DAT"),
+		MTK_FUNCTION(5, "PCM1_DI"),
+		MTK_FUNCTION(6, "SPI1_MI"),
+		MTK_FUNCTION(7, "NLD10")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(34, "IDDIG"),
+		"N17", "mt8135",
+		MTK_EINT_FUNCTION(2, 34),
+		MTK_FUNCTION(0, "GPIO34"),
+		MTK_FUNCTION(1, "IDDIG"),
+		MTK_FUNCTION(2, "EINT34")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(35, "SCL3"),
+		"L19", "mt8135",
+		MTK_EINT_FUNCTION(2, 96),
+		MTK_FUNCTION(0, "GPIO35"),
+		MTK_FUNCTION(1, "SCL3"),
+		MTK_FUNCTION(2, "EINT96"),
+		MTK_FUNCTION(3, "CLKM6"),
+		MTK_FUNCTION(4, "PWM6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(36, "SDA3"),
+		"L20", "mt8135",
+		MTK_EINT_FUNCTION(2, 97),
+		MTK_FUNCTION(0, "GPIO36"),
+		MTK_FUNCTION(1, "SDA3"),
+		MTK_FUNCTION(2, "EINT97")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(37, "AUD_CLK_MOSI"),
+		"L21", "mt8135",
+		MTK_EINT_FUNCTION(4, 19),
+		MTK_FUNCTION(0, "GPIO37"),
+		MTK_FUNCTION(1, "AUD_CLK"),
+		MTK_FUNCTION(2, "ADC_CK"),
+		MTK_FUNCTION(3, " HDMI_SDATA0"),
+		MTK_FUNCTION(4, "EINT19"),
+		MTK_FUNCTION(5, "USB_TEST_IO[6]"),
+		MTK_FUNCTION(7, "TESTA_OUT19")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(38, "AUD_DAT_MOSI"),
+		"L23", "mt8135",
+		MTK_EINT_FUNCTION(4, 21),
+		MTK_FUNCTION(0, "GPIO38"),
+		MTK_FUNCTION(1, "AUD_DAT_MOSI"),
+		MTK_FUNCTION(2, "ADC_WS"),
+		MTK_FUNCTION(3, "AUD_DAT_MISO"),
+		MTK_FUNCTION(4, "EINT21"),
+		MTK_FUNCTION(5, "USB_TEST_IO[7]"),
+		MTK_FUNCTION(7, "TESTA_OUT20")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(39, "AUD_DAT_MISO"),
+		"L22", "mt8135",
+		MTK_EINT_FUNCTION(4, 20),
+		MTK_FUNCTION(0, "GPIO39"),
+		MTK_FUNCTION(1, "AUD_DAT_MISO"),
+		MTK_FUNCTION(2, "ADC_DAT_IN"),
+		MTK_FUNCTION(3, "AUD_DAT_MOSI"),
+		MTK_FUNCTION(4, "EINT20"),
+		MTK_FUNCTION(5, "USB_TEST_IO[8]"),
+		MTK_FUNCTION(7, "TESTA_OUT21")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(40, "DAC_CLK"),
+		"P21", "mt8135",
+		MTK_EINT_FUNCTION(2, 22),
+		MTK_FUNCTION(0, "GPIO40"),
+		MTK_FUNCTION(1, "DAC_CK"),
+		MTK_FUNCTION(2, "EINT22"),
+		MTK_FUNCTION(3, " HDMI_SDATA1"),
+		MTK_FUNCTION(5, "USB_TEST_IO[9]"),
+		MTK_FUNCTION(7, "TESTA_OUT22")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(41, "DAC_WS"),
+		"N18", "mt8135",
+		MTK_EINT_FUNCTION(2, 24),
+		MTK_FUNCTION(0, "GPIO41"),
+		MTK_FUNCTION(1, "DAC_WS"),
+		MTK_FUNCTION(2, "EINT24"),
+		MTK_FUNCTION(3, " HDMI_SDATA2"),
+		MTK_FUNCTION(5, "USB_TEST_IO[10]"),
+		MTK_FUNCTION(7, "TESTA_OUT23")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(42, "DAC_DAT_OUT"),
+		"N22", "mt8135",
+		MTK_EINT_FUNCTION(2, 23),
+		MTK_FUNCTION(0, "GPIO42"),
+		MTK_FUNCTION(1, "DAC_DAT_OUT"),
+		MTK_FUNCTION(2, "EINT23"),
+		MTK_FUNCTION(3, " HDMI_SDATA3"),
+		MTK_FUNCTION(5, "USB_TEST_IO[11]"),
+		MTK_FUNCTION(7, "TESTA_OUT24")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(43, "PWRAP_SPI0_MO"),
+		"M22", "mt8135",
+		MTK_EINT_FUNCTION(2, 29),
+		MTK_FUNCTION(0, "GPIO43"),
+		MTK_FUNCTION(1, "PWRAP_SPIDI"),
+		MTK_FUNCTION(2, "EINT29")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(44, "PWRAP_SPI0_MI"),
+		"P23", "mt8135",
+		MTK_EINT_FUNCTION(2, 28),
+		MTK_FUNCTION(0, "GPIO44"),
+		MTK_FUNCTION(1, "PWRAP_SPIDO"),
+		MTK_FUNCTION(2, "EINT28")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(45, "PWRAP_SPI0_CSN"),
+		"M21", "mt8135",
+		MTK_EINT_FUNCTION(2, 27),
+		MTK_FUNCTION(0, "GPIO45"),
+		MTK_FUNCTION(1, "PWRAP_SPICS_B_I"),
+		MTK_FUNCTION(2, "EINT27")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(46, "PWRAP_SPI0_CLK"),
+		"P22", "mt8135",
+		MTK_EINT_FUNCTION(2, 26),
+		MTK_FUNCTION(0, "GPIO46"),
+		MTK_FUNCTION(1, "PWRAP_SPICK_I"),
+		MTK_FUNCTION(2, "EINT26")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(47, "PWRAP_EVENT"),
+		"M23", "mt8135",
+		MTK_EINT_FUNCTION(2, 25),
+		MTK_FUNCTION(0, "GPIO47"),
+		MTK_FUNCTION(1, "PWRAP_EVENT_IN"),
+		MTK_FUNCTION(2, "EINT25"),
+		MTK_FUNCTION(7, "TESTA_OUT2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(48, "RTC32K_CK"),
+		"N20", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO48"),
+		MTK_FUNCTION(1, "RTC32K_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(49, "WATCHDOG"),
+		"R22", "mt8135",
+		MTK_EINT_FUNCTION(2, 36),
+		MTK_FUNCTION(0, "GPIO49"),
+		MTK_FUNCTION(1, "WATCHDOG"),
+		MTK_FUNCTION(2, "EINT36")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(50, "SRCLKENA"),
+		"T22", "mt8135",
+		MTK_EINT_FUNCTION(2, 38),
+		MTK_FUNCTION(0, "GPIO50"),
+		MTK_FUNCTION(1, "SRCLKENA"),
+		MTK_FUNCTION(2, "EINT38")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(51, "SRCVOLTEN"),
+		"T23", "mt8135",
+		MTK_EINT_FUNCTION(2, 37),
+		MTK_FUNCTION(0, "GPIO51"),
+		MTK_FUNCTION(1, "SRCVOLTEN"),
+		MTK_FUNCTION(2, "EINT37")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(52, "EINT0"),
+		"T21", "mt8135",
+		MTK_EINT_FUNCTION(1, 0),
+		MTK_FUNCTION(0, "GPIO52"),
+		MTK_FUNCTION(1, "EINT0"),
+		MTK_FUNCTION(2, "PWM1"),
+		MTK_FUNCTION(3, "CLKM0"),
+		MTK_FUNCTION(4, " SPDIF_OUT"),
+		MTK_FUNCTION(5, "USB_TEST_IO[12]"),
+		MTK_FUNCTION(7, "USB_SCL")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(53, "URXD2"),
+		"R18", "mt8135",
+		MTK_EINT_FUNCTION(2, 83),
+		MTK_FUNCTION(0, "GPIO53"),
+		MTK_FUNCTION(1, "URXD2"),
+		MTK_FUNCTION(2, "EINT83"),
+		MTK_FUNCTION(4, " HDMI_LRCK"),
+		MTK_FUNCTION(5, "CLKM3"),
+		MTK_FUNCTION(7, "UTXD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(54, "UTXD2"),
+		"R17", "mt8135",
+		MTK_EINT_FUNCTION(2, 82),
+		MTK_FUNCTION(0, "GPIO54"),
+		MTK_FUNCTION(1, "UTXD2"),
+		MTK_FUNCTION(2, "EINT82"),
+		MTK_FUNCTION(4, " HDMI_BCK_OUT"),
+		MTK_FUNCTION(5, "CLKM2"),
+		MTK_FUNCTION(7, "URXD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(55, "UCTS2"),
+		"R20", "mt8135",
+		MTK_EINT_FUNCTION(2, 84),
+		MTK_FUNCTION(0, "GPIO55"),
+		MTK_FUNCTION(1, "UCTS2"),
+		MTK_FUNCTION(2, "EINT84"),
+		MTK_FUNCTION(5, "PWM1"),
+		MTK_FUNCTION(7, "URTS2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(56, "URTS2"),
+		"R19", "mt8135",
+		MTK_EINT_FUNCTION(2, 85),
+		MTK_FUNCTION(0, "GPIO56"),
+		MTK_FUNCTION(1, "URTS2"),
+		MTK_FUNCTION(2, "EINT85"),
+		MTK_FUNCTION(5, "PWM2"),
+		MTK_FUNCTION(7, "UCTS2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(57, "JTCK"),
+		"V17", "mt8135",
+		MTK_EINT_FUNCTION(2, 188),
+		MTK_FUNCTION(0, "GPIO57"),
+		MTK_FUNCTION(1, "JTCK"),
+		MTK_FUNCTION(2, "EINT188"),
+		MTK_FUNCTION(3, "DSP1_ICK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(58, "JTDO"),
+		"T16", "mt8135",
+		MTK_EINT_FUNCTION(2, 190),
+		MTK_FUNCTION(0, "GPIO58"),
+		MTK_FUNCTION(1, "JTDO"),
+		MTK_FUNCTION(2, "EINT190"),
+		MTK_FUNCTION(3, "DSP2_IMS")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(59, "JTRST_B"),
+		"T19", "mt8135",
+		MTK_EINT_FUNCTION(2, 0),
+		MTK_FUNCTION(0, "GPIO59"),
+		MTK_FUNCTION(1, "JTRST_B"),
+		MTK_FUNCTION(2, "EINT0"),
+		MTK_FUNCTION(3, "DSP2_ICK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(60, "JTDI"),
+		"T18", "mt8135",
+		MTK_EINT_FUNCTION(2, 189),
+		MTK_FUNCTION(0, "GPIO60"),
+		MTK_FUNCTION(1, "JTDI"),
+		MTK_FUNCTION(2, "EINT189"),
+		MTK_FUNCTION(3, "DSP1_IMS")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(61, "JRTCK"),
+		"T20", "mt8135",
+		MTK_EINT_FUNCTION(2, 187),
+		MTK_FUNCTION(0, "GPIO61"),
+		MTK_FUNCTION(1, "JRTCK"),
+		MTK_FUNCTION(2, "EINT187"),
+		MTK_FUNCTION(3, "DSP1_ID")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(62, "JTMS"),
+		"T17", "mt8135",
+		MTK_EINT_FUNCTION(2, 191),
+		MTK_FUNCTION(0, "GPIO62"),
+		MTK_FUNCTION(1, "JTMS"),
+		MTK_FUNCTION(2, "EINT191"),
+		MTK_FUNCTION(3, "DSP2_ID")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(63, "MSDC1_INSI"),
+		"V18", "mt8135",
+		MTK_EINT_FUNCTION(1, 15),
+		MTK_FUNCTION(0, "GPIO63"),
+		MTK_FUNCTION(1, "MSDC1_INSI"),
+		MTK_FUNCTION(3, "SCL5"),
+		MTK_FUNCTION(4, "PWM6"),
+		MTK_FUNCTION(5, "CLKM5"),
+		MTK_FUNCTION(7, "TESTB_OUT6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(64, "MSDC1_SDWPI"),
+		"W18", "mt8135",
+		MTK_EINT_FUNCTION(2, 58),
+		MTK_FUNCTION(0, "GPIO64"),
+		MTK_FUNCTION(1, "MSDC1_SDWPI"),
+		MTK_FUNCTION(2, "EINT58"),
+		MTK_FUNCTION(3, "SDA5"),
+		MTK_FUNCTION(4, "PWM7"),
+		MTK_FUNCTION(5, "CLKM6"),
+		MTK_FUNCTION(7, "TESTB_OUT7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(65, "MSDC2_INSI"),
+		"U22", "mt8135",
+		MTK_EINT_FUNCTION(1, 14),
+		MTK_FUNCTION(0, "GPIO65"),
+		MTK_FUNCTION(1, "MSDC2_INSI"),
+		MTK_FUNCTION(5, "USB_TEST_IO[27]"),
+		MTK_FUNCTION(7, "TESTA_OUT3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(66, "MSDC2_SDWPI"),
+		"U21", "mt8135",
+		MTK_EINT_FUNCTION(2, 66),
+		MTK_FUNCTION(0, "GPIO66"),
+		MTK_FUNCTION(1, "MSDC2_SDWPI"),
+		MTK_FUNCTION(2, "EINT66"),
+		MTK_FUNCTION(5, "USB_TEST_IO[28]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(67, "URXD4"),
+		"V23", "mt8135",
+		MTK_EINT_FUNCTION(2, 89),
+		MTK_FUNCTION(0, "GPIO67"),
+		MTK_FUNCTION(1, "URXD4"),
+		MTK_FUNCTION(2, "EINT89"),
+		MTK_FUNCTION(3, "URXD1"),
+		MTK_FUNCTION(6, "UTXD4"),
+		MTK_FUNCTION(7, "TESTB_OUT10")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(68, "UTXD4"),
+		"V22", "mt8135",
+		MTK_EINT_FUNCTION(2, 88),
+		MTK_FUNCTION(0, "GPIO68"),
+		MTK_FUNCTION(1, "UTXD4"),
+		MTK_FUNCTION(2, "EINT88"),
+		MTK_FUNCTION(3, "UTXD1"),
+		MTK_FUNCTION(6, "URXD4"),
+		MTK_FUNCTION(7, "TESTB_OUT11")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(69, "URXD1"),
+		"W22", "mt8135",
+		MTK_EINT_FUNCTION(2, 79),
+		MTK_FUNCTION(0, "GPIO69"),
+		MTK_FUNCTION(1, "URXD1"),
+		MTK_FUNCTION(2, "EINT79"),
+		MTK_FUNCTION(3, "URXD4"),
+		MTK_FUNCTION(6, "UTXD1"),
+		MTK_FUNCTION(7, "TESTB_OUT24")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(70, "UTXD1"),
+		"V21", "mt8135",
+		MTK_EINT_FUNCTION(2, 78),
+		MTK_FUNCTION(0, "GPIO70"),
+		MTK_FUNCTION(1, "UTXD1"),
+		MTK_FUNCTION(2, "EINT78"),
+		MTK_FUNCTION(3, "UTXD4"),
+		MTK_FUNCTION(6, "URXD1"),
+		MTK_FUNCTION(7, "TESTB_OUT25")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(71, "UCTS1"),
+		"V19", "mt8135",
+		MTK_EINT_FUNCTION(2, 80),
+		MTK_FUNCTION(0, "GPIO71"),
+		MTK_FUNCTION(1, "UCTS1"),
+		MTK_FUNCTION(2, "EINT80"),
+		MTK_FUNCTION(5, "CLKM0"),
+		MTK_FUNCTION(6, "URTS1"),
+		MTK_FUNCTION(7, "TESTB_OUT31")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(72, "URTS1"),
+		"V20", "mt8135",
+		MTK_EINT_FUNCTION(2, 81),
+		MTK_FUNCTION(0, "GPIO72"),
+		MTK_FUNCTION(1, "URTS1"),
+		MTK_FUNCTION(2, "EINT81"),
+		MTK_FUNCTION(5, "CLKM1"),
+		MTK_FUNCTION(6, "UCTS1"),
+		MTK_FUNCTION(7, "TESTB_OUT21")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(73, "PWM1"),
+		"W17", "mt8135",
+		MTK_EINT_FUNCTION(2, 73),
+		MTK_FUNCTION(0, "GPIO73"),
+		MTK_FUNCTION(1, "PWM1"),
+		MTK_FUNCTION(2, "EINT73"),
+		MTK_FUNCTION(5, "USB_DRVVBUS"),
+		MTK_FUNCTION(6, "DISP_PWM"),
+		MTK_FUNCTION(7, "TESTB_OUT8")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(74, "PWM2"),
+		"Y17", "mt8135",
+		MTK_EINT_FUNCTION(2, 74),
+		MTK_FUNCTION(0, "GPIO74"),
+		MTK_FUNCTION(1, "PWM2"),
+		MTK_FUNCTION(2, "EINT74"),
+		MTK_FUNCTION(3, "DPI33_CK"),
+		MTK_FUNCTION(4, "PWM5"),
+		MTK_FUNCTION(5, "URXD2"),
+		MTK_FUNCTION(6, "DISP_PWM"),
+		MTK_FUNCTION(7, "TESTB_OUT9")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(75, "PWM3"),
+		"Y19", "mt8135",
+		MTK_EINT_FUNCTION(2, 75),
+		MTK_FUNCTION(0, "GPIO75"),
+		MTK_FUNCTION(1, "PWM3"),
+		MTK_FUNCTION(2, "EINT75"),
+		MTK_FUNCTION(3, "DPI33_D0"),
+		MTK_FUNCTION(4, "PWM6"),
+		MTK_FUNCTION(5, "UTXD2"),
+		MTK_FUNCTION(6, "DISP_PWM"),
+		MTK_FUNCTION(7, "TESTB_OUT12")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(76, "PWM4"),
+		"W19", "mt8135",
+		MTK_EINT_FUNCTION(2, 76),
+		MTK_FUNCTION(0, "GPIO76"),
+		MTK_FUNCTION(1, "PWM4"),
+		MTK_FUNCTION(2, "EINT76"),
+		MTK_FUNCTION(3, "DPI33_D1"),
+		MTK_FUNCTION(4, "PWM7"),
+		MTK_FUNCTION(6, "DISP_PWM"),
+		MTK_FUNCTION(7, "TESTB_OUT13")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(77, "MSDC2_DAT2"),
+		"W21", "mt8135",
+		MTK_EINT_FUNCTION(2, 63),
+		MTK_FUNCTION(0, "GPIO77"),
+		MTK_FUNCTION(1, "MSDC2_DAT2"),
+		MTK_FUNCTION(2, "EINT63"),
+		MTK_FUNCTION(4, "DSP2_IMS"),
+		MTK_FUNCTION(6, "DPI33_D6"),
+		MTK_FUNCTION(7, "TESTA_OUT25")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(78, "MSDC2_DAT3"),
+		"AA23", "mt8135",
+		MTK_EINT_FUNCTION(2, 64),
+		MTK_FUNCTION(0, "GPIO78"),
+		MTK_FUNCTION(1, "MSDC2_DAT3"),
+		MTK_FUNCTION(2, "EINT64"),
+		MTK_FUNCTION(4, "DSP2_ID"),
+		MTK_FUNCTION(6, "DPI33_D7"),
+		MTK_FUNCTION(7, "TESTA_OUT26")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(79, "MSDC2_CMD"),
+		"Y22", "mt8135",
+		MTK_EINT_FUNCTION(2, 60),
+		MTK_FUNCTION(0, "GPIO79"),
+		MTK_FUNCTION(1, "MSDC2_CMD"),
+		MTK_FUNCTION(2, "EINT60"),
+		MTK_FUNCTION(4, "DSP1_IMS"),
+		MTK_FUNCTION(5, "PCM1_WS"),
+		MTK_FUNCTION(6, "DPI33_D3"),
+		MTK_FUNCTION(7, "TESTA_OUT0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(80, "MSDC2_CLK"),
+		"AA22", "mt8135",
+		MTK_EINT_FUNCTION(2, 59),
+		MTK_FUNCTION(0, "GPIO80"),
+		MTK_FUNCTION(1, "MSDC2_CLK"),
+		MTK_FUNCTION(2, "EINT59"),
+		MTK_FUNCTION(4, "DSP1_ICK"),
+		MTK_FUNCTION(5, "PCM1_CK"),
+		MTK_FUNCTION(6, "DPI33_D2"),
+		MTK_FUNCTION(7, "TESTA_OUT1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(81, "MSDC2_DAT1"),
+		"Y21", "mt8135",
+		MTK_EINT_FUNCTION(2, 62),
+		MTK_FUNCTION(0, "GPIO81"),
+		MTK_FUNCTION(1, "MSDC2_DAT1"),
+		MTK_FUNCTION(2, "EINT62"),
+		MTK_FUNCTION(4, "DSP2_ICK"),
+		MTK_FUNCTION(5, "PCM1_DO"),
+		MTK_FUNCTION(6, "DPI33_D5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(82, "MSDC2_DAT0"),
+		"AB22", "mt8135",
+		MTK_EINT_FUNCTION(2, 61),
+		MTK_FUNCTION(0, "GPIO82"),
+		MTK_FUNCTION(1, "MSDC2_DAT0"),
+		MTK_FUNCTION(2, "EINT61"),
+		MTK_FUNCTION(4, "DSP1_ID"),
+		MTK_FUNCTION(5, "PCM1_DI"),
+		MTK_FUNCTION(6, "DPI33_D4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(83, "MSDC1_DAT0"),
+		"AC19", "mt8135",
+		MTK_EINT_FUNCTION(2, 53),
+		MTK_FUNCTION(0, "GPIO83"),
+		MTK_FUNCTION(1, "MSDC1_DAT0"),
+		MTK_FUNCTION(2, "EINT53"),
+		MTK_FUNCTION(3, "SCL1"),
+		MTK_FUNCTION(4, "PWM2"),
+		MTK_FUNCTION(5, "CLKM1"),
+		MTK_FUNCTION(7, "TESTB_OUT2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(84, "MSDC1_DAT1"),
+		"AA19", "mt8135",
+		MTK_EINT_FUNCTION(2, 54),
+		MTK_FUNCTION(0, "GPIO84"),
+		MTK_FUNCTION(1, "MSDC1_DAT1"),
+		MTK_FUNCTION(2, "EINT54"),
+		MTK_FUNCTION(3, "SDA1"),
+		MTK_FUNCTION(4, "PWM3"),
+		MTK_FUNCTION(5, "CLKM2"),
+		MTK_FUNCTION(7, "TESTB_OUT3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(85, "MSDC1_CMD"),
+		"AA20", "mt8135",
+		MTK_EINT_FUNCTION(2, 52),
+		MTK_FUNCTION(0, "GPIO85"),
+		MTK_FUNCTION(1, "MSDC1_CMD"),
+		MTK_FUNCTION(2, "EINT52"),
+		MTK_FUNCTION(3, "SDA0"),
+		MTK_FUNCTION(4, "PWM1"),
+		MTK_FUNCTION(5, "CLKM0"),
+		MTK_FUNCTION(7, "TESTB_OUT1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(86, "MSDC1_CLK"),
+		"AB19", "mt8135",
+		MTK_EINT_FUNCTION(2, 51),
+		MTK_FUNCTION(0, "GPIO86"),
+		MTK_FUNCTION(1, "MSDC1_CLK"),
+		MTK_FUNCTION(2, "EINT51"),
+		MTK_FUNCTION(3, "SCL0"),
+		MTK_FUNCTION(4, "DISP_PWM"),
+		MTK_FUNCTION(7, "TESTB_OUT0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(87, "MSDC1_DAT2"),
+		"AA21", "mt8135",
+		MTK_EINT_FUNCTION(2, 55),
+		MTK_FUNCTION(0, "GPIO87"),
+		MTK_FUNCTION(1, "MSDC1_DAT2"),
+		MTK_FUNCTION(2, "EINT55"),
+		MTK_FUNCTION(3, "SCL4"),
+		MTK_FUNCTION(4, "PWM4"),
+		MTK_FUNCTION(5, "CLKM3"),
+		MTK_FUNCTION(7, "TESTB_OUT4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(88, "MSDC1_DAT3"),
+		"AB20", "mt8135",
+		MTK_EINT_FUNCTION(2, 56),
+		MTK_FUNCTION(0, "GPIO88"),
+		MTK_FUNCTION(1, "MSDC1_DAT3"),
+		MTK_FUNCTION(2, "EINT56"),
+		MTK_FUNCTION(3, "SDA4"),
+		MTK_FUNCTION(4, "PWM5"),
+		MTK_FUNCTION(5, "CLKM4"),
+		MTK_FUNCTION(7, "TESTB_OUT5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(89, "MSDC4_DAT0"),
+		"AB8", "mt8135",
+		MTK_EINT_FUNCTION(2, 133),
+		MTK_FUNCTION(0, "GPIO89"),
+		MTK_FUNCTION(1, "MSDC4_DAT0"),
+		MTK_FUNCTION(2, "EINT133"),
+		MTK_FUNCTION(4, "EXT_FRAME_SYNC"),
+		MTK_FUNCTION(5, "USB_DRVVBUS"),
+		MTK_FUNCTION(6, "A_FUNC_DIN[9]"),
+		MTK_FUNCTION(7, "LPTE")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(90, "MSDC4_DAT1"),
+		"AB7", "mt8135",
+		MTK_EINT_FUNCTION(2, 134),
+		MTK_FUNCTION(0, "GPIO90"),
+		MTK_FUNCTION(1, "MSDC4_DAT1"),
+		MTK_FUNCTION(2, "EINT134"),
+		MTK_FUNCTION(6, "A_FUNC_DIN[10]"),
+		MTK_FUNCTION(7, "LRSTB_1X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(91, "MSDC4_DAT5"),
+		"AA8", "mt8135",
+		MTK_EINT_FUNCTION(2, 136),
+		MTK_FUNCTION(0, "GPIO91"),
+		MTK_FUNCTION(1, "MSDC4_DAT5"),
+		MTK_FUNCTION(2, "EINT136"),
+		MTK_FUNCTION(3, "I2SIN_WS"),
+		MTK_FUNCTION(4, "DAC_WS"),
+		MTK_FUNCTION(5, "PCM1_WS"),
+		MTK_FUNCTION(6, "A_FUNC_DIN[11]"),
+		MTK_FUNCTION(7, "SPI1_CSN")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(92, "MSDC4_DAT6"),
+		"AC4", "mt8135",
+		MTK_EINT_FUNCTION(2, 137),
+		MTK_FUNCTION(0, "GPIO92"),
+		MTK_FUNCTION(1, "MSDC4_DAT6"),
+		MTK_FUNCTION(2, "EINT137"),
+		MTK_FUNCTION(3, "I2SOUT_DAT"),
+		MTK_FUNCTION(4, "DAC_DAT_OUT"),
+		MTK_FUNCTION(5, "PCM1_DO"),
+		MTK_FUNCTION(6, "A_FUNC_DIN[12]"),
+		MTK_FUNCTION(7, "SPI1_MO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(93, "MSDC4_DAT7"),
+		"AC6", "mt8135",
+		MTK_EINT_FUNCTION(2, 138),
+		MTK_FUNCTION(0, "GPIO93"),
+		MTK_FUNCTION(1, "MSDC4_DAT7"),
+		MTK_FUNCTION(2, "EINT138"),
+		MTK_FUNCTION(3, "I2SIN_DAT"),
+		MTK_FUNCTION(5, "PCM1_DI"),
+		MTK_FUNCTION(6, "A_FUNC_DIN[13]"),
+		MTK_FUNCTION(7, "SPI1_MI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(94, "MSDC4_DAT4"),
+		"AA7", "mt8135",
+		MTK_EINT_FUNCTION(2, 135),
+		MTK_FUNCTION(0, "GPIO94"),
+		MTK_FUNCTION(1, "MSDC4_DAT4"),
+		MTK_FUNCTION(2, "EINT135"),
+		MTK_FUNCTION(3, "I2SIN_CK"),
+		MTK_FUNCTION(4, "DAC_CK"),
+		MTK_FUNCTION(5, "PCM1_CK"),
+		MTK_FUNCTION(6, "A_FUNC_DIN[14]"),
+		MTK_FUNCTION(7, "SPI1_CLK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(95, "MSDC4_DAT2"),
+		"AB6", "mt8135",
+		MTK_EINT_FUNCTION(2, 131),
+		MTK_FUNCTION(0, "GPIO95"),
+		MTK_FUNCTION(1, "MSDC4_DAT2"),
+		MTK_FUNCTION(2, "EINT131"),
+		MTK_FUNCTION(3, "I2SIN_WS"),
+		MTK_FUNCTION(4, "CM2PDN_2X"),
+		MTK_FUNCTION(5, "DAC_WS"),
+		MTK_FUNCTION(6, "PCM1_WS"),
+		MTK_FUNCTION(7, "LSCE0B_1X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(96, "MSDC4_CLK"),
+		"AB5", "mt8135",
+		MTK_EINT_FUNCTION(2, 129),
+		MTK_FUNCTION(0, "GPIO96"),
+		MTK_FUNCTION(1, "MSDC4_CLK"),
+		MTK_FUNCTION(2, "EINT129"),
+		MTK_FUNCTION(3, "DPI1_CK_2X"),
+		MTK_FUNCTION(4, "CM2PCLK_2X"),
+		MTK_FUNCTION(5, "PWM4"),
+		MTK_FUNCTION(6, "PCM1_DI"),
+		MTK_FUNCTION(7, "LSCK_1X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(97, "MSDC4_DAT3"),
+		"Y8", "mt8135",
+		MTK_EINT_FUNCTION(2, 132),
+		MTK_FUNCTION(0, "GPIO97"),
+		MTK_FUNCTION(1, "MSDC4_DAT3"),
+		MTK_FUNCTION(2, "EINT132"),
+		MTK_FUNCTION(3, "I2SOUT_DAT"),
+		MTK_FUNCTION(4, "CM2RST_2X"),
+		MTK_FUNCTION(5, "DAC_DAT_OUT"),
+		MTK_FUNCTION(6, "PCM1_DO"),
+		MTK_FUNCTION(7, "LSCE1B_1X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(98, "MSDC4_CMD"),
+		"AC3", "mt8135",
+		MTK_EINT_FUNCTION(2, 128),
+		MTK_FUNCTION(0, "GPIO98"),
+		MTK_FUNCTION(1, "MSDC4_CMD"),
+		MTK_FUNCTION(2, "EINT128"),
+		MTK_FUNCTION(3, "DPI1_DE_2X"),
+		MTK_FUNCTION(5, "PWM3"),
+		MTK_FUNCTION(7, "LSDA_1X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(99, "MSDC4_RSTB"),
+		"AB4", "mt8135",
+		MTK_EINT_FUNCTION(2, 130),
+		MTK_FUNCTION(0, "GPIO99"),
+		MTK_FUNCTION(1, "MSDC4_RSTB"),
+		MTK_FUNCTION(2, "EINT130"),
+		MTK_FUNCTION(3, "I2SIN_CK"),
+		MTK_FUNCTION(4, "CM2MCLK_2X"),
+		MTK_FUNCTION(5, "DAC_CK"),
+		MTK_FUNCTION(6, "PCM1_CK"),
+		MTK_FUNCTION(7, "LSA0_1X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(100, "SDA0"),
+		"W9", "mt8135",
+		MTK_EINT_FUNCTION(2, 91),
+		MTK_FUNCTION(0, "GPIO100"),
+		MTK_FUNCTION(1, "SDA0"),
+		MTK_FUNCTION(2, "EINT91"),
+		MTK_FUNCTION(3, "CLKM1"),
+		MTK_FUNCTION(4, "PWM1"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[15]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(101, "SCL0"),
+		"W11", "mt8135",
+		MTK_EINT_FUNCTION(2, 90),
+		MTK_FUNCTION(0, "GPIO101"),
+		MTK_FUNCTION(1, "SCL0"),
+		MTK_FUNCTION(2, "EINT90"),
+		MTK_FUNCTION(3, "CLKM0"),
+		MTK_FUNCTION(4, "DISP_PWM"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[16]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(102, "EINT10_AUXIN2"),
+		"AA3", "mt8135",
+		MTK_EINT_FUNCTION(1, 10),
+		MTK_FUNCTION(0, "GPIO102"),
+		MTK_FUNCTION(1, "EINT10"),
+		MTK_FUNCTION(5, "USB_TEST_IO[16]"),
+		MTK_FUNCTION(6, "TESTB_OUT16"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[17]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(103, "EINT11_AUXIN3"),
+		"AB2", "mt8135",
+		MTK_EINT_FUNCTION(1, 11),
+		MTK_FUNCTION(0, "GPIO103"),
+		MTK_FUNCTION(1, "EINT11"),
+		MTK_FUNCTION(5, "USB_TEST_IO[17]"),
+		MTK_FUNCTION(6, "TESTB_OUT17"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[18]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(104, "EINT16_AUXIN4"),
+		"AB3", "mt8135",
+		MTK_EINT_FUNCTION(1, 16),
+		MTK_FUNCTION(0, "GPIO104"),
+		MTK_FUNCTION(1, "EINT16"),
+		MTK_FUNCTION(5, "USB_TEST_IO[18]"),
+		MTK_FUNCTION(6, "TESTB_OUT18"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[19]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(105, "I2S_CLK"),
+		"W6", "mt8135",
+		MTK_EINT_FUNCTION(2, 10),
+		MTK_FUNCTION(0, "GPIO105"),
+		MTK_FUNCTION(1, "I2SIN_CK"),
+		MTK_FUNCTION(2, "EINT10"),
+		MTK_FUNCTION(3, "DAC_CK"),
+		MTK_FUNCTION(4, "PCM1_CK"),
+		MTK_FUNCTION(5, "USB_TEST_IO[19]"),
+		MTK_FUNCTION(6, "TESTB_OUT19"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[20]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(106, "I2S_WS"),
+		"AA6", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO106"),
+		MTK_FUNCTION(1, "I2SIN_WS"),
+		MTK_FUNCTION(3, "DAC_WS"),
+		MTK_FUNCTION(4, "PCM1_WS"),
+		MTK_FUNCTION(5, "USB_TEST_IO[20]"),
+		MTK_FUNCTION(6, "TESTB_OUT20"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[21]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(107, "I2S_DATA_IN"),
+		"AA5", "mt8135",
+		MTK_EINT_FUNCTION(2, 11),
+		MTK_FUNCTION(0, "GPIO107"),
+		MTK_FUNCTION(1, "I2SIN_DAT"),
+		MTK_FUNCTION(2, "EINT11"),
+		MTK_FUNCTION(4, "PCM1_DI"),
+		MTK_FUNCTION(5, "USB_TEST_IO[21]"),
+		MTK_FUNCTION(6, "TESTB_OUT22"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[22]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(108, "I2S_DATA_OUT"),
+		"AA4", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO108"),
+		MTK_FUNCTION(1, "I2SOUT_DAT"),
+		MTK_FUNCTION(3, "DAC_DAT_OUT"),
+		MTK_FUNCTION(4, "PCM1_DO"),
+		MTK_FUNCTION(5, "USB_TEST_IO[22]"),
+		MTK_FUNCTION(6, "TESTB_OUT23"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[23]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(109, "EINT5"),
+		"W5", "mt8135",
+		MTK_EINT_FUNCTION(1, 5),
+		MTK_FUNCTION(0, "GPIO109"),
+		MTK_FUNCTION(1, "EINT5"),
+		MTK_FUNCTION(2, "PWM5"),
+		MTK_FUNCTION(3, "CLKM3"),
+		MTK_FUNCTION(4, "GPU_JTRSTB"),
+		MTK_FUNCTION(5, "USB_TEST_IO[23]"),
+		MTK_FUNCTION(6, "TESTB_OUT26"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[24]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(110, "EINT6"),
+		"V5", "mt8135",
+		MTK_EINT_FUNCTION(1, 6),
+		MTK_FUNCTION(0, "GPIO110"),
+		MTK_FUNCTION(1, "EINT6"),
+		MTK_FUNCTION(2, "PWM6"),
+		MTK_FUNCTION(3, "CLKM4"),
+		MTK_FUNCTION(4, "GPU_JTMS"),
+		MTK_FUNCTION(5, "USB_TEST_IO[24]"),
+		MTK_FUNCTION(6, "TESTB_OUT27"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[25]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(111, "EINT7"),
+		"W3", "mt8135",
+		MTK_EINT_FUNCTION(1, 7),
+		MTK_FUNCTION(0, "GPIO111"),
+		MTK_FUNCTION(1, "EINT7"),
+		MTK_FUNCTION(2, "PWM7"),
+		MTK_FUNCTION(3, "CLKM5"),
+		MTK_FUNCTION(4, "GPU_JTDO"),
+		MTK_FUNCTION(5, "USB_TEST_IO[25]"),
+		MTK_FUNCTION(6, "TESTB_OUT28"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[26]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(112, "EINT8"),
+		"V6", "mt8135",
+		MTK_EINT_FUNCTION(1, 8),
+		MTK_FUNCTION(0, "GPIO112"),
+		MTK_FUNCTION(1, "EINT8"),
+		MTK_FUNCTION(2, "DISP_PWM"),
+		MTK_FUNCTION(3, "CLKM6"),
+		MTK_FUNCTION(4, "GPU_JTDI"),
+		MTK_FUNCTION(5, "USB_TEST_IO[26]"),
+		MTK_FUNCTION(6, "TESTB_OUT29"),
+		MTK_FUNCTION(7, "EXT_FRAME_SYNC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(113, "EINT9"),
+		"W8", "mt8135",
+		MTK_EINT_FUNCTION(1, 9),
+		MTK_FUNCTION(0, "GPIO113"),
+		MTK_FUNCTION(1, "EINT9"),
+		MTK_FUNCTION(4, "GPU_JTCK"),
+		MTK_FUNCTION(5, "USB_DRVVBUS"),
+		MTK_FUNCTION(6, "TESTB_OUT30"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[27]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(114, "LPCE1B"),
+		"W4", "mt8135",
+		MTK_EINT_FUNCTION(2, 127),
+		MTK_FUNCTION(0, "GPIO114"),
+		MTK_FUNCTION(1, "LPCE1B"),
+		MTK_FUNCTION(2, "EINT127"),
+		MTK_FUNCTION(5, "PWM2"),
+		MTK_FUNCTION(6, "TESTB_OUT14"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[28]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(115, "LPCE0B"),
+		"T5", "mt8135",
+		MTK_EINT_FUNCTION(2, 126),
+		MTK_FUNCTION(0, "GPIO115"),
+		MTK_FUNCTION(1, "LPCE0B"),
+		MTK_FUNCTION(2, "EINT126"),
+		MTK_FUNCTION(5, "PWM1"),
+		MTK_FUNCTION(6, "TESTB_OUT15"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[29]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(116, "DISP_PWM"),
+		"V4", "mt8135",
+		MTK_EINT_FUNCTION(2, 77),
+		MTK_FUNCTION(0, "GPIO116"),
+		MTK_FUNCTION(1, "DISP_PWM"),
+		MTK_FUNCTION(2, "EINT77"),
+		MTK_FUNCTION(3, "LSDI"),
+		MTK_FUNCTION(4, "PWM1"),
+		MTK_FUNCTION(5, "PWM2"),
+		MTK_FUNCTION(7, "PWM3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(117, "EINT1"),
+		"T6", "mt8135",
+		MTK_EINT_FUNCTION(1, 1),
+		MTK_FUNCTION(0, "GPIO117"),
+		MTK_FUNCTION(1, "EINT1"),
+		MTK_FUNCTION(2, "PWM2"),
+		MTK_FUNCTION(3, "CLKM1"),
+		MTK_FUNCTION(5, "USB_TEST_IO[13]"),
+		MTK_FUNCTION(7, "USB_SDA")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(118, "EINT2"),
+		"T4", "mt8135",
+		MTK_EINT_FUNCTION(1, 2),
+		MTK_FUNCTION(0, "GPIO118"),
+		MTK_FUNCTION(1, "EINT2"),
+		MTK_FUNCTION(2, "PWM3"),
+		MTK_FUNCTION(3, "CLKM2"),
+		MTK_FUNCTION(5, "USB_TEST_IO[14]"),
+		MTK_FUNCTION(6, "SRCLKENAI2"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[30]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(119, "EINT3"),
+		"R4", "mt8135",
+		MTK_EINT_FUNCTION(1, 3),
+		MTK_FUNCTION(0, "GPIO119"),
+		MTK_FUNCTION(1, "EINT3"),
+		MTK_FUNCTION(5, "USB_TEST_IO[15]"),
+		MTK_FUNCTION(6, "SRCLKENAI1"),
+		MTK_FUNCTION(7, "EXT_26M_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(120, "EINT4"),
+		"R5", "mt8135",
+		MTK_EINT_FUNCTION(1, 4),
+		MTK_FUNCTION(0, "GPIO120"),
+		MTK_FUNCTION(1, "EINT4"),
+		MTK_FUNCTION(2, "PWM4"),
+		MTK_FUNCTION(5, "USB_DRVVBUS"),
+		MTK_FUNCTION(7, "A_FUNC_DIN[31]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(121, "DPIDE"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 100),
+		MTK_FUNCTION(0, "GPIO121"),
+		MTK_FUNCTION(1, "DPI0_DE"),
+		MTK_FUNCTION(2, "EINT100"),
+		MTK_FUNCTION(3, "I2SOUT_DAT"),
+		MTK_FUNCTION(4, "DAC_DAT_OUT"),
+		MTK_FUNCTION(5, "PCM1_DO"),
+		MTK_FUNCTION(6, "IRDA_TXD")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(122, "DPICK"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 101),
+		MTK_FUNCTION(0, "GPIO122"),
+		MTK_FUNCTION(1, "DPI0_CK"),
+		MTK_FUNCTION(2, "EINT101"),
+		MTK_FUNCTION(3, "I2SIN_DAT"),
+		MTK_FUNCTION(5, "PCM1_DI"),
+		MTK_FUNCTION(6, "IRDA_PDN")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(123, "DPIG4"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 114),
+		MTK_FUNCTION(0, "GPIO123"),
+		MTK_FUNCTION(1, "DPI0_G4"),
+		MTK_FUNCTION(2, "EINT114"),
+		MTK_FUNCTION(4, "CM2DAT_2X[0]"),
+		MTK_FUNCTION(5, "DSP2_ID")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(124, "DPIG5"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 115),
+		MTK_FUNCTION(0, "GPIO124"),
+		MTK_FUNCTION(1, "DPI0_G5"),
+		MTK_FUNCTION(2, "EINT115"),
+		MTK_FUNCTION(4, "CM2DAT_2X[1]"),
+		MTK_FUNCTION(5, "DSP2_ICK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(125, "DPIR3"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 121),
+		MTK_FUNCTION(0, "GPIO125"),
+		MTK_FUNCTION(1, "DPI0_R3"),
+		MTK_FUNCTION(2, "EINT121"),
+		MTK_FUNCTION(4, "CM2DAT_2X[7]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(126, "DPIG1"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 111),
+		MTK_FUNCTION(0, "GPIO126"),
+		MTK_FUNCTION(1, "DPI0_G1"),
+		MTK_FUNCTION(2, "EINT111"),
+		MTK_FUNCTION(5, "DSP1_ICK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(127, "DPIVSYNC"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 98),
+		MTK_FUNCTION(0, "GPIO127"),
+		MTK_FUNCTION(1, "DPI0_VSYNC"),
+		MTK_FUNCTION(2, "EINT98"),
+		MTK_FUNCTION(3, "I2SIN_CK"),
+		MTK_FUNCTION(4, "DAC_CK"),
+		MTK_FUNCTION(5, "PCM1_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(128, "DPIHSYNC"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 99),
+		MTK_FUNCTION(0, "GPIO128"),
+		MTK_FUNCTION(1, "DPI0_HSYNC"),
+		MTK_FUNCTION(2, "EINT99"),
+		MTK_FUNCTION(3, "I2SIN_WS"),
+		MTK_FUNCTION(4, "DAC_WS"),
+		MTK_FUNCTION(5, "PCM1_WS"),
+		MTK_FUNCTION(6, "IRDA_RXD")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(129, "DPIB0"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 102),
+		MTK_FUNCTION(0, "GPIO129"),
+		MTK_FUNCTION(1, "DPI0_B0"),
+		MTK_FUNCTION(2, "EINT102"),
+		MTK_FUNCTION(4, "SCL0"),
+		MTK_FUNCTION(5, "DISP_PWM")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(130, "DPIB1"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 103),
+		MTK_FUNCTION(0, "GPIO130"),
+		MTK_FUNCTION(1, "DPI0_B1"),
+		MTK_FUNCTION(2, "EINT103"),
+		MTK_FUNCTION(3, "CLKM0"),
+		MTK_FUNCTION(4, "SDA0"),
+		MTK_FUNCTION(5, "PWM1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(131, "DPIB2"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 104),
+		MTK_FUNCTION(0, "GPIO131"),
+		MTK_FUNCTION(1, "DPI0_B2"),
+		MTK_FUNCTION(2, "EINT104"),
+		MTK_FUNCTION(3, "CLKM1"),
+		MTK_FUNCTION(4, "SCL1"),
+		MTK_FUNCTION(5, "PWM2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(132, "DPIB3"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 105),
+		MTK_FUNCTION(0, "GPIO132"),
+		MTK_FUNCTION(1, "DPI0_B3"),
+		MTK_FUNCTION(2, "EINT105"),
+		MTK_FUNCTION(3, "CLKM2"),
+		MTK_FUNCTION(4, "SDA1"),
+		MTK_FUNCTION(5, "PWM3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(133, "DPIB4"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 106),
+		MTK_FUNCTION(0, "GPIO133"),
+		MTK_FUNCTION(1, "DPI0_B4"),
+		MTK_FUNCTION(2, "EINT106"),
+		MTK_FUNCTION(3, "CLKM3"),
+		MTK_FUNCTION(4, "SCL2"),
+		MTK_FUNCTION(5, "PWM4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(134, "DPIB5"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 107),
+		MTK_FUNCTION(0, "GPIO134"),
+		MTK_FUNCTION(1, "DPI0_B5"),
+		MTK_FUNCTION(2, "EINT107"),
+		MTK_FUNCTION(3, "CLKM4"),
+		MTK_FUNCTION(4, "SDA2"),
+		MTK_FUNCTION(5, "PWM5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(135, "DPIB6"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 108),
+		MTK_FUNCTION(0, "GPIO135"),
+		MTK_FUNCTION(1, "DPI0_B6"),
+		MTK_FUNCTION(2, "EINT108"),
+		MTK_FUNCTION(3, "CLKM5"),
+		MTK_FUNCTION(4, "SCL3"),
+		MTK_FUNCTION(5, "PWM6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(136, "DPIB7"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 109),
+		MTK_FUNCTION(0, "GPIO136"),
+		MTK_FUNCTION(1, "DPI0_B7"),
+		MTK_FUNCTION(2, "EINT109"),
+		MTK_FUNCTION(3, "CLKM6"),
+		MTK_FUNCTION(4, "SDA3"),
+		MTK_FUNCTION(5, "PWM7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(137, "DPIG0"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 110),
+		MTK_FUNCTION(0, "GPIO137"),
+		MTK_FUNCTION(1, "DPI0_G0"),
+		MTK_FUNCTION(2, "EINT110"),
+		MTK_FUNCTION(5, "DSP1_ID")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(138, "DPIG2"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 112),
+		MTK_FUNCTION(0, "GPIO138"),
+		MTK_FUNCTION(1, "DPI0_G2"),
+		MTK_FUNCTION(2, "EINT112"),
+		MTK_FUNCTION(5, "DSP1_IMS")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(139, "DPIG3"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 113),
+		MTK_FUNCTION(0, "GPIO139"),
+		MTK_FUNCTION(1, "DPI0_G3"),
+		MTK_FUNCTION(2, "EINT113"),
+		MTK_FUNCTION(5, "DSP2_IMS")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(140, "DPIG6"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 116),
+		MTK_FUNCTION(0, "GPIO140"),
+		MTK_FUNCTION(1, "DPI0_G6"),
+		MTK_FUNCTION(2, "EINT116"),
+		MTK_FUNCTION(4, "CM2DAT_2X[2]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(141, "DPIG7"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 117),
+		MTK_FUNCTION(0, "GPIO141"),
+		MTK_FUNCTION(1, "DPI0_G7"),
+		MTK_FUNCTION(2, "EINT117"),
+		MTK_FUNCTION(4, "CM2DAT_2X[3]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(142, "DPIR0"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 118),
+		MTK_FUNCTION(0, "GPIO142"),
+		MTK_FUNCTION(1, "DPI0_R0"),
+		MTK_FUNCTION(2, "EINT118"),
+		MTK_FUNCTION(4, "CM2DAT_2X[4]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(143, "DPIR1"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 119),
+		MTK_FUNCTION(0, "GPIO143"),
+		MTK_FUNCTION(1, "DPI0_R1"),
+		MTK_FUNCTION(2, "EINT119"),
+		MTK_FUNCTION(4, "CM2DAT_2X[5]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(144, "DPIR2"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 120),
+		MTK_FUNCTION(0, "GPIO144"),
+		MTK_FUNCTION(1, "DPI0_R2"),
+		MTK_FUNCTION(2, "EINT120"),
+		MTK_FUNCTION(4, "CM2DAT_2X[6]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(145, "DPIR4"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 122),
+		MTK_FUNCTION(0, "GPIO145"),
+		MTK_FUNCTION(1, "DPI0_R4"),
+		MTK_FUNCTION(2, "EINT122"),
+		MTK_FUNCTION(4, "CM2DAT_2X[8]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(146, "DPIR5"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 123),
+		MTK_FUNCTION(0, "GPIO146"),
+		MTK_FUNCTION(1, "DPI0_R5"),
+		MTK_FUNCTION(2, "EINT123"),
+		MTK_FUNCTION(4, "CM2DAT_2X[9]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(147, "DPIR6"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 124),
+		MTK_FUNCTION(0, "GPIO147"),
+		MTK_FUNCTION(1, "DPI0_R6"),
+		MTK_FUNCTION(2, "EINT124"),
+		MTK_FUNCTION(4, "CM2VSYNC_2X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(148, "DPIR7"),
+		NULL, "mt8135",
+		MTK_EINT_FUNCTION(2, 125),
+		MTK_FUNCTION(0, "GPIO148"),
+		MTK_FUNCTION(1, "DPI0_R7"),
+		MTK_FUNCTION(2, "EINT125"),
+		MTK_FUNCTION(4, "CM2HSYNC_2X")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(149, "TDN3/LVDS(TDN3)"),
+		"AA2", "mt8135",
+		MTK_EINT_FUNCTION(2, 36),
+		MTK_FUNCTION(0, "GPIO149"),
+		MTK_FUNCTION(2, "EINT36")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(150, "TDP3/LVDS(TDP3)"),
+		"AA1", "mt8135",
+		MTK_EINT_FUNCTION(2, 35),
+		MTK_FUNCTION(0, "GPIO150"),
+		MTK_FUNCTION(2, "EINT35")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(151, "TDN2/LVDS(TCN)"),
+		"Y2", "mt8135",
+		MTK_EINT_FUNCTION(2, 169),
+		MTK_FUNCTION(0, "GPIO151"),
+		MTK_FUNCTION(2, "EINT169")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(152, "TDP2/LVDS(TCP)"),
+		"Y1", "mt8135",
+		MTK_EINT_FUNCTION(2, 168),
+		MTK_FUNCTION(0, "GPIO152"),
+		MTK_FUNCTION(2, "EINT168")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(153, "TCN/LVDS(TDN2)"),
+		"W2", "mt8135",
+		MTK_EINT_FUNCTION(2, 163),
+		MTK_FUNCTION(0, "GPIO153"),
+		MTK_FUNCTION(2, "EINT163")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(154, "TCP/LVDS(TDP2)"),
+		"W1", "mt8135",
+		MTK_EINT_FUNCTION(2, 162),
+		MTK_FUNCTION(0, "GPIO154"),
+		MTK_FUNCTION(2, "EINT162")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(155, "TDN1/LVDS(TDN1)"),
+		"V3", "mt8135",
+		MTK_EINT_FUNCTION(2, 167),
+		MTK_FUNCTION(0, "GPIO155"),
+		MTK_FUNCTION(2, "EINT167")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(156, "TDP1/LVDS(TDP1)"),
+		"V2", "mt8135",
+		MTK_EINT_FUNCTION(2, 166),
+		MTK_FUNCTION(0, "GPIO156"),
+		MTK_FUNCTION(2, "EINT166")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(157, "TDN0/LVDS(TDN0)"),
+		"U3", "mt8135",
+		MTK_EINT_FUNCTION(2, 165),
+		MTK_FUNCTION(0, "GPIO157"),
+		MTK_FUNCTION(2, "EINT165")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(158, "TDP0/LVDS(TDP0)"),
+		"U2", "mt8135",
+		MTK_EINT_FUNCTION(2, 164),
+		MTK_FUNCTION(0, "GPIO158"),
+		MTK_FUNCTION(2, "EINT164")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(159, "RDN3"),
+		"N5", "mt8135",
+		MTK_EINT_FUNCTION(2, 18),
+		MTK_FUNCTION(0, "GPIO159"),
+		MTK_FUNCTION(2, "EINT18")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(160, "RDP3"),
+		"N4", "mt8135",
+		MTK_EINT_FUNCTION(2, 30),
+		MTK_FUNCTION(0, "GPIO160"),
+		MTK_FUNCTION(2, "EINT30")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(161, "RDN2"),
+		"T2", "mt8135",
+		MTK_EINT_FUNCTION(2, 31),
+		MTK_FUNCTION(0, "GPIO161"),
+		MTK_FUNCTION(2, "EINT31")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(162, "RDP2"),
+		"T3", "mt8135",
+		MTK_EINT_FUNCTION(2, 32),
+		MTK_FUNCTION(0, "GPIO162"),
+		MTK_FUNCTION(2, "EINT32")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(163, "RCN"),
+		"P2", "mt8135",
+		MTK_EINT_FUNCTION(2, 33),
+		MTK_FUNCTION(0, "GPIO163"),
+		MTK_FUNCTION(2, "EINT33")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(164, "RCP"),
+		"P3", "mt8135",
+		MTK_EINT_FUNCTION(2, 39),
+		MTK_FUNCTION(0, "GPIO164"),
+		MTK_FUNCTION(2, "EINT39")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(165, "RDN1"),
+		"R3", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO165")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(166, "RDP1"),
+		"R2", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO166")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(167, "RDN0"),
+		"N3", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO167")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(168, "RDP0"),
+		"N2", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO168")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(169, "RDN1_A"),
+		"M4", "mt8135",
+		MTK_EINT_FUNCTION(2, 175),
+		MTK_FUNCTION(0, "GPIO169"),
+		MTK_FUNCTION(1, "CMDAT6"),
+		MTK_FUNCTION(2, "EINT175")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(170, "RDP1_A"),
+		"M3", "mt8135",
+		MTK_EINT_FUNCTION(2, 174),
+		MTK_FUNCTION(0, "GPIO170"),
+		MTK_FUNCTION(1, "CMDAT7"),
+		MTK_FUNCTION(2, "EINT174")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(171, "RCN_A"),
+		"L3", "mt8135",
+		MTK_EINT_FUNCTION(2, 171),
+		MTK_FUNCTION(0, "GPIO171"),
+		MTK_FUNCTION(1, "CMDAT8"),
+		MTK_FUNCTION(2, "EINT171")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(172, "RCP_A"),
+		"L2", "mt8135",
+		MTK_EINT_FUNCTION(2, 170),
+		MTK_FUNCTION(0, "GPIO172"),
+		MTK_FUNCTION(1, "CMDAT9"),
+		MTK_FUNCTION(2, "EINT170")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(173, "RDN0_A"),
+		"M2", "mt8135",
+		MTK_EINT_FUNCTION(2, 173),
+		MTK_FUNCTION(0, "GPIO173"),
+		MTK_FUNCTION(1, "CMHSYNC"),
+		MTK_FUNCTION(2, "EINT173")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(174, "RDP0_A"),
+		"M1", "mt8135",
+		MTK_EINT_FUNCTION(2, 172),
+		MTK_FUNCTION(0, "GPIO174"),
+		MTK_FUNCTION(1, "CMVSYNC"),
+		MTK_FUNCTION(2, "EINT172")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(175, "RDN1_B"),
+		"H2", "mt8135",
+		MTK_EINT_FUNCTION(2, 181),
+		MTK_FUNCTION(0, "GPIO175"),
+		MTK_FUNCTION(1, "CMDAT2"),
+		MTK_FUNCTION(2, "EINT181"),
+		MTK_FUNCTION(3, "CMCSD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(176, "RDP1_B"),
+		"H1", "mt8135",
+		MTK_EINT_FUNCTION(2, 180),
+		MTK_FUNCTION(0, "GPIO176"),
+		MTK_FUNCTION(1, "CMDAT3"),
+		MTK_FUNCTION(2, "EINT180"),
+		MTK_FUNCTION(3, "CMCSD3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(177, "RCN_B"),
+		"K3", "mt8135",
+		MTK_EINT_FUNCTION(2, 177),
+		MTK_FUNCTION(0, "GPIO177"),
+		MTK_FUNCTION(1, "CMDAT4"),
+		MTK_FUNCTION(2, "EINT177")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(178, "RCP_B"),
+		"K2", "mt8135",
+		MTK_EINT_FUNCTION(2, 176),
+		MTK_FUNCTION(0, "GPIO178"),
+		MTK_FUNCTION(1, "CMDAT5"),
+		MTK_FUNCTION(2, "EINT176")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(179, "RDN0_B"),
+		"J3", "mt8135",
+		MTK_EINT_FUNCTION(2, 179),
+		MTK_FUNCTION(0, "GPIO179"),
+		MTK_FUNCTION(1, "CMDAT0"),
+		MTK_FUNCTION(2, "EINT179"),
+		MTK_FUNCTION(3, "CMCSD0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(180, "RDP0_B"),
+		"J2", "mt8135",
+		MTK_EINT_FUNCTION(2, 178),
+		MTK_FUNCTION(0, "GPIO180"),
+		MTK_FUNCTION(1, "CMDAT1"),
+		MTK_FUNCTION(2, "EINT178"),
+		MTK_FUNCTION(3, "CMCSD1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(181, "CMPCLK"),
+		"K4", "mt8135",
+		MTK_EINT_FUNCTION(2, 182),
+		MTK_FUNCTION(0, "GPIO181"),
+		MTK_FUNCTION(1, "CMPCLK"),
+		MTK_FUNCTION(2, "EINT182"),
+		MTK_FUNCTION(3, "CMCSK"),
+		MTK_FUNCTION(4, "CM2MCLK_4X"),
+		MTK_FUNCTION(5, "TS_AUXADC_SEL[3]"),
+		MTK_FUNCTION(6, "VENC_TEST_CK"),
+		MTK_FUNCTION(7, "TESTA_OUT27")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(182, "CMMCLK"),
+		"J5", "mt8135",
+		MTK_EINT_FUNCTION(2, 183),
+		MTK_FUNCTION(0, "GPIO182"),
+		MTK_FUNCTION(1, "CMMCLK"),
+		MTK_FUNCTION(2, "EINT183"),
+		MTK_FUNCTION(5, "TS_AUXADC_SEL[2]"),
+		MTK_FUNCTION(7, "TESTA_OUT28")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(183, "CMRST"),
+		"J6", "mt8135",
+		MTK_EINT_FUNCTION(2, 185),
+		MTK_FUNCTION(0, "GPIO183"),
+		MTK_FUNCTION(1, "CMRST"),
+		MTK_FUNCTION(2, "EINT185"),
+		MTK_FUNCTION(5, "TS_AUXADC_SEL[1]"),
+		MTK_FUNCTION(7, "TESTA_OUT30")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(184, "CMPDN"),
+		"J4", "mt8135",
+		MTK_EINT_FUNCTION(2, 184),
+		MTK_FUNCTION(0, "GPIO184"),
+		MTK_FUNCTION(1, "CMPDN"),
+		MTK_FUNCTION(2, "EINT184"),
+		MTK_FUNCTION(5, "TS_AUXADC_SEL[0]"),
+		MTK_FUNCTION(7, "TESTA_OUT29")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(185, "CMFLASH"),
+		"G4", "mt8135",
+		MTK_EINT_FUNCTION(2, 186),
+		MTK_FUNCTION(0, "GPIO185"),
+		MTK_FUNCTION(1, "CMFLASH"),
+		MTK_FUNCTION(2, "EINT186"),
+		MTK_FUNCTION(3, "CM2MCLK_3X"),
+		MTK_FUNCTION(6, "MFG_TEST_CK_1"),
+		MTK_FUNCTION(7, "TESTA_OUT31")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(186, "MRG_I2S_PCM_CLK"),
+		"F5", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO186"),
+		MTK_FUNCTION(1, "MRG_I2S_PCM_CLK"),
+		MTK_FUNCTION(3, "I2SIN_CK"),
+		MTK_FUNCTION(4, "PCM0_CK"),
+		MTK_FUNCTION(5, "DSP2_ICK"),
+		MTK_FUNCTION(6, "IMG_TEST_CK"),
+		MTK_FUNCTION(7, "USB_SCL")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(187, "MRG_I2S_PCM_SYNC"),
+		"G6", "mt8135",
+		MTK_EINT_FUNCTION(2, 16),
+		MTK_FUNCTION(0, "GPIO187"),
+		MTK_FUNCTION(1, "MRG_I2S_PCM_SYNC"),
+		MTK_FUNCTION(2, "EINT16"),
+		MTK_FUNCTION(3, "I2SIN_WS"),
+		MTK_FUNCTION(4, "PCM0_WS"),
+		MTK_FUNCTION(6, "DISP_TEST_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(188, "MRG_I2S_PCM_RX"),
+		"G3", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO188"),
+		MTK_FUNCTION(1, "MRG_I2S_PCM_RX"),
+		MTK_FUNCTION(3, "I2SIN_DAT"),
+		MTK_FUNCTION(4, "PCM0_DI"),
+		MTK_FUNCTION(5, "DSP2_ID"),
+		MTK_FUNCTION(6, "MFG_TEST_CK"),
+		MTK_FUNCTION(7, "USB_SDA")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(189, "MRG_I2S_PCM_TX"),
+		"G5", "mt8135",
+		MTK_EINT_FUNCTION(2, 17),
+		MTK_FUNCTION(0, "GPIO189"),
+		MTK_FUNCTION(1, "MRG_I2S_PCM_TX"),
+		MTK_FUNCTION(2, "EINT17"),
+		MTK_FUNCTION(3, "I2SOUT_DAT"),
+		MTK_FUNCTION(4, "PCM0_DO"),
+		MTK_FUNCTION(6, "VDEC_TEST_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(190, "SRCLKENAI"),
+		"K5", "mt8135",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO190"),
+		MTK_FUNCTION(1, "SRCLKENAI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(191, "URXD3"),
+		"C3", "mt8135",
+		MTK_EINT_FUNCTION(2, 87),
+		MTK_FUNCTION(0, "GPIO191"),
+		MTK_FUNCTION(1, "URXD3"),
+		MTK_FUNCTION(2, "EINT87"),
+		MTK_FUNCTION(3, "UTXD3"),
+		MTK_FUNCTION(5, "TS_AUX_ST"),
+		MTK_FUNCTION(6, "PWM4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(192, "UTXD3"),
+		"B2", "mt8135",
+		MTK_EINT_FUNCTION(2, 86),
+		MTK_FUNCTION(0, "GPIO192"),
+		MTK_FUNCTION(1, "UTXD3"),
+		MTK_FUNCTION(2, "EINT86"),
+		MTK_FUNCTION(3, "URXD3"),
+		MTK_FUNCTION(5, "TS_AUX_CS_B"),
+		MTK_FUNCTION(6, "PWM3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(193, "SDA2"),
+		"G2", "mt8135",
+		MTK_EINT_FUNCTION(2, 95),
+		MTK_FUNCTION(0, "GPIO193"),
+		MTK_FUNCTION(1, "SDA2"),
+		MTK_FUNCTION(2, "EINT95"),
+		MTK_FUNCTION(3, "CLKM5"),
+		MTK_FUNCTION(4, "PWM5"),
+		MTK_FUNCTION(5, "TS_AUX_PWDB")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(194, "SCL2"),
+		"F4", "mt8135",
+		MTK_EINT_FUNCTION(2, 94),
+		MTK_FUNCTION(0, "GPIO194"),
+		MTK_FUNCTION(1, "SCL2"),
+		MTK_FUNCTION(2, "EINT94"),
+		MTK_FUNCTION(3, "CLKM4"),
+		MTK_FUNCTION(4, "PWM4"),
+		MTK_FUNCTION(5, "TS_AUXADC_TEST_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(195, "SDA1"),
+		"F2", "mt8135",
+		MTK_EINT_FUNCTION(2, 93),
+		MTK_FUNCTION(0, "GPIO195"),
+		MTK_FUNCTION(1, "SDA1"),
+		MTK_FUNCTION(2, "EINT93"),
+		MTK_FUNCTION(3, "CLKM3"),
+		MTK_FUNCTION(4, "PWM3"),
+		MTK_FUNCTION(5, "TS_AUX_SCLK_PWDB")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(196, "SCL1"),
+		"F3", "mt8135",
+		MTK_EINT_FUNCTION(2, 92),
+		MTK_FUNCTION(0, "GPIO196"),
+		MTK_FUNCTION(1, "SCL1"),
+		MTK_FUNCTION(2, "EINT92"),
+		MTK_FUNCTION(3, "CLKM2"),
+		MTK_FUNCTION(4, "PWM2"),
+		MTK_FUNCTION(5, "TS_AUX_DIN")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(197, "MSDC3_DAT2"),
+		"E1", "mt8135",
+		MTK_EINT_FUNCTION(2, 71),
+		MTK_FUNCTION(0, "GPIO197"),
+		MTK_FUNCTION(1, "MSDC3_DAT2"),
+		MTK_FUNCTION(2, "EINT71"),
+		MTK_FUNCTION(3, "SCL6"),
+		MTK_FUNCTION(4, "PWM5"),
+		MTK_FUNCTION(5, "CLKM4"),
+		MTK_FUNCTION(6, "MFG_TEST_CK_2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(198, "MSDC3_DAT3"),
+		"C2", "mt8135",
+		MTK_EINT_FUNCTION(2, 72),
+		MTK_FUNCTION(0, "GPIO198"),
+		MTK_FUNCTION(1, "MSDC3_DAT3"),
+		MTK_FUNCTION(2, "EINT72"),
+		MTK_FUNCTION(3, "SDA6"),
+		MTK_FUNCTION(4, "PWM6"),
+		MTK_FUNCTION(5, "CLKM5"),
+		MTK_FUNCTION(6, "MFG_TEST_CK_3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(199, "MSDC3_CMD"),
+		"D2", "mt8135",
+		MTK_EINT_FUNCTION(2, 68),
+		MTK_FUNCTION(0, "GPIO199"),
+		MTK_FUNCTION(1, "MSDC3_CMD"),
+		MTK_FUNCTION(2, "EINT68"),
+		MTK_FUNCTION(3, "SDA2"),
+		MTK_FUNCTION(4, "PWM2"),
+		MTK_FUNCTION(5, "CLKM1"),
+		MTK_FUNCTION(6, "MFG_TEST_CK_4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(200, "MSDC3_CLK"),
+		"E2", "mt8135",
+		MTK_EINT_FUNCTION(2, 67),
+		MTK_FUNCTION(0, "GPIO200"),
+		MTK_FUNCTION(1, "MSDC3_CLK"),
+		MTK_FUNCTION(2, "EINT67"),
+		MTK_FUNCTION(3, "SCL2"),
+		MTK_FUNCTION(4, "PWM1"),
+		MTK_FUNCTION(5, "CLKM0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(201, "MSDC3_DAT1"),
+		"D3", "mt8135",
+		MTK_EINT_FUNCTION(2, 70),
+		MTK_FUNCTION(0, "GPIO201"),
+		MTK_FUNCTION(1, "MSDC3_DAT1"),
+		MTK_FUNCTION(2, "EINT70"),
+		MTK_FUNCTION(3, "SDA3"),
+		MTK_FUNCTION(4, "PWM4"),
+		MTK_FUNCTION(5, "CLKM3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(202, "MSDC3_DAT0"),
+		"E3", "mt8135",
+		MTK_EINT_FUNCTION(2, 69),
+		MTK_FUNCTION(0, "GPIO202"),
+		MTK_FUNCTION(1, "MSDC3_DAT0"),
+		MTK_FUNCTION(2, "EINT69"),
+		MTK_FUNCTION(3, "SCL3"),
+		MTK_FUNCTION(4, "PWM3"),
+		MTK_FUNCTION(5, "CLKM2")
+	),
+};
+
+#endif /* __PINCTRL_MTK_MT8135_H */
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-mt8173.h b/drivers/pinctrl/mediatek/pinctrl-mtk-mt8173.h
new file mode 100644
index 000000000000..13e5b68bfe1b
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-mt8173.h
@@ -0,0 +1,1226 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __PINCTRL_MTK_MT8173_H
+#define __PINCTRL_MTK_MT8173_H
+
+#include <linux/pinctrl/pinctrl.h>
+#include "pinctrl-mtk-common.h"
+
+static const struct mtk_desc_pin mtk_pins_mt8173[] = {
+	MTK_PIN(
+		PINCTRL_PIN(0, "EINT0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 0),
+		MTK_FUNCTION(0, "GPIO0"),
+		MTK_FUNCTION(1, "IRDA_PDN"),
+		MTK_FUNCTION(2, "I2S1_WS"),
+		MTK_FUNCTION(3, "AUD_SPDIF"),
+		MTK_FUNCTION(4, "UTXD0"),
+		MTK_FUNCTION(7, "DBG_MON_A_20_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(1, "EINT1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 1),
+		MTK_FUNCTION(0, "GPIO1"),
+		MTK_FUNCTION(1, "IRDA_RXD"),
+		MTK_FUNCTION(2, "I2S1_BCK"),
+		MTK_FUNCTION(3, "SDA5"),
+		MTK_FUNCTION(4, "URXD0"),
+		MTK_FUNCTION(7, "DBG_MON_A_21_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(2, "EINT2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 2),
+		MTK_FUNCTION(0, "GPIO2"),
+		MTK_FUNCTION(1, "IRDA_TXD"),
+		MTK_FUNCTION(2, "I2S1_MCK"),
+		MTK_FUNCTION(3, "SCL5"),
+		MTK_FUNCTION(4, "UTXD3"),
+		MTK_FUNCTION(7, "DBG_MON_A_22_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(3, "EINT3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 3),
+		MTK_FUNCTION(0, "GPIO3"),
+		MTK_FUNCTION(1, "DSI1_TE"),
+		MTK_FUNCTION(2, "I2S1_DO_1"),
+		MTK_FUNCTION(3, "SDA3"),
+		MTK_FUNCTION(4, "URXD3"),
+		MTK_FUNCTION(7, "DBG_MON_A_23_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(4, "EINT4"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 4),
+		MTK_FUNCTION(0, "GPIO4"),
+		MTK_FUNCTION(1, "DISP_PWM1"),
+		MTK_FUNCTION(2, "I2S1_DO_2"),
+		MTK_FUNCTION(3, "SCL3"),
+		MTK_FUNCTION(4, "UCTS3"),
+		MTK_FUNCTION(6, "SFWP_B")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(5, "EINT5"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 5),
+		MTK_FUNCTION(0, "GPIO5"),
+		MTK_FUNCTION(1, "PCM1_CLK"),
+		MTK_FUNCTION(2, "I2S2_WS"),
+		MTK_FUNCTION(3, "SPI_CK_3_"),
+		MTK_FUNCTION(4, "URTS3"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TMS"),
+		MTK_FUNCTION(6, "SFOUT")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(6, "EINT6"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 6),
+		MTK_FUNCTION(0, "GPIO6"),
+		MTK_FUNCTION(1, "PCM1_SYNC"),
+		MTK_FUNCTION(2, "I2S2_BCK"),
+		MTK_FUNCTION(3, "SPI_MI_3_"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TCK"),
+		MTK_FUNCTION(6, "SFCS0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(7, "EINT7"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 7),
+		MTK_FUNCTION(0, "GPIO7"),
+		MTK_FUNCTION(1, "PCM1_DI"),
+		MTK_FUNCTION(2, "I2S2_DI_1"),
+		MTK_FUNCTION(3, "SPI_MO_3_"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TDI"),
+		MTK_FUNCTION(6, "SFHOLD")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(8, "EINT8"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 8),
+		MTK_FUNCTION(0, "GPIO8"),
+		MTK_FUNCTION(1, "PCM1_DO"),
+		MTK_FUNCTION(2, "I2S2_DI_2"),
+		MTK_FUNCTION(3, "SPI_CS_3_"),
+		MTK_FUNCTION(4, "AUD_SPDIF"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TDO"),
+		MTK_FUNCTION(6, "SFIN")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(9, "EINT9"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 9),
+		MTK_FUNCTION(0, "GPIO9"),
+		MTK_FUNCTION(1, "USB_DRVVBUS_P0"),
+		MTK_FUNCTION(2, "I2S2_MCK"),
+		MTK_FUNCTION(4, "USB_DRVVBUS_P1"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TRST"),
+		MTK_FUNCTION(6, "SFCK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(10, "EINT10"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 10),
+		MTK_FUNCTION(0, "GPIO10"),
+		MTK_FUNCTION(1, "CLKM0"),
+		MTK_FUNCTION(2, "DSI1_TE"),
+		MTK_FUNCTION(3, "DISP_PWM1"),
+		MTK_FUNCTION(4, "PWM4"),
+		MTK_FUNCTION(5, "IRDA_RXD")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(11, "EINT11"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 11),
+		MTK_FUNCTION(0, "GPIO11"),
+		MTK_FUNCTION(1, "CLKM1"),
+		MTK_FUNCTION(2, "I2S3_WS"),
+		MTK_FUNCTION(3, "USB_DRVVBUS_P0"),
+		MTK_FUNCTION(4, "PWM5"),
+		MTK_FUNCTION(5, "IRDA_TXD"),
+		MTK_FUNCTION(6, "USB_DRVVBUS_P1"),
+		MTK_FUNCTION(7, "DBG_MON_B_30_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(12, "EINT12"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 12),
+		MTK_FUNCTION(0, "GPIO12"),
+		MTK_FUNCTION(1, "CLKM2"),
+		MTK_FUNCTION(2, "I2S3_BCK"),
+		MTK_FUNCTION(3, "SRCLKENA0"),
+		MTK_FUNCTION(5, "I2S2_WS"),
+		MTK_FUNCTION(7, "DBG_MON_B_32_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(13, "EINT13"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 13),
+		MTK_FUNCTION(0, "GPIO13"),
+		MTK_FUNCTION(1, "CLKM3"),
+		MTK_FUNCTION(2, "I2S3_MCK"),
+		MTK_FUNCTION(3, "SRCLKENA0"),
+		MTK_FUNCTION(5, "I2S2_BCK"),
+		MTK_FUNCTION(7, "DBG_MON_A_32_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(14, "EINT14"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 14),
+		MTK_FUNCTION(0, "GPIO14"),
+		MTK_FUNCTION(1, "CMDAT0"),
+		MTK_FUNCTION(2, "CMCSD0"),
+		MTK_FUNCTION(4, "CLKM2"),
+		MTK_FUNCTION(7, "DBG_MON_B_6_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(15, "EINT15"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 15),
+		MTK_FUNCTION(0, "GPIO15"),
+		MTK_FUNCTION(1, "CMDAT1"),
+		MTK_FUNCTION(2, "CMCSD1"),
+		MTK_FUNCTION(3, "CMFLASH"),
+		MTK_FUNCTION(4, "CLKM3"),
+		MTK_FUNCTION(7, "DBG_MON_B_29_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(16, "IDDIG"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 16),
+		MTK_FUNCTION(0, "GPIO16"),
+		MTK_FUNCTION(1, "IDDIG"),
+		MTK_FUNCTION(2, "CMFLASH"),
+		MTK_FUNCTION(4, "PWM5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(17, "WATCHDOG"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 17),
+		MTK_FUNCTION(0, "GPIO17"),
+		MTK_FUNCTION(1, "WATCHDOG_AO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(18, "CEC"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 18),
+		MTK_FUNCTION(0, "GPIO18"),
+		MTK_FUNCTION(1, "CEC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(19, "HDMISCK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 19),
+		MTK_FUNCTION(0, "GPIO19"),
+		MTK_FUNCTION(1, "HDMISCK"),
+		MTK_FUNCTION(2, "HDCP_SCL")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(20, "HDMISD"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 20),
+		MTK_FUNCTION(0, "GPIO20"),
+		MTK_FUNCTION(1, "HDMISD"),
+		MTK_FUNCTION(2, "HDCP_SDA")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(21, "HTPLG"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 21),
+		MTK_FUNCTION(0, "GPIO21"),
+		MTK_FUNCTION(1, "HTPLG")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(22, "MSDC3_DAT0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 22),
+		MTK_FUNCTION(0, "GPIO22"),
+		MTK_FUNCTION(1, "MSDC3_DAT0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(23, "MSDC3_DAT1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 23),
+		MTK_FUNCTION(0, "GPIO23"),
+		MTK_FUNCTION(1, "MSDC3_DAT1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(24, "MSDC3_DAT2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 24),
+		MTK_FUNCTION(0, "GPIO24"),
+		MTK_FUNCTION(1, "MSDC3_DAT2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(25, "MSDC3_DAT3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 25),
+		MTK_FUNCTION(0, "GPIO25"),
+		MTK_FUNCTION(1, "MSDC3_DAT3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(26, "MSDC3_CLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 26),
+		MTK_FUNCTION(0, "GPIO26"),
+		MTK_FUNCTION(1, "MSDC3_CLK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(27, "MSDC3_CMD"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 27),
+		MTK_FUNCTION(0, "GPIO27"),
+		MTK_FUNCTION(1, "MSDC3_CMD")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(28, "MSDC3_DSL"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 28),
+		MTK_FUNCTION(0, "GPIO28"),
+		MTK_FUNCTION(1, "MSDC3_DSL")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(29, "UCTS2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 29),
+		MTK_FUNCTION(0, "GPIO29"),
+		MTK_FUNCTION(1, "UCTS2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(30, "URTS2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 30),
+		MTK_FUNCTION(0, "GPIO30"),
+		MTK_FUNCTION(1, "URTS2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(31, "URXD2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 31),
+		MTK_FUNCTION(0, "GPIO31"),
+		MTK_FUNCTION(1, "URXD2"),
+		MTK_FUNCTION(2, "UTXD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(32, "UTXD2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 32),
+		MTK_FUNCTION(0, "GPIO32"),
+		MTK_FUNCTION(1, "UTXD2"),
+		MTK_FUNCTION(2, "URXD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(33, "DAICLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 33),
+		MTK_FUNCTION(0, "GPIO33"),
+		MTK_FUNCTION(1, " MRG_CLK"),
+		MTK_FUNCTION(2, "PCM0_CLK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(34, "DAIPCMIN"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 34),
+		MTK_FUNCTION(0, "GPIO34"),
+		MTK_FUNCTION(1, " MRG_DI"),
+		MTK_FUNCTION(2, "PCM0_DI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(35, "DAIPCMOUT"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 35),
+		MTK_FUNCTION(0, "GPIO35"),
+		MTK_FUNCTION(1, " MRG_DO"),
+		MTK_FUNCTION(2, "PCM0_DO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(36, "DAISYNC"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 36),
+		MTK_FUNCTION(0, "GPIO36"),
+		MTK_FUNCTION(1, " MRG_SYNC"),
+		MTK_FUNCTION(2, "PCM0_SYNC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(37, "EINT16"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 37),
+		MTK_FUNCTION(0, "GPIO37"),
+		MTK_FUNCTION(1, "USB_DRVVBUS_P0"),
+		MTK_FUNCTION(2, "USB_DRVVBUS_P1"),
+		MTK_FUNCTION(3, "PWM0"),
+		MTK_FUNCTION(4, "PWM1"),
+		MTK_FUNCTION(5, "PWM2"),
+		MTK_FUNCTION(6, "CLKM0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(38, "CONN_RST"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 38),
+		MTK_FUNCTION(0, "GPIO38"),
+		MTK_FUNCTION(1, "USB_DRVVBUS_P0"),
+		MTK_FUNCTION(2, "USB_DRVVBUS_P1"),
+		MTK_FUNCTION(6, "CLKM1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(39, "CM2MCLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 39),
+		MTK_FUNCTION(0, "GPIO39"),
+		MTK_FUNCTION(1, "CM2MCLK"),
+		MTK_FUNCTION(2, "CMCSD0"),
+		MTK_FUNCTION(7, "DBG_MON_A_17_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(40, "CMPCLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 40),
+		MTK_FUNCTION(0, "GPIO40"),
+		MTK_FUNCTION(1, "CMPCLK"),
+		MTK_FUNCTION(2, "CMCSK"),
+		MTK_FUNCTION(3, "CMCSD2"),
+		MTK_FUNCTION(7, "DBG_MON_A_18_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(41, "CMMCLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 41),
+		MTK_FUNCTION(0, "GPIO41"),
+		MTK_FUNCTION(1, "CMMCLK"),
+		MTK_FUNCTION(7, "DBG_MON_A_19_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(42, "DSI_TE"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 42),
+		MTK_FUNCTION(0, "GPIO42"),
+		MTK_FUNCTION(1, "DSI_TE")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(43, "SDA2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 43),
+		MTK_FUNCTION(0, "GPIO43"),
+		MTK_FUNCTION(1, "SDA2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(44, "SCL2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 44),
+		MTK_FUNCTION(0, "GPIO44"),
+		MTK_FUNCTION(1, "SCL2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(45, "SDA0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 45),
+		MTK_FUNCTION(0, "GPIO45"),
+		MTK_FUNCTION(1, "SDA0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(46, "SCL0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 46),
+		MTK_FUNCTION(0, "GPIO46"),
+		MTK_FUNCTION(1, "SCL0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(47, "RDN0_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 47),
+		MTK_FUNCTION(0, "GPIO47"),
+		MTK_FUNCTION(1, "CMDAT2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(48, "RDP0_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 48),
+		MTK_FUNCTION(0, "GPIO48"),
+		MTK_FUNCTION(1, "CMDAT3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(49, "RDN1_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 49),
+		MTK_FUNCTION(0, "GPIO49"),
+		MTK_FUNCTION(1, "CMDAT4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(50, "RDP1_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 50),
+		MTK_FUNCTION(0, "GPIO50"),
+		MTK_FUNCTION(1, "CMDAT5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(51, "RCN_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 51),
+		MTK_FUNCTION(0, "GPIO51"),
+		MTK_FUNCTION(1, "CMDAT6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(52, "RCP_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 52),
+		MTK_FUNCTION(0, "GPIO52"),
+		MTK_FUNCTION(1, "CMDAT7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(53, "RDN2_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 53),
+		MTK_FUNCTION(0, "GPIO53"),
+		MTK_FUNCTION(1, "CMDAT8"),
+		MTK_FUNCTION(2, "CMCSD3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(54, "RDP2_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 54),
+		MTK_FUNCTION(0, "GPIO54"),
+		MTK_FUNCTION(1, "CMDAT9"),
+		MTK_FUNCTION(2, "CMCSD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(55, "RDN3_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 55),
+		MTK_FUNCTION(0, "GPIO55"),
+		MTK_FUNCTION(1, "CMHSYNC"),
+		MTK_FUNCTION(2, "CMCSD1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(56, "RDP3_A"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 56),
+		MTK_FUNCTION(0, "GPIO56"),
+		MTK_FUNCTION(1, "CMVSYNC"),
+		MTK_FUNCTION(2, "CMCSD0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(57, "MSDC0_DAT0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 57),
+		MTK_FUNCTION(0, "GPIO57"),
+		MTK_FUNCTION(1, "MSDC0_DAT0"),
+		MTK_FUNCTION(2, "I2S1_WS"),
+		MTK_FUNCTION(7, "DBG_MON_B_7_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(58, "MSDC0_DAT1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 58),
+		MTK_FUNCTION(0, "GPIO58"),
+		MTK_FUNCTION(1, "MSDC0_DAT1"),
+		MTK_FUNCTION(2, "I2S1_BCK"),
+		MTK_FUNCTION(7, "DBG_MON_B_8_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(59, "MSDC0_DAT2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 59),
+		MTK_FUNCTION(0, "GPIO59"),
+		MTK_FUNCTION(1, "MSDC0_DAT2"),
+		MTK_FUNCTION(2, "I2S1_MCK"),
+		MTK_FUNCTION(7, "DBG_MON_B_9_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(60, "MSDC0_DAT3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 60),
+		MTK_FUNCTION(0, "GPIO60"),
+		MTK_FUNCTION(1, "MSDC0_DAT3"),
+		MTK_FUNCTION(2, "I2S1_DO_1"),
+		MTK_FUNCTION(7, "DBG_MON_B_10_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(61, "MSDC0_DAT4"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 61),
+		MTK_FUNCTION(0, "GPIO61"),
+		MTK_FUNCTION(1, "MSDC0_DAT4"),
+		MTK_FUNCTION(2, "I2S1_DO_2"),
+		MTK_FUNCTION(7, "DBG_MON_B_11_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(62, "MSDC0_DAT5"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 62),
+		MTK_FUNCTION(0, "GPIO62"),
+		MTK_FUNCTION(1, "MSDC0_DAT5"),
+		MTK_FUNCTION(2, "I2S2_WS"),
+		MTK_FUNCTION(7, "DBG_MON_B_12_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(63, "MSDC0_DAT6"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 63),
+		MTK_FUNCTION(0, "GPIO63"),
+		MTK_FUNCTION(1, "MSDC0_DAT6"),
+		MTK_FUNCTION(2, "I2S2_BCK"),
+		MTK_FUNCTION(7, "DBG_MON_B_13_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(64, "MSDC0_DAT7"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 64),
+		MTK_FUNCTION(0, "GPIO64"),
+		MTK_FUNCTION(1, "MSDC0_DAT7"),
+		MTK_FUNCTION(2, "I2S2_DI_1"),
+		MTK_FUNCTION(7, "DBG_MON_B_14_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(65, "MSDC0_CLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 65),
+		MTK_FUNCTION(0, "GPIO65"),
+		MTK_FUNCTION(1, "MSDC0_CLK"),
+		MTK_FUNCTION(7, "DBG_MON_B_16_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(66, "MSDC0_CMD"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 66),
+		MTK_FUNCTION(0, "GPIO66"),
+		MTK_FUNCTION(1, "MSDC0_CMD"),
+		MTK_FUNCTION(2, "I2S2_DI_2"),
+		MTK_FUNCTION(7, "DBG_MON_B_15_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(67, "MSDC0_DSL"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 67),
+		MTK_FUNCTION(0, "GPIO67"),
+		MTK_FUNCTION(1, "MSDC0_DSL"),
+		MTK_FUNCTION(7, "DBG_MON_B_17_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(68, "MSDC0_RST_"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 68),
+		MTK_FUNCTION(0, "GPIO68"),
+		MTK_FUNCTION(1, "MSDC0_RSTB"),
+		MTK_FUNCTION(2, "I2S2_MCK"),
+		MTK_FUNCTION(7, "DBG_MON_B_18_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(69, "SPI_CK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 69),
+		MTK_FUNCTION(0, "GPIO69"),
+		MTK_FUNCTION(1, "SPI_CK_0_"),
+		MTK_FUNCTION(2, "I2S3_DO_1"),
+		MTK_FUNCTION(3, "PWM0"),
+		MTK_FUNCTION(4, "PWM5"),
+		MTK_FUNCTION(5, "I2S2_MCK"),
+		MTK_FUNCTION(7, "DBG_MON_B_19_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(70, "SPI_MI"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 70),
+		MTK_FUNCTION(0, "GPIO70"),
+		MTK_FUNCTION(1, "SPI_MI_0_"),
+		MTK_FUNCTION(2, "I2S3_DO_2"),
+		MTK_FUNCTION(3, "PWM1"),
+		MTK_FUNCTION(4, "SPI_MO_0_"),
+		MTK_FUNCTION(5, "I2S2_DI_1"),
+		MTK_FUNCTION(6, "DSI1_TE"),
+		MTK_FUNCTION(7, "DBG_MON_B_20_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(71, "SPI_MO"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 71),
+		MTK_FUNCTION(0, "GPIO71"),
+		MTK_FUNCTION(1, "SPI_MO_0_"),
+		MTK_FUNCTION(2, "I2S3_DO_3"),
+		MTK_FUNCTION(3, "PWM2"),
+		MTK_FUNCTION(4, "SPI_MI_0_"),
+		MTK_FUNCTION(5, "I2S2_DI_2"),
+		MTK_FUNCTION(7, "DBG_MON_B_21_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(72, "SPI_CS"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 72),
+		MTK_FUNCTION(0, "GPIO72"),
+		MTK_FUNCTION(1, "SPI_CS_0_"),
+		MTK_FUNCTION(2, "I2S3_DO_4"),
+		MTK_FUNCTION(3, "PWM3"),
+		MTK_FUNCTION(4, "PWM6"),
+		MTK_FUNCTION(5, "DISP_PWM1"),
+		MTK_FUNCTION(7, "DBG_MON_B_22_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(73, "MSDC1_DAT0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 73),
+		MTK_FUNCTION(0, "GPIO73"),
+		MTK_FUNCTION(1, "MSDC1_DAT0"),
+		MTK_FUNCTION(7, "DBG_MON_B_24_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(74, "MSDC1_DAT1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 74),
+		MTK_FUNCTION(0, "GPIO74"),
+		MTK_FUNCTION(1, "MSDC1_DAT1"),
+		MTK_FUNCTION(7, "DBG_MON_B_25_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(75, "MSDC1_DAT2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 75),
+		MTK_FUNCTION(0, "GPIO75"),
+		MTK_FUNCTION(1, "MSDC1_DAT2"),
+		MTK_FUNCTION(7, "DBG_MON_B_26_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(76, "MSDC1_DAT3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 76),
+		MTK_FUNCTION(0, "GPIO76"),
+		MTK_FUNCTION(1, "MSDC1_DAT3"),
+		MTK_FUNCTION(7, "DBG_MON_B_27_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(77, "MSDC1_CLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 77),
+		MTK_FUNCTION(0, "GPIO77"),
+		MTK_FUNCTION(1, "MSDC1_CLK"),
+		MTK_FUNCTION(7, "DBG_MON_B_28_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(78, "MSDC1_CMD"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 78),
+		MTK_FUNCTION(0, "GPIO78"),
+		MTK_FUNCTION(1, "MSDC1_CMD"),
+		MTK_FUNCTION(7, "DBG_MON_B_23_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(79, "PWRAP_SPI0_MI"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 79),
+		MTK_FUNCTION(0, "GPIO79"),
+		MTK_FUNCTION(1, "PWRAP_SPIMI"),
+		MTK_FUNCTION(2, "PWRAP_SPIMO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(80, "PWRAP_SPI0_MO"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 80),
+		MTK_FUNCTION(0, "GPIO80"),
+		MTK_FUNCTION(1, "PWRAP_SPIMO"),
+		MTK_FUNCTION(2, "PWRAP_SPIMI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(81, "PWRAP_SPI0_CK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 81),
+		MTK_FUNCTION(0, "GPIO81"),
+		MTK_FUNCTION(1, "PWRAP_SPICK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(82, "PWRAP_SPI0_CSN"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 82),
+		MTK_FUNCTION(0, "GPIO82"),
+		MTK_FUNCTION(1, "PWRAP_SPICS")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(83, "AUD_CLK_MOSI"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 83),
+		MTK_FUNCTION(0, "GPIO83"),
+		MTK_FUNCTION(1, "AUD_CLK_MOSI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(84, "AUD_DAT_MISO"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 84),
+		MTK_FUNCTION(0, "GPIO84"),
+		MTK_FUNCTION(1, "AUD_DAT_MISO"),
+		MTK_FUNCTION(2, "AUD_DAT_MOSI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(85, "AUD_DAT_MOSI"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 85),
+		MTK_FUNCTION(0, "GPIO85"),
+		MTK_FUNCTION(1, "AUD_DAT_MOSI"),
+		MTK_FUNCTION(2, "AUD_DAT_MISO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(86, "RTC32K_CK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 86),
+		MTK_FUNCTION(0, "GPIO86"),
+		MTK_FUNCTION(1, "RTC32K_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(87, "DISP_PWM0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 87),
+		MTK_FUNCTION(0, "GPIO87"),
+		MTK_FUNCTION(1, "DISP_PWM0"),
+		MTK_FUNCTION(2, "DISP_PWM1"),
+		MTK_FUNCTION(7, "DBG_MON_B_31_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(88, "SRCLKENAI"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 88),
+		MTK_FUNCTION(0, "GPIO88"),
+		MTK_FUNCTION(1, "SRCLKENAI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(89, "SRCLKENAI2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 89),
+		MTK_FUNCTION(0, "GPIO89"),
+		MTK_FUNCTION(1, "SRCLKENAI2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(90, "SRCLKENA0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 90),
+		MTK_FUNCTION(0, "GPIO90"),
+		MTK_FUNCTION(1, "SRCLKENA0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(91, "SRCLKENA1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 91),
+		MTK_FUNCTION(0, "GPIO91"),
+		MTK_FUNCTION(1, "SRCLKENA1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(92, "PCM_CLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 92),
+		MTK_FUNCTION(0, "GPIO92"),
+		MTK_FUNCTION(1, "PCM1_CLK"),
+		MTK_FUNCTION(2, "I2S0_BCK"),
+		MTK_FUNCTION(7, "DBG_MON_A_24_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(93, "PCM_SYNC"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 93),
+		MTK_FUNCTION(0, "GPIO93"),
+		MTK_FUNCTION(1, "PCM1_SYNC"),
+		MTK_FUNCTION(2, "I2S0_WS"),
+		MTK_FUNCTION(7, "DBG_MON_A_25_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(94, "PCM_RX"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 94),
+		MTK_FUNCTION(0, "GPIO94"),
+		MTK_FUNCTION(1, "PCM1_DI"),
+		MTK_FUNCTION(2, "I2S0_DI"),
+		MTK_FUNCTION(7, "DBG_MON_A_26_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(95, "PCM_TX"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 95),
+		MTK_FUNCTION(0, "GPIO95"),
+		MTK_FUNCTION(1, "PCM1_DO"),
+		MTK_FUNCTION(2, "I2S0_DO"),
+		MTK_FUNCTION(7, "DBG_MON_A_27_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(96, "URXD1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 96),
+		MTK_FUNCTION(0, "GPIO96"),
+		MTK_FUNCTION(1, "URXD1"),
+		MTK_FUNCTION(2, "UTXD1"),
+		MTK_FUNCTION(7, "DBG_MON_A_28_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(97, "UTXD1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 97),
+		MTK_FUNCTION(0, "GPIO97"),
+		MTK_FUNCTION(1, "UTXD1"),
+		MTK_FUNCTION(2, "URXD1"),
+		MTK_FUNCTION(7, "DBG_MON_A_29_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(98, "URTS1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 98),
+		MTK_FUNCTION(0, "GPIO98"),
+		MTK_FUNCTION(1, "URTS1"),
+		MTK_FUNCTION(2, "UCTS1"),
+		MTK_FUNCTION(7, "DBG_MON_A_30_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(99, "UCTS1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 99),
+		MTK_FUNCTION(0, "GPIO99"),
+		MTK_FUNCTION(1, "UCTS1"),
+		MTK_FUNCTION(2, "URTS1"),
+		MTK_FUNCTION(7, "DBG_MON_A_31_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(100, "MSDC2_DAT0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 100),
+		MTK_FUNCTION(0, "GPIO100"),
+		MTK_FUNCTION(1, "MSDC2_DAT0"),
+		MTK_FUNCTION(3, "USB_DRVVBUS_P0"),
+		MTK_FUNCTION(4, "SDA5"),
+		MTK_FUNCTION(5, "USB_DRVVBUS_P1"),
+		MTK_FUNCTION(7, "DBG_MON_B_0_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(101, "MSDC2_DAT1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 101),
+		MTK_FUNCTION(0, "GPIO101"),
+		MTK_FUNCTION(1, "MSDC2_DAT1"),
+		MTK_FUNCTION(3, "AUD_SPDIF"),
+		MTK_FUNCTION(4, "SCL5"),
+		MTK_FUNCTION(7, "DBG_MON_B_1_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(102, "MSDC2_DAT2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 102),
+		MTK_FUNCTION(0, "GPIO102"),
+		MTK_FUNCTION(1, "MSDC2_DAT2"),
+		MTK_FUNCTION(3, "UTXD0"),
+		MTK_FUNCTION(5, "PWM0"),
+		MTK_FUNCTION(6, "SPI_CK_1_"),
+		MTK_FUNCTION(7, "DBG_MON_B_2_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(103, "MSDC2_DAT3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 103),
+		MTK_FUNCTION(0, "GPIO103"),
+		MTK_FUNCTION(1, "MSDC2_DAT3"),
+		MTK_FUNCTION(3, "URXD0"),
+		MTK_FUNCTION(5, "PWM1"),
+		MTK_FUNCTION(6, "SPI_MI_1_"),
+		MTK_FUNCTION(7, "DBG_MON_B_3_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(104, "MSDC2_CLK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 104),
+		MTK_FUNCTION(0, "GPIO104"),
+		MTK_FUNCTION(1, "MSDC2_CLK"),
+		MTK_FUNCTION(3, "UTXD3"),
+		MTK_FUNCTION(4, "SDA3"),
+		MTK_FUNCTION(5, "PWM2"),
+		MTK_FUNCTION(6, "SPI_MO_1_"),
+		MTK_FUNCTION(7, "DBG_MON_B_4_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(105, "MSDC2_CMD"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 105),
+		MTK_FUNCTION(0, "GPIO105"),
+		MTK_FUNCTION(1, "MSDC2_CMD"),
+		MTK_FUNCTION(3, "URXD3"),
+		MTK_FUNCTION(4, "SCL3"),
+		MTK_FUNCTION(5, "PWM3"),
+		MTK_FUNCTION(6, "SPI_CS_1_"),
+		MTK_FUNCTION(7, "DBG_MON_B_5_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(106, "SDA3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 106),
+		MTK_FUNCTION(0, "GPIO106"),
+		MTK_FUNCTION(1, "SDA3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(107, "SCL3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 107),
+		MTK_FUNCTION(0, "GPIO107"),
+		MTK_FUNCTION(1, "SCL3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(108, "JTMS"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 108),
+		MTK_FUNCTION(0, "GPIO108"),
+		MTK_FUNCTION(1, "JTMS"),
+		MTK_FUNCTION(2, " MFG_JTAG_TMS"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TMS"),
+		MTK_FUNCTION(6, "DFD_TMS")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(109, "JTCK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 109),
+		MTK_FUNCTION(0, "GPIO109"),
+		MTK_FUNCTION(1, "JTCK"),
+		MTK_FUNCTION(2, " MFG_JTAG_TCK"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TCK"),
+		MTK_FUNCTION(6, "DFD_TCK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(110, "JTDI"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 110),
+		MTK_FUNCTION(0, "GPIO110"),
+		MTK_FUNCTION(1, "JTDI"),
+		MTK_FUNCTION(2, " MFG_JTAG_TDI"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TDI"),
+		MTK_FUNCTION(6, "DFD_TDI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(111, "JTDO"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 111),
+		MTK_FUNCTION(0, "GPIO111"),
+		MTK_FUNCTION(1, "JTDO"),
+		MTK_FUNCTION(2, "MFG_JTAG_TDO"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TDO"),
+		MTK_FUNCTION(6, "DFD_TDO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(112, "JTRST_B"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 112),
+		MTK_FUNCTION(0, "GPIO112"),
+		MTK_FUNCTION(1, "JTRST_B"),
+		MTK_FUNCTION(2, " MFG_JTAG_TRSTN"),
+		MTK_FUNCTION(5, "AP_MD32_JTAG_TRST"),
+		MTK_FUNCTION(6, "DFD_NTRST")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(113, "URXD0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 113),
+		MTK_FUNCTION(0, "GPIO113"),
+		MTK_FUNCTION(1, "URXD0"),
+		MTK_FUNCTION(2, "UTXD0"),
+		MTK_FUNCTION(6, "I2S2_WS"),
+		MTK_FUNCTION(7, "DBG_MON_A_0_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(114, "UTXD0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 114),
+		MTK_FUNCTION(0, "GPIO114"),
+		MTK_FUNCTION(1, "UTXD0"),
+		MTK_FUNCTION(2, "URXD0"),
+		MTK_FUNCTION(6, "I2S2_BCK"),
+		MTK_FUNCTION(7, "DBG_MON_A_1_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(115, "URTS0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 115),
+		MTK_FUNCTION(0, "GPIO115"),
+		MTK_FUNCTION(1, "URTS0"),
+		MTK_FUNCTION(2, "UCTS0"),
+		MTK_FUNCTION(6, "I2S2_MCK"),
+		MTK_FUNCTION(7, "DBG_MON_A_2_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(116, "UCTS0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 116),
+		MTK_FUNCTION(0, "GPIO116"),
+		MTK_FUNCTION(1, "UCTS0"),
+		MTK_FUNCTION(2, "URTS0"),
+		MTK_FUNCTION(6, "I2S2_DI_1"),
+		MTK_FUNCTION(7, "DBG_MON_A_3_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(117, "URXD3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 117),
+		MTK_FUNCTION(0, "GPIO117"),
+		MTK_FUNCTION(1, "URXD3"),
+		MTK_FUNCTION(2, "UTXD3"),
+		MTK_FUNCTION(7, "DBG_MON_A_9_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(118, "UTXD3"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 118),
+		MTK_FUNCTION(0, "GPIO118"),
+		MTK_FUNCTION(1, "UTXD3"),
+		MTK_FUNCTION(2, "URXD3"),
+		MTK_FUNCTION(7, "DBG_MON_A_10_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(119, "KPROW0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 119),
+		MTK_FUNCTION(0, "GPIO119"),
+		MTK_FUNCTION(1, "KROW0"),
+		MTK_FUNCTION(7, "DBG_MON_A_11_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(120, "KPROW1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 120),
+		MTK_FUNCTION(0, "GPIO120"),
+		MTK_FUNCTION(1, "KROW1"),
+		MTK_FUNCTION(3, "PWM6"),
+		MTK_FUNCTION(7, "DBG_MON_A_12_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(121, "KPROW2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 121),
+		MTK_FUNCTION(0, "GPIO121"),
+		MTK_FUNCTION(1, "KROW2"),
+		MTK_FUNCTION(2, "IRDA_PDN"),
+		MTK_FUNCTION(3, "USB_DRVVBUS_P0"),
+		MTK_FUNCTION(4, "PWM4"),
+		MTK_FUNCTION(5, "USB_DRVVBUS_P1"),
+		MTK_FUNCTION(7, "DBG_MON_A_13_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(122, "KPCOL0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 122),
+		MTK_FUNCTION(0, "GPIO122"),
+		MTK_FUNCTION(1, "KCOL0"),
+		MTK_FUNCTION(7, "DBG_MON_A_14_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(123, "KPCOL1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 123),
+		MTK_FUNCTION(0, "GPIO123"),
+		MTK_FUNCTION(1, "KCOL1"),
+		MTK_FUNCTION(2, "IRDA_RXD"),
+		MTK_FUNCTION(3, "PWM5"),
+		MTK_FUNCTION(7, "DBG_MON_A_15_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(124, "KPCOL2"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 124),
+		MTK_FUNCTION(0, "GPIO124"),
+		MTK_FUNCTION(1, "KCOL2"),
+		MTK_FUNCTION(2, "IRDA_TXD"),
+		MTK_FUNCTION(3, "USB_DRVVBUS_P0"),
+		MTK_FUNCTION(4, "PWM3"),
+		MTK_FUNCTION(5, "USB_DRVVBUS_P1"),
+		MTK_FUNCTION(7, "DBG_MON_A_16_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(125, "SDA1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 125),
+		MTK_FUNCTION(0, "GPIO125"),
+		MTK_FUNCTION(1, "SDA1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(126, "SCL1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 126),
+		MTK_FUNCTION(0, "GPIO126"),
+		MTK_FUNCTION(1, "SCL1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(127, "LCM_RST"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 127),
+		MTK_FUNCTION(0, "GPIO127"),
+		MTK_FUNCTION(1, "LCM_RST")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(128, "I2S0_LRCK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 128),
+		MTK_FUNCTION(0, "GPIO128"),
+		MTK_FUNCTION(1, "I2S0_WS"),
+		MTK_FUNCTION(2, "I2S1_WS"),
+		MTK_FUNCTION(3, "I2S2_WS"),
+		MTK_FUNCTION(5, "SPI_CK_2_"),
+		MTK_FUNCTION(7, "DBG_MON_A_4_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(129, "I2S0_BCK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 129),
+		MTK_FUNCTION(0, "GPIO129"),
+		MTK_FUNCTION(1, "I2S0_BCK"),
+		MTK_FUNCTION(2, "I2S1_BCK"),
+		MTK_FUNCTION(3, "I2S2_BCK"),
+		MTK_FUNCTION(5, "SPI_MI_2_"),
+		MTK_FUNCTION(7, "DBG_MON_A_5_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(130, "I2S0_MCK"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 130),
+		MTK_FUNCTION(0, "GPIO130"),
+		MTK_FUNCTION(1, "I2S0_MCK"),
+		MTK_FUNCTION(2, "I2S1_MCK"),
+		MTK_FUNCTION(3, "I2S2_MCK"),
+		MTK_FUNCTION(5, "SPI_MO_2_"),
+		MTK_FUNCTION(7, "DBG_MON_A_6_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(131, "I2S0_DATA0"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 131),
+		MTK_FUNCTION(0, "GPIO131"),
+		MTK_FUNCTION(1, "I2S0_DO"),
+		MTK_FUNCTION(2, "I2S1_DO_1"),
+		MTK_FUNCTION(3, "I2S2_DI_1"),
+		MTK_FUNCTION(5, "SPI_CS_2_"),
+		MTK_FUNCTION(7, "DBG_MON_A_7_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(132, "I2S0_DATA1"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 132),
+		MTK_FUNCTION(0, "GPIO132"),
+		MTK_FUNCTION(1, "I2S0_DI"),
+		MTK_FUNCTION(2, "I2S1_DO_2"),
+		MTK_FUNCTION(3, "I2S2_DI_2"),
+		MTK_FUNCTION(7, "DBG_MON_A_8_")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(133, "SDA4"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 133),
+		MTK_FUNCTION(0, "GPIO133"),
+		MTK_FUNCTION(1, "SDA4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(134, "SCL4"),
+		NULL, "mt8173",
+		MTK_EINT_FUNCTION(0, 134),
+		MTK_FUNCTION(0, "GPIO134"),
+		MTK_FUNCTION(1, "SCL4")
+	),
+};
+
+#endif /* __PINCTRL_MTK_MT8173_H */
diff --git a/drivers/pinctrl/meson/Makefile b/drivers/pinctrl/meson/Makefile
index eafc216067a4..c751d22fdf29 100644
--- a/drivers/pinctrl/meson/Makefile
+++ b/drivers/pinctrl/meson/Makefile
@@ -1,2 +1,2 @@
-obj-y	+= pinctrl-meson8.o
+obj-y	+= pinctrl-meson8.o pinctrl-meson8b.o
 obj-y	+= pinctrl-meson.o
diff --git a/drivers/pinctrl/meson/pinctrl-meson.c b/drivers/pinctrl/meson/pinctrl-meson.c
index a2bf49ce16e7..edcd140e0899 100644
--- a/drivers/pinctrl/meson/pinctrl-meson.c
+++ b/drivers/pinctrl/meson/pinctrl-meson.c
@@ -13,8 +13,9 @@
 
 /*
  * The available pins are organized in banks (A,B,C,D,E,X,Y,Z,AO,
- * BOOT,CARD for meson6 and X,Y,DV,H,Z,AO,BOOT,CARD for meson8) and
- * each bank has a variable number of pins.
+ * BOOT,CARD for meson6, X,Y,DV,H,Z,AO,BOOT,CARD for meson8 and
+ * X,Y,DV,H,AO,BOOT,CARD,DIF for meson8b) and each bank has a
+ * variable number of pins.
  *
  * The AO bank is special because it belongs to the Always-On power
  * domain which can't be powered off; the bank also uses a set of
@@ -544,6 +545,10 @@ static const struct of_device_id meson_pinctrl_dt_match[] = {
 		.compatible = "amlogic,meson8-pinctrl",
 		.data = &meson8_pinctrl_data,
 	},
+	{
+		.compatible = "amlogic,meson8b-pinctrl",
+		.data = &meson8b_pinctrl_data,
+	},
 	{ },
 };
 MODULE_DEVICE_TABLE(of, meson_pinctrl_dt_match);
diff --git a/drivers/pinctrl/meson/pinctrl-meson.h b/drivers/pinctrl/meson/pinctrl-meson.h
index bfea8adc7953..0fe7d53849ce 100644
--- a/drivers/pinctrl/meson/pinctrl-meson.h
+++ b/drivers/pinctrl/meson/pinctrl-meson.h
@@ -155,6 +155,8 @@ struct meson_pinctrl {
 	struct meson_domain *domains;
 };
 
+#define PIN(x, b)	(b + x)
+
 #define GROUP(grp, r, b)						\
 	{								\
 		.name = #grp,						\
@@ -165,10 +167,10 @@ struct meson_pinctrl {
 		.domain = 0,						\
 	 }
 
-#define GPIO_GROUP(gpio)						\
+#define GPIO_GROUP(gpio, b)						\
 	{								\
 		.name = #gpio,						\
-		.pins = (const unsigned int[]){ PIN_ ## gpio},		\
+		.pins = (const unsigned int[]){ PIN(gpio, b) },		\
 		.num_pins = 1,						\
 		.is_gpio = true,					\
 	 }
@@ -204,6 +206,7 @@ struct meson_pinctrl {
 		},							\
 	 }
 
-#define MESON_PIN(x) PINCTRL_PIN(PIN_ ## x, #x)
+#define MESON_PIN(x, b) PINCTRL_PIN(PIN(x, b), #x)
 
 extern struct meson_pinctrl_data meson8_pinctrl_data;
+extern struct meson_pinctrl_data meson8b_pinctrl_data;
diff --git a/drivers/pinctrl/meson/pinctrl-meson8.c b/drivers/pinctrl/meson/pinctrl-meson8.c
index f8aa3a281767..7b1cc91733ef 100644
--- a/drivers/pinctrl/meson/pinctrl-meson8.c
+++ b/drivers/pinctrl/meson/pinctrl-meson8.c
@@ -14,620 +14,482 @@
 #include <dt-bindings/gpio/meson8-gpio.h>
 #include "pinctrl-meson.h"
 
-#define AO_OFFSET	120
-
-#define PIN_GPIOX_0	GPIOX_0
-#define PIN_GPIOX_1	GPIOX_1
-#define PIN_GPIOX_2	GPIOX_2
-#define PIN_GPIOX_3	GPIOX_3
-#define PIN_GPIOX_4	GPIOX_4
-#define PIN_GPIOX_5	GPIOX_5
-#define PIN_GPIOX_6	GPIOX_6
-#define PIN_GPIOX_7	GPIOX_7
-#define PIN_GPIOX_8	GPIOX_8
-#define PIN_GPIOX_9	GPIOX_9
-#define PIN_GPIOX_10	GPIOX_10
-#define PIN_GPIOX_11	GPIOX_11
-#define PIN_GPIOX_12	GPIOX_12
-#define PIN_GPIOX_13	GPIOX_13
-#define PIN_GPIOX_14	GPIOX_14
-#define PIN_GPIOX_15	GPIOX_15
-#define PIN_GPIOX_16	GPIOX_16
-#define PIN_GPIOX_17	GPIOX_17
-#define PIN_GPIOX_18	GPIOX_18
-#define PIN_GPIOX_19	GPIOX_19
-#define PIN_GPIOX_20	GPIOX_20
-#define PIN_GPIOX_21	GPIOX_21
-#define PIN_GPIOY_0	GPIOY_0
-#define PIN_GPIOY_1	GPIOY_1
-#define PIN_GPIOY_2	GPIOY_2
-#define PIN_GPIOY_3	GPIOY_3
-#define PIN_GPIOY_4	GPIOY_4
-#define PIN_GPIOY_5	GPIOY_5
-#define PIN_GPIOY_6	GPIOY_6
-#define PIN_GPIOY_7	GPIOY_7
-#define PIN_GPIOY_8	GPIOY_8
-#define PIN_GPIOY_9	GPIOY_9
-#define PIN_GPIOY_10	GPIOY_10
-#define PIN_GPIOY_11	GPIOY_11
-#define PIN_GPIOY_12	GPIOY_12
-#define PIN_GPIOY_13	GPIOY_13
-#define PIN_GPIOY_14	GPIOY_14
-#define PIN_GPIOY_15	GPIOY_15
-#define PIN_GPIOY_16	GPIOY_16
-#define PIN_GPIODV_0	GPIODV_0
-#define PIN_GPIODV_1	GPIODV_1
-#define PIN_GPIODV_2	GPIODV_2
-#define PIN_GPIODV_3	GPIODV_3
-#define PIN_GPIODV_4	GPIODV_4
-#define PIN_GPIODV_5	GPIODV_5
-#define PIN_GPIODV_6	GPIODV_6
-#define PIN_GPIODV_7	GPIODV_7
-#define PIN_GPIODV_8	GPIODV_8
-#define PIN_GPIODV_9	GPIODV_9
-#define PIN_GPIODV_10	GPIODV_10
-#define PIN_GPIODV_11	GPIODV_11
-#define PIN_GPIODV_12	GPIODV_12
-#define PIN_GPIODV_13	GPIODV_13
-#define PIN_GPIODV_14	GPIODV_14
-#define PIN_GPIODV_15	GPIODV_15
-#define PIN_GPIODV_16	GPIODV_16
-#define PIN_GPIODV_17	GPIODV_17
-#define PIN_GPIODV_18	GPIODV_18
-#define PIN_GPIODV_19	GPIODV_19
-#define PIN_GPIODV_20	GPIODV_20
-#define PIN_GPIODV_21	GPIODV_21
-#define PIN_GPIODV_22	GPIODV_22
-#define PIN_GPIODV_23	GPIODV_23
-#define PIN_GPIODV_24	GPIODV_24
-#define PIN_GPIODV_25	GPIODV_25
-#define PIN_GPIODV_26	GPIODV_26
-#define PIN_GPIODV_27	GPIODV_27
-#define PIN_GPIODV_28	GPIODV_28
-#define PIN_GPIODV_29	GPIODV_29
-#define PIN_GPIOH_0	GPIOH_0
-#define PIN_GPIOH_1	GPIOH_1
-#define PIN_GPIOH_2	GPIOH_2
-#define PIN_GPIOH_3	GPIOH_3
-#define PIN_GPIOH_4	GPIOH_4
-#define PIN_GPIOH_5	GPIOH_5
-#define PIN_GPIOH_6	GPIOH_6
-#define PIN_GPIOH_7	GPIOH_7
-#define PIN_GPIOH_8	GPIOH_8
-#define PIN_GPIOH_9	GPIOH_9
-#define PIN_GPIOZ_0	GPIOZ_0
-#define PIN_GPIOZ_1	GPIOZ_1
-#define PIN_GPIOZ_2	GPIOZ_2
-#define PIN_GPIOZ_3	GPIOZ_3
-#define PIN_GPIOZ_4	GPIOZ_4
-#define PIN_GPIOZ_5	GPIOZ_5
-#define PIN_GPIOZ_6	GPIOZ_6
-#define PIN_GPIOZ_7	GPIOZ_7
-#define PIN_GPIOZ_8	GPIOZ_8
-#define PIN_GPIOZ_9	GPIOZ_9
-#define PIN_GPIOZ_10	GPIOZ_10
-#define PIN_GPIOZ_11	GPIOZ_11
-#define PIN_GPIOZ_12	GPIOZ_12
-#define PIN_GPIOZ_13	GPIOZ_13
-#define PIN_GPIOZ_14	GPIOZ_14
-#define PIN_CARD_0	CARD_0
-#define PIN_CARD_1	CARD_1
-#define PIN_CARD_2	CARD_2
-#define PIN_CARD_3	CARD_3
-#define PIN_CARD_4	CARD_4
-#define PIN_CARD_5	CARD_5
-#define PIN_CARD_6	CARD_6
-#define PIN_BOOT_0	BOOT_0
-#define PIN_BOOT_1	BOOT_1
-#define PIN_BOOT_2	BOOT_2
-#define PIN_BOOT_3	BOOT_3
-#define PIN_BOOT_4	BOOT_4
-#define PIN_BOOT_5	BOOT_5
-#define PIN_BOOT_6	BOOT_6
-#define PIN_BOOT_7	BOOT_7
-#define PIN_BOOT_8	BOOT_8
-#define PIN_BOOT_9	BOOT_9
-#define PIN_BOOT_10	BOOT_10
-#define PIN_BOOT_11	BOOT_11
-#define PIN_BOOT_12	BOOT_12
-#define PIN_BOOT_13	BOOT_13
-#define PIN_BOOT_14	BOOT_14
-#define PIN_BOOT_15	BOOT_15
-#define PIN_BOOT_16	BOOT_16
-#define PIN_BOOT_17	BOOT_17
-#define PIN_BOOT_18	BOOT_18
-
-#define PIN_GPIOAO_0	(AO_OFFSET + GPIOAO_0)
-#define PIN_GPIOAO_1	(AO_OFFSET + GPIOAO_1)
-#define PIN_GPIOAO_2	(AO_OFFSET + GPIOAO_2)
-#define PIN_GPIOAO_3	(AO_OFFSET + GPIOAO_3)
-#define PIN_GPIOAO_4	(AO_OFFSET + GPIOAO_4)
-#define PIN_GPIOAO_5	(AO_OFFSET + GPIOAO_5)
-#define PIN_GPIOAO_6	(AO_OFFSET + GPIOAO_6)
-#define PIN_GPIOAO_7	(AO_OFFSET + GPIOAO_7)
-#define PIN_GPIOAO_8	(AO_OFFSET + GPIOAO_8)
-#define PIN_GPIOAO_9	(AO_OFFSET + GPIOAO_9)
-#define PIN_GPIOAO_10	(AO_OFFSET + GPIOAO_10)
-#define PIN_GPIOAO_11	(AO_OFFSET + GPIOAO_11)
-#define PIN_GPIOAO_12	(AO_OFFSET + GPIOAO_12)
-#define PIN_GPIOAO_13	(AO_OFFSET + GPIOAO_13)
-#define PIN_GPIO_BSD_EN	(AO_OFFSET + GPIO_BSD_EN)
-#define PIN_GPIO_TEST_N	(AO_OFFSET + GPIO_TEST_N)
+#define AO_OFF	120
 
 static const struct pinctrl_pin_desc meson8_pins[] = {
-	MESON_PIN(GPIOX_0),
-	MESON_PIN(GPIOX_1),
-	MESON_PIN(GPIOX_2),
-	MESON_PIN(GPIOX_3),
-	MESON_PIN(GPIOX_4),
-	MESON_PIN(GPIOX_5),
-	MESON_PIN(GPIOX_6),
-	MESON_PIN(GPIOX_7),
-	MESON_PIN(GPIOX_8),
-	MESON_PIN(GPIOX_9),
-	MESON_PIN(GPIOX_10),
-	MESON_PIN(GPIOX_11),
-	MESON_PIN(GPIOX_12),
-	MESON_PIN(GPIOX_13),
-	MESON_PIN(GPIOX_14),
-	MESON_PIN(GPIOX_15),
-	MESON_PIN(GPIOX_16),
-	MESON_PIN(GPIOX_17),
-	MESON_PIN(GPIOX_18),
-	MESON_PIN(GPIOX_19),
-	MESON_PIN(GPIOX_20),
-	MESON_PIN(GPIOX_21),
-	MESON_PIN(GPIOY_0),
-	MESON_PIN(GPIOY_1),
-	MESON_PIN(GPIOY_2),
-	MESON_PIN(GPIOY_3),
-	MESON_PIN(GPIOY_4),
-	MESON_PIN(GPIOY_5),
-	MESON_PIN(GPIOY_6),
-	MESON_PIN(GPIOY_7),
-	MESON_PIN(GPIOY_8),
-	MESON_PIN(GPIOY_9),
-	MESON_PIN(GPIOY_10),
-	MESON_PIN(GPIOY_11),
-	MESON_PIN(GPIOY_12),
-	MESON_PIN(GPIOY_13),
-	MESON_PIN(GPIOY_14),
-	MESON_PIN(GPIOY_15),
-	MESON_PIN(GPIOY_16),
-	MESON_PIN(GPIODV_0),
-	MESON_PIN(GPIODV_1),
-	MESON_PIN(GPIODV_2),
-	MESON_PIN(GPIODV_3),
-	MESON_PIN(GPIODV_4),
-	MESON_PIN(GPIODV_5),
-	MESON_PIN(GPIODV_6),
-	MESON_PIN(GPIODV_7),
-	MESON_PIN(GPIODV_8),
-	MESON_PIN(GPIODV_9),
-	MESON_PIN(GPIODV_10),
-	MESON_PIN(GPIODV_11),
-	MESON_PIN(GPIODV_12),
-	MESON_PIN(GPIODV_13),
-	MESON_PIN(GPIODV_14),
-	MESON_PIN(GPIODV_15),
-	MESON_PIN(GPIODV_16),
-	MESON_PIN(GPIODV_17),
-	MESON_PIN(GPIODV_18),
-	MESON_PIN(GPIODV_19),
-	MESON_PIN(GPIODV_20),
-	MESON_PIN(GPIODV_21),
-	MESON_PIN(GPIODV_22),
-	MESON_PIN(GPIODV_23),
-	MESON_PIN(GPIODV_24),
-	MESON_PIN(GPIODV_25),
-	MESON_PIN(GPIODV_26),
-	MESON_PIN(GPIODV_27),
-	MESON_PIN(GPIODV_28),
-	MESON_PIN(GPIODV_29),
-	MESON_PIN(GPIOH_0),
-	MESON_PIN(GPIOH_1),
-	MESON_PIN(GPIOH_2),
-	MESON_PIN(GPIOH_3),
-	MESON_PIN(GPIOH_4),
-	MESON_PIN(GPIOH_5),
-	MESON_PIN(GPIOH_6),
-	MESON_PIN(GPIOH_7),
-	MESON_PIN(GPIOH_8),
-	MESON_PIN(GPIOH_9),
-	MESON_PIN(GPIOZ_0),
-	MESON_PIN(GPIOZ_1),
-	MESON_PIN(GPIOZ_2),
-	MESON_PIN(GPIOZ_3),
-	MESON_PIN(GPIOZ_4),
-	MESON_PIN(GPIOZ_5),
-	MESON_PIN(GPIOZ_6),
-	MESON_PIN(GPIOZ_7),
-	MESON_PIN(GPIOZ_8),
-	MESON_PIN(GPIOZ_9),
-	MESON_PIN(GPIOZ_10),
-	MESON_PIN(GPIOZ_11),
-	MESON_PIN(GPIOZ_12),
-	MESON_PIN(GPIOZ_13),
-	MESON_PIN(GPIOZ_14),
-	MESON_PIN(CARD_0),
-	MESON_PIN(CARD_1),
-	MESON_PIN(CARD_2),
-	MESON_PIN(CARD_3),
-	MESON_PIN(CARD_4),
-	MESON_PIN(CARD_5),
-	MESON_PIN(CARD_6),
-	MESON_PIN(BOOT_0),
-	MESON_PIN(BOOT_1),
-	MESON_PIN(BOOT_2),
-	MESON_PIN(BOOT_3),
-	MESON_PIN(BOOT_4),
-	MESON_PIN(BOOT_5),
-	MESON_PIN(BOOT_6),
-	MESON_PIN(BOOT_7),
-	MESON_PIN(BOOT_8),
-	MESON_PIN(BOOT_9),
-	MESON_PIN(BOOT_10),
-	MESON_PIN(BOOT_11),
-	MESON_PIN(BOOT_12),
-	MESON_PIN(BOOT_13),
-	MESON_PIN(BOOT_14),
-	MESON_PIN(BOOT_15),
-	MESON_PIN(BOOT_16),
-	MESON_PIN(BOOT_17),
-	MESON_PIN(BOOT_18),
-	MESON_PIN(GPIOAO_0),
-	MESON_PIN(GPIOAO_1),
-	MESON_PIN(GPIOAO_2),
-	MESON_PIN(GPIOAO_3),
-	MESON_PIN(GPIOAO_4),
-	MESON_PIN(GPIOAO_5),
-	MESON_PIN(GPIOAO_6),
-	MESON_PIN(GPIOAO_7),
-	MESON_PIN(GPIOAO_8),
-	MESON_PIN(GPIOAO_9),
-	MESON_PIN(GPIOAO_10),
-	MESON_PIN(GPIOAO_11),
-	MESON_PIN(GPIOAO_12),
-	MESON_PIN(GPIOAO_13),
-	MESON_PIN(GPIO_BSD_EN),
-	MESON_PIN(GPIO_TEST_N),
+	MESON_PIN(GPIOX_0, 0),
+	MESON_PIN(GPIOX_1, 0),
+	MESON_PIN(GPIOX_2, 0),
+	MESON_PIN(GPIOX_3, 0),
+	MESON_PIN(GPIOX_4, 0),
+	MESON_PIN(GPIOX_5, 0),
+	MESON_PIN(GPIOX_6, 0),
+	MESON_PIN(GPIOX_7, 0),
+	MESON_PIN(GPIOX_8, 0),
+	MESON_PIN(GPIOX_9, 0),
+	MESON_PIN(GPIOX_10, 0),
+	MESON_PIN(GPIOX_11, 0),
+	MESON_PIN(GPIOX_12, 0),
+	MESON_PIN(GPIOX_13, 0),
+	MESON_PIN(GPIOX_14, 0),
+	MESON_PIN(GPIOX_15, 0),
+	MESON_PIN(GPIOX_16, 0),
+	MESON_PIN(GPIOX_17, 0),
+	MESON_PIN(GPIOX_18, 0),
+	MESON_PIN(GPIOX_19, 0),
+	MESON_PIN(GPIOX_20, 0),
+	MESON_PIN(GPIOX_21, 0),
+	MESON_PIN(GPIOY_0, 0),
+	MESON_PIN(GPIOY_1, 0),
+	MESON_PIN(GPIOY_2, 0),
+	MESON_PIN(GPIOY_3, 0),
+	MESON_PIN(GPIOY_4, 0),
+	MESON_PIN(GPIOY_5, 0),
+	MESON_PIN(GPIOY_6, 0),
+	MESON_PIN(GPIOY_7, 0),
+	MESON_PIN(GPIOY_8, 0),
+	MESON_PIN(GPIOY_9, 0),
+	MESON_PIN(GPIOY_10, 0),
+	MESON_PIN(GPIOY_11, 0),
+	MESON_PIN(GPIOY_12, 0),
+	MESON_PIN(GPIOY_13, 0),
+	MESON_PIN(GPIOY_14, 0),
+	MESON_PIN(GPIOY_15, 0),
+	MESON_PIN(GPIOY_16, 0),
+	MESON_PIN(GPIODV_0, 0),
+	MESON_PIN(GPIODV_1, 0),
+	MESON_PIN(GPIODV_2, 0),
+	MESON_PIN(GPIODV_3, 0),
+	MESON_PIN(GPIODV_4, 0),
+	MESON_PIN(GPIODV_5, 0),
+	MESON_PIN(GPIODV_6, 0),
+	MESON_PIN(GPIODV_7, 0),
+	MESON_PIN(GPIODV_8, 0),
+	MESON_PIN(GPIODV_9, 0),
+	MESON_PIN(GPIODV_10, 0),
+	MESON_PIN(GPIODV_11, 0),
+	MESON_PIN(GPIODV_12, 0),
+	MESON_PIN(GPIODV_13, 0),
+	MESON_PIN(GPIODV_14, 0),
+	MESON_PIN(GPIODV_15, 0),
+	MESON_PIN(GPIODV_16, 0),
+	MESON_PIN(GPIODV_17, 0),
+	MESON_PIN(GPIODV_18, 0),
+	MESON_PIN(GPIODV_19, 0),
+	MESON_PIN(GPIODV_20, 0),
+	MESON_PIN(GPIODV_21, 0),
+	MESON_PIN(GPIODV_22, 0),
+	MESON_PIN(GPIODV_23, 0),
+	MESON_PIN(GPIODV_24, 0),
+	MESON_PIN(GPIODV_25, 0),
+	MESON_PIN(GPIODV_26, 0),
+	MESON_PIN(GPIODV_27, 0),
+	MESON_PIN(GPIODV_28, 0),
+	MESON_PIN(GPIODV_29, 0),
+	MESON_PIN(GPIOH_0, 0),
+	MESON_PIN(GPIOH_1, 0),
+	MESON_PIN(GPIOH_2, 0),
+	MESON_PIN(GPIOH_3, 0),
+	MESON_PIN(GPIOH_4, 0),
+	MESON_PIN(GPIOH_5, 0),
+	MESON_PIN(GPIOH_6, 0),
+	MESON_PIN(GPIOH_7, 0),
+	MESON_PIN(GPIOH_8, 0),
+	MESON_PIN(GPIOH_9, 0),
+	MESON_PIN(GPIOZ_0, 0),
+	MESON_PIN(GPIOZ_1, 0),
+	MESON_PIN(GPIOZ_2, 0),
+	MESON_PIN(GPIOZ_3, 0),
+	MESON_PIN(GPIOZ_4, 0),
+	MESON_PIN(GPIOZ_5, 0),
+	MESON_PIN(GPIOZ_6, 0),
+	MESON_PIN(GPIOZ_7, 0),
+	MESON_PIN(GPIOZ_8, 0),
+	MESON_PIN(GPIOZ_9, 0),
+	MESON_PIN(GPIOZ_10, 0),
+	MESON_PIN(GPIOZ_11, 0),
+	MESON_PIN(GPIOZ_12, 0),
+	MESON_PIN(GPIOZ_13, 0),
+	MESON_PIN(GPIOZ_14, 0),
+	MESON_PIN(CARD_0, 0),
+	MESON_PIN(CARD_1, 0),
+	MESON_PIN(CARD_2, 0),
+	MESON_PIN(CARD_3, 0),
+	MESON_PIN(CARD_4, 0),
+	MESON_PIN(CARD_5, 0),
+	MESON_PIN(CARD_6, 0),
+	MESON_PIN(BOOT_0, 0),
+	MESON_PIN(BOOT_1, 0),
+	MESON_PIN(BOOT_2, 0),
+	MESON_PIN(BOOT_3, 0),
+	MESON_PIN(BOOT_4, 0),
+	MESON_PIN(BOOT_5, 0),
+	MESON_PIN(BOOT_6, 0),
+	MESON_PIN(BOOT_7, 0),
+	MESON_PIN(BOOT_8, 0),
+	MESON_PIN(BOOT_9, 0),
+	MESON_PIN(BOOT_10, 0),
+	MESON_PIN(BOOT_11, 0),
+	MESON_PIN(BOOT_12, 0),
+	MESON_PIN(BOOT_13, 0),
+	MESON_PIN(BOOT_14, 0),
+	MESON_PIN(BOOT_15, 0),
+	MESON_PIN(BOOT_16, 0),
+	MESON_PIN(BOOT_17, 0),
+	MESON_PIN(BOOT_18, 0),
+	MESON_PIN(GPIOAO_0, AO_OFF),
+	MESON_PIN(GPIOAO_1, AO_OFF),
+	MESON_PIN(GPIOAO_2, AO_OFF),
+	MESON_PIN(GPIOAO_3, AO_OFF),
+	MESON_PIN(GPIOAO_4, AO_OFF),
+	MESON_PIN(GPIOAO_5, AO_OFF),
+	MESON_PIN(GPIOAO_6, AO_OFF),
+	MESON_PIN(GPIOAO_7, AO_OFF),
+	MESON_PIN(GPIOAO_8, AO_OFF),
+	MESON_PIN(GPIOAO_9, AO_OFF),
+	MESON_PIN(GPIOAO_10, AO_OFF),
+	MESON_PIN(GPIOAO_11, AO_OFF),
+	MESON_PIN(GPIOAO_12, AO_OFF),
+	MESON_PIN(GPIOAO_13, AO_OFF),
+	MESON_PIN(GPIO_BSD_EN, AO_OFF),
+	MESON_PIN(GPIO_TEST_N, AO_OFF),
 };
 
 /* bank X */
-static const unsigned int sd_d0_a_pins[] = { PIN_GPIOX_0 };
-static const unsigned int sd_d1_a_pins[] = { PIN_GPIOX_1 };
-static const unsigned int sd_d2_a_pins[] = { PIN_GPIOX_2 };
-static const unsigned int sd_d3_a_pins[] = { PIN_GPIOX_3 };
-static const unsigned int sd_clk_a_pins[] = { PIN_GPIOX_8 };
-static const unsigned int sd_cmd_a_pins[] = { PIN_GPIOX_9 };
-
-static const unsigned int sdxc_d0_a_pins[] = { PIN_GPIOX_0 };
-static const unsigned int sdxc_d13_a_pins[] = { PIN_GPIOX_1, PIN_GPIOX_2,
-						PIN_GPIOX_3 };
-static const unsigned int sdxc_d47_a_pins[] = { PIN_GPIOX_4, PIN_GPIOX_5,
-						PIN_GPIOX_6, PIN_GPIOX_7 };
-static const unsigned int sdxc_clk_a_pins[] = { PIN_GPIOX_8 };
-static const unsigned int sdxc_cmd_a_pins[] = { PIN_GPIOX_9 };
-
-static const unsigned int pcm_out_a_pins[] = { PIN_GPIOX_4 };
-static const unsigned int pcm_in_a_pins[] = { PIN_GPIOX_5 };
-static const unsigned int pcm_fs_a_pins[] = { PIN_GPIOX_6 };
-static const unsigned int pcm_clk_a_pins[] = { PIN_GPIOX_7 };
-
-static const unsigned int uart_tx_a0_pins[] = { PIN_GPIOX_4 };
-static const unsigned int uart_rx_a0_pins[] = { PIN_GPIOX_5 };
-static const unsigned int uart_cts_a0_pins[] = { PIN_GPIOX_6 };
-static const unsigned int uart_rts_a0_pins[] = { PIN_GPIOX_7 };
-
-static const unsigned int uart_tx_a1_pins[] = { PIN_GPIOX_12 };
-static const unsigned int uart_rx_a1_pins[] = { PIN_GPIOX_13 };
-static const unsigned int uart_cts_a1_pins[] = { PIN_GPIOX_14 };
-static const unsigned int uart_rts_a1_pins[] = { PIN_GPIOX_15 };
-
-static const unsigned int uart_tx_b0_pins[] = { PIN_GPIOX_16 };
-static const unsigned int uart_rx_b0_pins[] = { PIN_GPIOX_17 };
-static const unsigned int uart_cts_b0_pins[] = { PIN_GPIOX_18 };
-static const unsigned int uart_rts_b0_pins[] = { PIN_GPIOX_19 };
-
-static const unsigned int iso7816_det_pins[] = { PIN_GPIOX_16 };
-static const unsigned int iso7816_reset_pins[] = { PIN_GPIOX_17 };
-static const unsigned int iso7816_clk_pins[] = { PIN_GPIOX_18 };
-static const unsigned int iso7816_data_pins[] = { PIN_GPIOX_19 };
-
-static const unsigned int i2c_sda_d0_pins[] = { PIN_GPIOX_16 };
-static const unsigned int i2c_sck_d0_pins[] = { PIN_GPIOX_17 };
-
-static const unsigned int xtal_32k_out_pins[] = { PIN_GPIOX_10 };
-static const unsigned int xtal_24m_out_pins[] = { PIN_GPIOX_11 };
+static const unsigned int sd_d0_a_pins[] = { PIN(GPIOX_0, 0) };
+static const unsigned int sd_d1_a_pins[] = { PIN(GPIOX_1, 0) };
+static const unsigned int sd_d2_a_pins[] = { PIN(GPIOX_2, 0) };
+static const unsigned int sd_d3_a_pins[] = { PIN(GPIOX_3, 0) };
+static const unsigned int sd_clk_a_pins[] = { PIN(GPIOX_8, 0) };
+static const unsigned int sd_cmd_a_pins[] = { PIN(GPIOX_9, 0) };
+
+static const unsigned int sdxc_d0_a_pins[] = { PIN(GPIOX_0, 0) };
+static const unsigned int sdxc_d13_a_pins[] = { PIN(GPIOX_1, 0), PIN(GPIOX_2, 0),
+						PIN(GPIOX_3, 0) };
+static const unsigned int sdxc_d47_a_pins[] = { PIN(GPIOX_4, 0), PIN(GPIOX_5, 0),
+						PIN(GPIOX_6, 0), PIN(GPIOX_7, 0) };
+static const unsigned int sdxc_clk_a_pins[] = { PIN(GPIOX_8, 0) };
+static const unsigned int sdxc_cmd_a_pins[] = { PIN(GPIOX_9, 0) };
+
+static const unsigned int pcm_out_a_pins[] = { PIN(GPIOX_4, 0) };
+static const unsigned int pcm_in_a_pins[] = { PIN(GPIOX_5, 0) };
+static const unsigned int pcm_fs_a_pins[] = { PIN(GPIOX_6, 0) };
+static const unsigned int pcm_clk_a_pins[] = { PIN(GPIOX_7, 0) };
+
+static const unsigned int uart_tx_a0_pins[] = { PIN(GPIOX_4, 0) };
+static const unsigned int uart_rx_a0_pins[] = { PIN(GPIOX_5, 0) };
+static const unsigned int uart_cts_a0_pins[] = { PIN(GPIOX_6, 0) };
+static const unsigned int uart_rts_a0_pins[] = { PIN(GPIOX_7, 0) };
+
+static const unsigned int uart_tx_a1_pins[] = { PIN(GPIOX_12, 0) };
+static const unsigned int uart_rx_a1_pins[] = { PIN(GPIOX_13, 0) };
+static const unsigned int uart_cts_a1_pins[] = { PIN(GPIOX_14, 0) };
+static const unsigned int uart_rts_a1_pins[] = { PIN(GPIOX_15, 0) };
+
+static const unsigned int uart_tx_b0_pins[] = { PIN(GPIOX_16, 0) };
+static const unsigned int uart_rx_b0_pins[] = { PIN(GPIOX_17, 0) };
+static const unsigned int uart_cts_b0_pins[] = { PIN(GPIOX_18, 0) };
+static const unsigned int uart_rts_b0_pins[] = { PIN(GPIOX_19, 0) };
+
+static const unsigned int iso7816_det_pins[] = { PIN(GPIOX_16, 0) };
+static const unsigned int iso7816_reset_pins[] = { PIN(GPIOX_17, 0) };
+static const unsigned int iso7816_clk_pins[] = { PIN(GPIOX_18, 0) };
+static const unsigned int iso7816_data_pins[] = { PIN(GPIOX_19, 0) };
+
+static const unsigned int i2c_sda_d0_pins[] = { PIN(GPIOX_16, 0) };
+static const unsigned int i2c_sck_d0_pins[] = { PIN(GPIOX_17, 0) };
+
+static const unsigned int xtal_32k_out_pins[] = { PIN(GPIOX_10, 0) };
+static const unsigned int xtal_24m_out_pins[] = { PIN(GPIOX_11, 0) };
 
 /* bank Y */
-static const unsigned int uart_tx_c_pins[] = { PIN_GPIOY_0 };
-static const unsigned int uart_rx_c_pins[] = { PIN_GPIOY_1 };
-static const unsigned int uart_cts_c_pins[] = { PIN_GPIOY_2 };
-static const unsigned int uart_rts_c_pins[] = { PIN_GPIOY_3 };
+static const unsigned int uart_tx_c_pins[] = { PIN(GPIOY_0, 0) };
+static const unsigned int uart_rx_c_pins[] = { PIN(GPIOY_1, 0) };
+static const unsigned int uart_cts_c_pins[] = { PIN(GPIOY_2, 0) };
+static const unsigned int uart_rts_c_pins[] = { PIN(GPIOY_3, 0) };
 
-static const unsigned int pcm_out_b_pins[] = { PIN_GPIOY_4 };
-static const unsigned int pcm_in_b_pins[] = { PIN_GPIOY_5 };
-static const unsigned int pcm_fs_b_pins[] = { PIN_GPIOY_6 };
-static const unsigned int pcm_clk_b_pins[] = { PIN_GPIOY_7 };
+static const unsigned int pcm_out_b_pins[] = { PIN(GPIOY_4, 0) };
+static const unsigned int pcm_in_b_pins[] = { PIN(GPIOY_5, 0) };
+static const unsigned int pcm_fs_b_pins[] = { PIN(GPIOY_6, 0) };
+static const unsigned int pcm_clk_b_pins[] = { PIN(GPIOY_7, 0) };
 
-static const unsigned int i2c_sda_c0_pins[] = { PIN_GPIOY_0 };
-static const unsigned int i2c_sck_c0_pins[] = { PIN_GPIOY_1 };
+static const unsigned int i2c_sda_c0_pins[] = { PIN(GPIOY_0, 0) };
+static const unsigned int i2c_sck_c0_pins[] = { PIN(GPIOY_1, 0) };
 
 /* bank DV */
-static const unsigned int dvin_rgb_pins[] = { PIN_GPIODV_0, PIN_GPIODV_1,
-					      PIN_GPIODV_2, PIN_GPIODV_3,
-					      PIN_GPIODV_4, PIN_GPIODV_5,
-					      PIN_GPIODV_6, PIN_GPIODV_7,
-					      PIN_GPIODV_8, PIN_GPIODV_9,
-					      PIN_GPIODV_10, PIN_GPIODV_11,
-					      PIN_GPIODV_12, PIN_GPIODV_13,
-					      PIN_GPIODV_14, PIN_GPIODV_15,
-					      PIN_GPIODV_16, PIN_GPIODV_17,
-					      PIN_GPIODV_18, PIN_GPIODV_19,
-					      PIN_GPIODV_20, PIN_GPIODV_21,
-					      PIN_GPIODV_22, PIN_GPIODV_23 };
-static const unsigned int dvin_vs_pins[] = { PIN_GPIODV_24 };
-static const unsigned int dvin_hs_pins[] = { PIN_GPIODV_25 };
-static const unsigned int dvin_clk_pins[] = { PIN_GPIODV_26 };
-static const unsigned int dvin_de_pins[] = { PIN_GPIODV_27 };
-
-static const unsigned int enc_0_pins[] = { PIN_GPIODV_0 };
-static const unsigned int enc_1_pins[] = { PIN_GPIODV_1 };
-static const unsigned int enc_2_pins[] = { PIN_GPIODV_2 };
-static const unsigned int enc_3_pins[] = { PIN_GPIODV_3 };
-static const unsigned int enc_4_pins[] = { PIN_GPIODV_4 };
-static const unsigned int enc_5_pins[] = { PIN_GPIODV_5 };
-static const unsigned int enc_6_pins[] = { PIN_GPIODV_6 };
-static const unsigned int enc_7_pins[] = { PIN_GPIODV_7 };
-static const unsigned int enc_8_pins[] = { PIN_GPIODV_8 };
-static const unsigned int enc_9_pins[] = { PIN_GPIODV_9 };
-static const unsigned int enc_10_pins[] = { PIN_GPIODV_10 };
-static const unsigned int enc_11_pins[] = { PIN_GPIODV_11 };
-static const unsigned int enc_12_pins[] = { PIN_GPIODV_12 };
-static const unsigned int enc_13_pins[] = { PIN_GPIODV_13 };
-static const unsigned int enc_14_pins[] = { PIN_GPIODV_14 };
-static const unsigned int enc_15_pins[] = { PIN_GPIODV_15 };
-static const unsigned int enc_16_pins[] = { PIN_GPIODV_16 };
-static const unsigned int enc_17_pins[] = { PIN_GPIODV_17 };
-
-static const unsigned int uart_tx_b1_pins[] = { PIN_GPIODV_24 };
-static const unsigned int uart_rx_b1_pins[] = { PIN_GPIODV_25 };
-static const unsigned int uart_cts_b1_pins[] = { PIN_GPIODV_26 };
-static const unsigned int uart_rts_b1_pins[] = { PIN_GPIODV_27 };
-
-static const unsigned int vga_vs_pins[] = { PIN_GPIODV_24 };
-static const unsigned int vga_hs_pins[] = { PIN_GPIODV_25 };
+static const unsigned int dvin_rgb_pins[] = { PIN(GPIODV_0, 0), PIN(GPIODV_1, 0),
+					      PIN(GPIODV_2, 0), PIN(GPIODV_3, 0),
+					      PIN(GPIODV_4, 0), PIN(GPIODV_5, 0),
+					      PIN(GPIODV_6, 0), PIN(GPIODV_7, 0),
+					      PIN(GPIODV_8, 0), PIN(GPIODV_9, 0),
+					      PIN(GPIODV_10, 0), PIN(GPIODV_11, 0),
+					      PIN(GPIODV_12, 0), PIN(GPIODV_13, 0),
+					      PIN(GPIODV_14, 0), PIN(GPIODV_15, 0),
+					      PIN(GPIODV_16, 0), PIN(GPIODV_17, 0),
+					      PIN(GPIODV_18, 0), PIN(GPIODV_19, 0),
+					      PIN(GPIODV_20, 0), PIN(GPIODV_21, 0),
+					      PIN(GPIODV_22, 0), PIN(GPIODV_23, 0) };
+static const unsigned int dvin_vs_pins[] = { PIN(GPIODV_24, 0) };
+static const unsigned int dvin_hs_pins[] = { PIN(GPIODV_25, 0) };
+static const unsigned int dvin_clk_pins[] = { PIN(GPIODV_26, 0) };
+static const unsigned int dvin_de_pins[] = { PIN(GPIODV_27, 0) };
+
+static const unsigned int enc_0_pins[] = { PIN(GPIODV_0, 0) };
+static const unsigned int enc_1_pins[] = { PIN(GPIODV_1, 0) };
+static const unsigned int enc_2_pins[] = { PIN(GPIODV_2, 0) };
+static const unsigned int enc_3_pins[] = { PIN(GPIODV_3, 0) };
+static const unsigned int enc_4_pins[] = { PIN(GPIODV_4, 0) };
+static const unsigned int enc_5_pins[] = { PIN(GPIODV_5, 0) };
+static const unsigned int enc_6_pins[] = { PIN(GPIODV_6, 0) };
+static const unsigned int enc_7_pins[] = { PIN(GPIODV_7, 0) };
+static const unsigned int enc_8_pins[] = { PIN(GPIODV_8, 0) };
+static const unsigned int enc_9_pins[] = { PIN(GPIODV_9, 0) };
+static const unsigned int enc_10_pins[] = { PIN(GPIODV_10, 0) };
+static const unsigned int enc_11_pins[] = { PIN(GPIODV_11, 0) };
+static const unsigned int enc_12_pins[] = { PIN(GPIODV_12, 0) };
+static const unsigned int enc_13_pins[] = { PIN(GPIODV_13, 0) };
+static const unsigned int enc_14_pins[] = { PIN(GPIODV_14, 0) };
+static const unsigned int enc_15_pins[] = { PIN(GPIODV_15, 0) };
+static const unsigned int enc_16_pins[] = { PIN(GPIODV_16, 0) };
+static const unsigned int enc_17_pins[] = { PIN(GPIODV_17, 0) };
+
+static const unsigned int uart_tx_b1_pins[] = { PIN(GPIODV_24, 0) };
+static const unsigned int uart_rx_b1_pins[] = { PIN(GPIODV_25, 0) };
+static const unsigned int uart_cts_b1_pins[] = { PIN(GPIODV_26, 0) };
+static const unsigned int uart_rts_b1_pins[] = { PIN(GPIODV_27, 0) };
+
+static const unsigned int vga_vs_pins[] = { PIN(GPIODV_24, 0) };
+static const unsigned int vga_hs_pins[] = { PIN(GPIODV_25, 0) };
 
 /* bank H */
-static const unsigned int hdmi_hpd_pins[] = { PIN_GPIOH_0 };
-static const unsigned int hdmi_sda_pins[] = { PIN_GPIOH_1 };
-static const unsigned int hdmi_scl_pins[] = { PIN_GPIOH_2 };
-static const unsigned int hdmi_cec_pins[] = { PIN_GPIOH_3 };
+static const unsigned int hdmi_hpd_pins[] = { PIN(GPIOH_0, 0) };
+static const unsigned int hdmi_sda_pins[] = { PIN(GPIOH_1, 0) };
+static const unsigned int hdmi_scl_pins[] = { PIN(GPIOH_2, 0) };
+static const unsigned int hdmi_cec_pins[] = { PIN(GPIOH_3, 0) };
 
-static const unsigned int spi_ss0_0_pins[] = { PIN_GPIOH_3 };
-static const unsigned int spi_miso_0_pins[] = { PIN_GPIOH_4 };
-static const unsigned int spi_mosi_0_pins[] = { PIN_GPIOH_5 };
-static const unsigned int spi_sclk_0_pins[] = { PIN_GPIOH_6 };
+static const unsigned int spi_ss0_0_pins[] = { PIN(GPIOH_3, 0) };
+static const unsigned int spi_miso_0_pins[] = { PIN(GPIOH_4, 0) };
+static const unsigned int spi_mosi_0_pins[] = { PIN(GPIOH_5, 0) };
+static const unsigned int spi_sclk_0_pins[] = { PIN(GPIOH_6, 0) };
 
-static const unsigned int i2c_sda_d1_pins[] = { PIN_GPIOH_7 };
-static const unsigned int i2c_sck_d1_pins[] = { PIN_GPIOH_8 };
+static const unsigned int i2c_sda_d1_pins[] = { PIN(GPIOH_7, 0) };
+static const unsigned int i2c_sck_d1_pins[] = { PIN(GPIOH_8, 0) };
 
 /* bank Z */
-static const unsigned int spi_ss0_1_pins[] = { PIN_GPIOZ_9 };
-static const unsigned int spi_ss1_1_pins[] = { PIN_GPIOZ_10 };
-static const unsigned int spi_sclk_1_pins[] = { PIN_GPIOZ_11 };
-static const unsigned int spi_mosi_1_pins[] = { PIN_GPIOZ_12 };
-static const unsigned int spi_miso_1_pins[] = { PIN_GPIOZ_13 };
-static const unsigned int spi_ss2_1_pins[] = { PIN_GPIOZ_14 };
-
-static const unsigned int eth_tx_clk_50m_pins[] = { PIN_GPIOZ_4 };
-static const unsigned int eth_tx_en_pins[] = { PIN_GPIOZ_5 };
-static const unsigned int eth_txd1_pins[] = { PIN_GPIOZ_6 };
-static const unsigned int eth_txd0_pins[] = { PIN_GPIOZ_7 };
-static const unsigned int eth_rx_clk_in_pins[] = { PIN_GPIOZ_8 };
-static const unsigned int eth_rx_dv_pins[] = { PIN_GPIOZ_9 };
-static const unsigned int eth_rxd1_pins[] = { PIN_GPIOZ_10 };
-static const unsigned int eth_rxd0_pins[] = { PIN_GPIOZ_11 };
-static const unsigned int eth_mdio_pins[] = { PIN_GPIOZ_12 };
-static const unsigned int eth_mdc_pins[] = { PIN_GPIOZ_13 };
-
-static const unsigned int i2c_sda_a0_pins[] = { PIN_GPIOZ_0 };
-static const unsigned int i2c_sck_a0_pins[] = { PIN_GPIOZ_1 };
-
-static const unsigned int i2c_sda_b_pins[] = { PIN_GPIOZ_2 };
-static const unsigned int i2c_sck_b_pins[] = { PIN_GPIOZ_3 };
-
-static const unsigned int i2c_sda_c1_pins[] = { PIN_GPIOZ_4 };
-static const unsigned int i2c_sck_c1_pins[] = { PIN_GPIOZ_5 };
-
-static const unsigned int i2c_sda_a1_pins[] = { PIN_GPIOZ_0 };
-static const unsigned int i2c_sck_a1_pins[] = { PIN_GPIOZ_1 };
-
-static const unsigned int i2c_sda_a2_pins[] = { PIN_GPIOZ_0 };
-static const unsigned int i2c_sck_a2_pins[] = { PIN_GPIOZ_1 };
+static const unsigned int spi_ss0_1_pins[] = { PIN(GPIOZ_9, 0) };
+static const unsigned int spi_ss1_1_pins[] = { PIN(GPIOZ_10, 0) };
+static const unsigned int spi_sclk_1_pins[] = { PIN(GPIOZ_11, 0) };
+static const unsigned int spi_mosi_1_pins[] = { PIN(GPIOZ_12, 0) };
+static const unsigned int spi_miso_1_pins[] = { PIN(GPIOZ_13, 0) };
+static const unsigned int spi_ss2_1_pins[] = { PIN(GPIOZ_14, 0) };
+
+static const unsigned int eth_tx_clk_50m_pins[] = { PIN(GPIOZ_4, 0) };
+static const unsigned int eth_tx_en_pins[] = { PIN(GPIOZ_5, 0) };
+static const unsigned int eth_txd1_pins[] = { PIN(GPIOZ_6, 0) };
+static const unsigned int eth_txd0_pins[] = { PIN(GPIOZ_7, 0) };
+static const unsigned int eth_rx_clk_in_pins[] = { PIN(GPIOZ_8, 0) };
+static const unsigned int eth_rx_dv_pins[] = { PIN(GPIOZ_9, 0) };
+static const unsigned int eth_rxd1_pins[] = { PIN(GPIOZ_10, 0) };
+static const unsigned int eth_rxd0_pins[] = { PIN(GPIOZ_11, 0) };
+static const unsigned int eth_mdio_pins[] = { PIN(GPIOZ_12, 0) };
+static const unsigned int eth_mdc_pins[] = { PIN(GPIOZ_13, 0) };
+
+static const unsigned int i2c_sda_a0_pins[] = { PIN(GPIOZ_0, 0) };
+static const unsigned int i2c_sck_a0_pins[] = { PIN(GPIOZ_1, 0) };
+
+static const unsigned int i2c_sda_b_pins[] = { PIN(GPIOZ_2, 0) };
+static const unsigned int i2c_sck_b_pins[] = { PIN(GPIOZ_3, 0) };
+
+static const unsigned int i2c_sda_c1_pins[] = { PIN(GPIOZ_4, 0) };
+static const unsigned int i2c_sck_c1_pins[] = { PIN(GPIOZ_5, 0) };
+
+static const unsigned int i2c_sda_a1_pins[] = { PIN(GPIOZ_0, 0) };
+static const unsigned int i2c_sck_a1_pins[] = { PIN(GPIOZ_1, 0) };
+
+static const unsigned int i2c_sda_a2_pins[] = { PIN(GPIOZ_0, 0) };
+static const unsigned int i2c_sck_a2_pins[] = { PIN(GPIOZ_1, 0) };
 
 /* bank BOOT */
-static const unsigned int sd_d0_c_pins[] = { PIN_BOOT_0 };
-static const unsigned int sd_d1_c_pins[] = { PIN_BOOT_1 };
-static const unsigned int sd_d2_c_pins[] = { PIN_BOOT_2 };
-static const unsigned int sd_d3_c_pins[] = { PIN_BOOT_3 };
-static const unsigned int sd_cmd_c_pins[] = { PIN_BOOT_16 };
-static const unsigned int sd_clk_c_pins[] = { PIN_BOOT_17 };
-
-static const unsigned int sdxc_d0_c_pins[] = { PIN_BOOT_0};
-static const unsigned int sdxc_d13_c_pins[] = { PIN_BOOT_1, PIN_BOOT_2,
-						PIN_BOOT_3 };
-static const unsigned int sdxc_d47_c_pins[] = { PIN_BOOT_4, PIN_BOOT_5,
-						PIN_BOOT_6, PIN_BOOT_7 };
-static const unsigned int sdxc_cmd_c_pins[] = { PIN_BOOT_16 };
-static const unsigned int sdxc_clk_c_pins[] = { PIN_BOOT_17 };
-
-static const unsigned int nand_io_pins[] = { PIN_BOOT_0, PIN_BOOT_1,
-					     PIN_BOOT_2, PIN_BOOT_3,
-					     PIN_BOOT_4, PIN_BOOT_5,
-					     PIN_BOOT_6, PIN_BOOT_7 };
-static const unsigned int nand_io_ce0_pins[] = { PIN_BOOT_8 };
-static const unsigned int nand_io_ce1_pins[] = { PIN_BOOT_9 };
-static const unsigned int nand_io_rb0_pins[] = { PIN_BOOT_10 };
-static const unsigned int nand_ale_pins[] = { PIN_BOOT_11 };
-static const unsigned int nand_cle_pins[] = { PIN_BOOT_12 };
-static const unsigned int nand_wen_clk_pins[] = { PIN_BOOT_13 };
-static const unsigned int nand_ren_clk_pins[] = { PIN_BOOT_14 };
-static const unsigned int nand_dqs_pins[] = { PIN_BOOT_15 };
-static const unsigned int nand_ce2_pins[] = { PIN_BOOT_16 };
-static const unsigned int nand_ce3_pins[] = { PIN_BOOT_17 };
-
-static const unsigned int nor_d_pins[] = { PIN_BOOT_11 };
-static const unsigned int nor_q_pins[] = { PIN_BOOT_12 };
-static const unsigned int nor_c_pins[] = { PIN_BOOT_13 };
-static const unsigned int nor_cs_pins[] = { PIN_BOOT_18 };
+static const unsigned int sd_d0_c_pins[] = { PIN(BOOT_0, 0) };
+static const unsigned int sd_d1_c_pins[] = { PIN(BOOT_1, 0) };
+static const unsigned int sd_d2_c_pins[] = { PIN(BOOT_2, 0) };
+static const unsigned int sd_d3_c_pins[] = { PIN(BOOT_3, 0) };
+static const unsigned int sd_cmd_c_pins[] = { PIN(BOOT_16, 0) };
+static const unsigned int sd_clk_c_pins[] = { PIN(BOOT_17, 0) };
+
+static const unsigned int sdxc_d0_c_pins[] = { PIN(BOOT_0, 0)};
+static const unsigned int sdxc_d13_c_pins[] = { PIN(BOOT_1, 0), PIN(BOOT_2, 0),
+						PIN(BOOT_3, 0) };
+static const unsigned int sdxc_d47_c_pins[] = { PIN(BOOT_4, 0), PIN(BOOT_5, 0),
+						PIN(BOOT_6, 0), PIN(BOOT_7, 0) };
+static const unsigned int sdxc_cmd_c_pins[] = { PIN(BOOT_16, 0) };
+static const unsigned int sdxc_clk_c_pins[] = { PIN(BOOT_17, 0) };
+
+static const unsigned int nand_io_pins[] = { PIN(BOOT_0, 0), PIN(BOOT_1, 0),
+					     PIN(BOOT_2, 0), PIN(BOOT_3, 0),
+					     PIN(BOOT_4, 0), PIN(BOOT_5, 0),
+					     PIN(BOOT_6, 0), PIN(BOOT_7, 0) };
+static const unsigned int nand_io_ce0_pins[] = { PIN(BOOT_8, 0) };
+static const unsigned int nand_io_ce1_pins[] = { PIN(BOOT_9, 0) };
+static const unsigned int nand_io_rb0_pins[] = { PIN(BOOT_10, 0) };
+static const unsigned int nand_ale_pins[] = { PIN(BOOT_11, 0) };
+static const unsigned int nand_cle_pins[] = { PIN(BOOT_12, 0) };
+static const unsigned int nand_wen_clk_pins[] = { PIN(BOOT_13, 0) };
+static const unsigned int nand_ren_clk_pins[] = { PIN(BOOT_14, 0) };
+static const unsigned int nand_dqs_pins[] = { PIN(BOOT_15, 0) };
+static const unsigned int nand_ce2_pins[] = { PIN(BOOT_16, 0) };
+static const unsigned int nand_ce3_pins[] = { PIN(BOOT_17, 0) };
+
+static const unsigned int nor_d_pins[] = { PIN(BOOT_11, 0) };
+static const unsigned int nor_q_pins[] = { PIN(BOOT_12, 0) };
+static const unsigned int nor_c_pins[] = { PIN(BOOT_13, 0) };
+static const unsigned int nor_cs_pins[] = { PIN(BOOT_18, 0) };
 
 /* bank CARD */
-static const unsigned int sd_d1_b_pins[] = { PIN_CARD_0 };
-static const unsigned int sd_d0_b_pins[] = { PIN_CARD_1 };
-static const unsigned int sd_clk_b_pins[] = { PIN_CARD_2 };
-static const unsigned int sd_cmd_b_pins[] = { PIN_CARD_3 };
-static const unsigned int sd_d3_b_pins[] = { PIN_CARD_4 };
-static const unsigned int sd_d2_b_pins[] = { PIN_CARD_5 };
-
-static const unsigned int sdxc_d13_b_pins[] = { PIN_CARD_0, PIN_CARD_4,
-						PIN_CARD_5 };
-static const unsigned int sdxc_d0_b_pins[] = { PIN_CARD_1 };
-static const unsigned int sdxc_clk_b_pins[] = { PIN_CARD_2 };
-static const unsigned int sdxc_cmd_b_pins[] = { PIN_CARD_3 };
+static const unsigned int sd_d1_b_pins[] = { PIN(CARD_0, 0) };
+static const unsigned int sd_d0_b_pins[] = { PIN(CARD_1, 0) };
+static const unsigned int sd_clk_b_pins[] = { PIN(CARD_2, 0) };
+static const unsigned int sd_cmd_b_pins[] = { PIN(CARD_3, 0) };
+static const unsigned int sd_d3_b_pins[] = { PIN(CARD_4, 0) };
+static const unsigned int sd_d2_b_pins[] = { PIN(CARD_5, 0) };
+
+static const unsigned int sdxc_d13_b_pins[] = { PIN(CARD_0, 0), PIN(CARD_4, 0),
+						PIN(CARD_5, 0) };
+static const unsigned int sdxc_d0_b_pins[] = { PIN(CARD_1, 0) };
+static const unsigned int sdxc_clk_b_pins[] = { PIN(CARD_2, 0) };
+static const unsigned int sdxc_cmd_b_pins[] = { PIN(CARD_3, 0) };
 
 /* bank AO */
-static const unsigned int uart_tx_ao_a_pins[] = { PIN_GPIOAO_0 };
-static const unsigned int uart_rx_ao_a_pins[] = { PIN_GPIOAO_1 };
-static const unsigned int uart_cts_ao_a_pins[] = { PIN_GPIOAO_2 };
-static const unsigned int uart_rts_ao_a_pins[] = { PIN_GPIOAO_3 };
+static const unsigned int uart_tx_ao_a_pins[] = { PIN(GPIOAO_0, AO_OFF) };
+static const unsigned int uart_rx_ao_a_pins[] = { PIN(GPIOAO_1, AO_OFF) };
+static const unsigned int uart_cts_ao_a_pins[] = { PIN(GPIOAO_2, AO_OFF) };
+static const unsigned int uart_rts_ao_a_pins[] = { PIN(GPIOAO_3, AO_OFF) };
 
-static const unsigned int remote_input_pins[] = { PIN_GPIOAO_7 };
+static const unsigned int remote_input_pins[] = { PIN(GPIOAO_7, AO_OFF) };
 
-static const unsigned int i2c_slave_sck_ao_pins[] = { PIN_GPIOAO_4 };
-static const unsigned int i2c_slave_sda_ao_pins[] = { PIN_GPIOAO_5 };
+static const unsigned int i2c_slave_sck_ao_pins[] = { PIN(GPIOAO_4, AO_OFF) };
+static const unsigned int i2c_slave_sda_ao_pins[] = { PIN(GPIOAO_5, AO_OFF) };
 
-static const unsigned int uart_tx_ao_b0_pins[] = { PIN_GPIOAO_0 };
-static const unsigned int uart_rx_ao_b0_pins[] = { PIN_GPIOAO_1 };
+static const unsigned int uart_tx_ao_b0_pins[] = { PIN(GPIOAO_0, AO_OFF) };
+static const unsigned int uart_rx_ao_b0_pins[] = { PIN(GPIOAO_1, AO_OFF) };
 
-static const unsigned int uart_tx_ao_b1_pins[] = { PIN_GPIOAO_4 };
-static const unsigned int uart_rx_ao_b1_pins[] = { PIN_GPIOAO_5 };
+static const unsigned int uart_tx_ao_b1_pins[] = { PIN(GPIOAO_4, AO_OFF) };
+static const unsigned int uart_rx_ao_b1_pins[] = { PIN(GPIOAO_5, AO_OFF) };
 
-static const unsigned int i2c_mst_sck_ao_pins[] = { PIN_GPIOAO_4 };
-static const unsigned int i2c_mst_sda_ao_pins[] = { PIN_GPIOAO_5 };
+static const unsigned int i2c_mst_sck_ao_pins[] = { PIN(GPIOAO_4, AO_OFF) };
+static const unsigned int i2c_mst_sda_ao_pins[] = { PIN(GPIOAO_5, AO_OFF) };
 
 static struct meson_pmx_group meson8_groups[] = {
-	GPIO_GROUP(GPIOX_0),
-	GPIO_GROUP(GPIOX_1),
-	GPIO_GROUP(GPIOX_2),
-	GPIO_GROUP(GPIOX_3),
-	GPIO_GROUP(GPIOX_4),
-	GPIO_GROUP(GPIOX_5),
-	GPIO_GROUP(GPIOX_6),
-	GPIO_GROUP(GPIOX_7),
-	GPIO_GROUP(GPIOX_8),
-	GPIO_GROUP(GPIOX_9),
-	GPIO_GROUP(GPIOX_10),
-	GPIO_GROUP(GPIOX_11),
-	GPIO_GROUP(GPIOX_12),
-	GPIO_GROUP(GPIOX_13),
-	GPIO_GROUP(GPIOX_14),
-	GPIO_GROUP(GPIOX_15),
-	GPIO_GROUP(GPIOX_16),
-	GPIO_GROUP(GPIOX_17),
-	GPIO_GROUP(GPIOX_18),
-	GPIO_GROUP(GPIOX_19),
-	GPIO_GROUP(GPIOX_20),
-	GPIO_GROUP(GPIOX_21),
-	GPIO_GROUP(GPIOY_0),
-	GPIO_GROUP(GPIOY_1),
-	GPIO_GROUP(GPIOY_2),
-	GPIO_GROUP(GPIOY_3),
-	GPIO_GROUP(GPIOY_4),
-	GPIO_GROUP(GPIOY_5),
-	GPIO_GROUP(GPIOY_6),
-	GPIO_GROUP(GPIOY_7),
-	GPIO_GROUP(GPIOY_8),
-	GPIO_GROUP(GPIOY_9),
-	GPIO_GROUP(GPIOY_10),
-	GPIO_GROUP(GPIOY_11),
-	GPIO_GROUP(GPIOY_12),
-	GPIO_GROUP(GPIOY_13),
-	GPIO_GROUP(GPIOY_14),
-	GPIO_GROUP(GPIOY_15),
-	GPIO_GROUP(GPIOY_16),
-	GPIO_GROUP(GPIODV_0),
-	GPIO_GROUP(GPIODV_1),
-	GPIO_GROUP(GPIODV_2),
-	GPIO_GROUP(GPIODV_3),
-	GPIO_GROUP(GPIODV_4),
-	GPIO_GROUP(GPIODV_5),
-	GPIO_GROUP(GPIODV_6),
-	GPIO_GROUP(GPIODV_7),
-	GPIO_GROUP(GPIODV_8),
-	GPIO_GROUP(GPIODV_9),
-	GPIO_GROUP(GPIODV_10),
-	GPIO_GROUP(GPIODV_11),
-	GPIO_GROUP(GPIODV_12),
-	GPIO_GROUP(GPIODV_13),
-	GPIO_GROUP(GPIODV_14),
-	GPIO_GROUP(GPIODV_15),
-	GPIO_GROUP(GPIODV_16),
-	GPIO_GROUP(GPIODV_17),
-	GPIO_GROUP(GPIODV_18),
-	GPIO_GROUP(GPIODV_19),
-	GPIO_GROUP(GPIODV_20),
-	GPIO_GROUP(GPIODV_21),
-	GPIO_GROUP(GPIODV_22),
-	GPIO_GROUP(GPIODV_23),
-	GPIO_GROUP(GPIODV_24),
-	GPIO_GROUP(GPIODV_25),
-	GPIO_GROUP(GPIODV_26),
-	GPIO_GROUP(GPIODV_27),
-	GPIO_GROUP(GPIODV_28),
-	GPIO_GROUP(GPIODV_29),
-	GPIO_GROUP(GPIOH_0),
-	GPIO_GROUP(GPIOH_1),
-	GPIO_GROUP(GPIOH_2),
-	GPIO_GROUP(GPIOH_3),
-	GPIO_GROUP(GPIOH_4),
-	GPIO_GROUP(GPIOH_5),
-	GPIO_GROUP(GPIOH_6),
-	GPIO_GROUP(GPIOH_7),
-	GPIO_GROUP(GPIOH_8),
-	GPIO_GROUP(GPIOH_9),
-	GPIO_GROUP(GPIOZ_0),
-	GPIO_GROUP(GPIOZ_1),
-	GPIO_GROUP(GPIOZ_2),
-	GPIO_GROUP(GPIOZ_3),
-	GPIO_GROUP(GPIOZ_4),
-	GPIO_GROUP(GPIOZ_5),
-	GPIO_GROUP(GPIOZ_6),
-	GPIO_GROUP(GPIOZ_7),
-	GPIO_GROUP(GPIOZ_8),
-	GPIO_GROUP(GPIOZ_9),
-	GPIO_GROUP(GPIOZ_10),
-	GPIO_GROUP(GPIOZ_11),
-	GPIO_GROUP(GPIOZ_12),
-	GPIO_GROUP(GPIOZ_13),
-	GPIO_GROUP(GPIOZ_14),
-	GPIO_GROUP(GPIOAO_0),
-	GPIO_GROUP(GPIOAO_1),
-	GPIO_GROUP(GPIOAO_2),
-	GPIO_GROUP(GPIOAO_3),
-	GPIO_GROUP(GPIOAO_4),
-	GPIO_GROUP(GPIOAO_5),
-	GPIO_GROUP(GPIOAO_6),
-	GPIO_GROUP(GPIOAO_7),
-	GPIO_GROUP(GPIOAO_8),
-	GPIO_GROUP(GPIOAO_9),
-	GPIO_GROUP(GPIOAO_10),
-	GPIO_GROUP(GPIOAO_11),
-	GPIO_GROUP(GPIOAO_12),
-	GPIO_GROUP(GPIOAO_13),
-	GPIO_GROUP(GPIO_BSD_EN),
-	GPIO_GROUP(GPIO_TEST_N),
+	GPIO_GROUP(GPIOX_0, 0),
+	GPIO_GROUP(GPIOX_1, 0),
+	GPIO_GROUP(GPIOX_2, 0),
+	GPIO_GROUP(GPIOX_3, 0),
+	GPIO_GROUP(GPIOX_4, 0),
+	GPIO_GROUP(GPIOX_5, 0),
+	GPIO_GROUP(GPIOX_6, 0),
+	GPIO_GROUP(GPIOX_7, 0),
+	GPIO_GROUP(GPIOX_8, 0),
+	GPIO_GROUP(GPIOX_9, 0),
+	GPIO_GROUP(GPIOX_10, 0),
+	GPIO_GROUP(GPIOX_11, 0),
+	GPIO_GROUP(GPIOX_12, 0),
+	GPIO_GROUP(GPIOX_13, 0),
+	GPIO_GROUP(GPIOX_14, 0),
+	GPIO_GROUP(GPIOX_15, 0),
+	GPIO_GROUP(GPIOX_16, 0),
+	GPIO_GROUP(GPIOX_17, 0),
+	GPIO_GROUP(GPIOX_18, 0),
+	GPIO_GROUP(GPIOX_19, 0),
+	GPIO_GROUP(GPIOX_20, 0),
+	GPIO_GROUP(GPIOX_21, 0),
+	GPIO_GROUP(GPIOY_0, 0),
+	GPIO_GROUP(GPIOY_1, 0),
+	GPIO_GROUP(GPIOY_2, 0),
+	GPIO_GROUP(GPIOY_3, 0),
+	GPIO_GROUP(GPIOY_4, 0),
+	GPIO_GROUP(GPIOY_5, 0),
+	GPIO_GROUP(GPIOY_6, 0),
+	GPIO_GROUP(GPIOY_7, 0),
+	GPIO_GROUP(GPIOY_8, 0),
+	GPIO_GROUP(GPIOY_9, 0),
+	GPIO_GROUP(GPIOY_10, 0),
+	GPIO_GROUP(GPIOY_11, 0),
+	GPIO_GROUP(GPIOY_12, 0),
+	GPIO_GROUP(GPIOY_13, 0),
+	GPIO_GROUP(GPIOY_14, 0),
+	GPIO_GROUP(GPIOY_15, 0),
+	GPIO_GROUP(GPIOY_16, 0),
+	GPIO_GROUP(GPIODV_0, 0),
+	GPIO_GROUP(GPIODV_1, 0),
+	GPIO_GROUP(GPIODV_2, 0),
+	GPIO_GROUP(GPIODV_3, 0),
+	GPIO_GROUP(GPIODV_4, 0),
+	GPIO_GROUP(GPIODV_5, 0),
+	GPIO_GROUP(GPIODV_6, 0),
+	GPIO_GROUP(GPIODV_7, 0),
+	GPIO_GROUP(GPIODV_8, 0),
+	GPIO_GROUP(GPIODV_9, 0),
+	GPIO_GROUP(GPIODV_10, 0),
+	GPIO_GROUP(GPIODV_11, 0),
+	GPIO_GROUP(GPIODV_12, 0),
+	GPIO_GROUP(GPIODV_13, 0),
+	GPIO_GROUP(GPIODV_14, 0),
+	GPIO_GROUP(GPIODV_15, 0),
+	GPIO_GROUP(GPIODV_16, 0),
+	GPIO_GROUP(GPIODV_17, 0),
+	GPIO_GROUP(GPIODV_18, 0),
+	GPIO_GROUP(GPIODV_19, 0),
+	GPIO_GROUP(GPIODV_20, 0),
+	GPIO_GROUP(GPIODV_21, 0),
+	GPIO_GROUP(GPIODV_22, 0),
+	GPIO_GROUP(GPIODV_23, 0),
+	GPIO_GROUP(GPIODV_24, 0),
+	GPIO_GROUP(GPIODV_25, 0),
+	GPIO_GROUP(GPIODV_26, 0),
+	GPIO_GROUP(GPIODV_27, 0),
+	GPIO_GROUP(GPIODV_28, 0),
+	GPIO_GROUP(GPIODV_29, 0),
+	GPIO_GROUP(GPIOH_0, 0),
+	GPIO_GROUP(GPIOH_1, 0),
+	GPIO_GROUP(GPIOH_2, 0),
+	GPIO_GROUP(GPIOH_3, 0),
+	GPIO_GROUP(GPIOH_4, 0),
+	GPIO_GROUP(GPIOH_5, 0),
+	GPIO_GROUP(GPIOH_6, 0),
+	GPIO_GROUP(GPIOH_7, 0),
+	GPIO_GROUP(GPIOH_8, 0),
+	GPIO_GROUP(GPIOH_9, 0),
+	GPIO_GROUP(GPIOZ_0, 0),
+	GPIO_GROUP(GPIOZ_1, 0),
+	GPIO_GROUP(GPIOZ_2, 0),
+	GPIO_GROUP(GPIOZ_3, 0),
+	GPIO_GROUP(GPIOZ_4, 0),
+	GPIO_GROUP(GPIOZ_5, 0),
+	GPIO_GROUP(GPIOZ_6, 0),
+	GPIO_GROUP(GPIOZ_7, 0),
+	GPIO_GROUP(GPIOZ_8, 0),
+	GPIO_GROUP(GPIOZ_9, 0),
+	GPIO_GROUP(GPIOZ_10, 0),
+	GPIO_GROUP(GPIOZ_11, 0),
+	GPIO_GROUP(GPIOZ_12, 0),
+	GPIO_GROUP(GPIOZ_13, 0),
+	GPIO_GROUP(GPIOZ_14, 0),
+	GPIO_GROUP(GPIOAO_0, AO_OFF),
+	GPIO_GROUP(GPIOAO_1, AO_OFF),
+	GPIO_GROUP(GPIOAO_2, AO_OFF),
+	GPIO_GROUP(GPIOAO_3, AO_OFF),
+	GPIO_GROUP(GPIOAO_4, AO_OFF),
+	GPIO_GROUP(GPIOAO_5, AO_OFF),
+	GPIO_GROUP(GPIOAO_6, AO_OFF),
+	GPIO_GROUP(GPIOAO_7, AO_OFF),
+	GPIO_GROUP(GPIOAO_8, AO_OFF),
+	GPIO_GROUP(GPIOAO_9, AO_OFF),
+	GPIO_GROUP(GPIOAO_10, AO_OFF),
+	GPIO_GROUP(GPIOAO_11, AO_OFF),
+	GPIO_GROUP(GPIOAO_12, AO_OFF),
+	GPIO_GROUP(GPIOAO_13, AO_OFF),
+	GPIO_GROUP(GPIO_BSD_EN, AO_OFF),
+	GPIO_GROUP(GPIO_TEST_N, AO_OFF),
 
 	/* bank X */
 	GROUP(sd_d0_a,		8,	5),
@@ -1045,19 +907,19 @@ static struct meson_pmx_func meson8_functions[] = {
 };
 
 static struct meson_bank meson8_banks[] = {
-	/*   name    first         last             pullen  pull     dir     out     in  */
-	BANK("X",    PIN_GPIOX_0,  PIN_GPIOX_21,    4,  0,  4,  0,  0,  0,  1,  0,  2,  0),
-	BANK("Y",    PIN_GPIOY_0,  PIN_GPIOY_16,    3,  0,  3,  0,  3,  0,  4,  0,  5,  0),
-	BANK("DV",   PIN_GPIODV_0, PIN_GPIODV_29,   0,  0,  0,  0,  7,  0,  8,  0,  9,  0),
-	BANK("H",    PIN_GPIOH_0,  PIN_GPIOH_9,     1, 16,  1, 16,  9, 19, 10, 19, 11, 19),
-	BANK("Z",    PIN_GPIOZ_0,  PIN_GPIOZ_14,    1,  0,  1,  0,  3, 17,  4, 17,  5, 17),
-	BANK("CARD", PIN_CARD_0,   PIN_CARD_6,      2, 20,  2, 20,  0, 22,  1, 22,  2, 22),
-	BANK("BOOT", PIN_BOOT_0,   PIN_BOOT_18,     2,  0,  2,  0,  9,  0, 10,  0, 11,  0),
+	/*   name    first             last                 pullen  pull    dir     out     in  */
+	BANK("X",    PIN(GPIOX_0, 0),  PIN(GPIOX_21, 0),    4,  0,  4,  0,  0,  0,  1,  0,  2,  0),
+	BANK("Y",    PIN(GPIOY_0, 0),  PIN(GPIOY_16, 0),    3,  0,  3,  0,  3,  0,  4,  0,  5,  0),
+	BANK("DV",   PIN(GPIODV_0, 0), PIN(GPIODV_29, 0),   0,  0,  0,  0,  7,  0,  8,  0,  9,  0),
+	BANK("H",    PIN(GPIOH_0, 0),  PIN(GPIOH_9, 0),     1, 16,  1, 16,  9, 19, 10, 19, 11, 19),
+	BANK("Z",    PIN(GPIOZ_0, 0),  PIN(GPIOZ_14, 0),    1,  0,  1,  0,  3, 17,  4, 17,  5, 17),
+	BANK("CARD", PIN(CARD_0, 0),   PIN(CARD_6, 0),      2, 20,  2, 20,  0, 22,  1, 22,  2, 22),
+	BANK("BOOT", PIN(BOOT_0, 0),   PIN(BOOT_18, 0),     2,  0,  2,  0,  9,  0, 10,  0, 11,  0),
 };
 
 static struct meson_bank meson8_ao_banks[] = {
-	/*   name    first         last             pullen  pull     dir     out     in  */
-	BANK("AO",   PIN_GPIOAO_0, PIN_GPIO_TEST_N, 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
+	/*   name    first                  last                      pullen  pull    dir     out     in  */
+	BANK("AO",   PIN(GPIOAO_0, AO_OFF), PIN(GPIO_TEST_N, AO_OFF), 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
 };
 
 static struct meson_domain_data meson8_domain_data[] = {
diff --git a/drivers/pinctrl/meson/pinctrl-meson8b.c b/drivers/pinctrl/meson/pinctrl-meson8b.c
new file mode 100644
index 000000000000..2f7ea6229880
--- /dev/null
+++ b/drivers/pinctrl/meson/pinctrl-meson8b.c
@@ -0,0 +1,899 @@
+/*
+ * Pin controller and GPIO driver for Amlogic Meson8b.
+ *
+ * Copyright (C) 2015 Endless Mobile, Inc.
+ * Author: Carlo Caione <carlo@endlessm.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <dt-bindings/gpio/meson8b-gpio.h>
+#include "pinctrl-meson.h"
+
+#define AO_OFF	130
+
+static const struct pinctrl_pin_desc meson8b_pins[] = {
+	MESON_PIN(GPIOX_0, 0),
+	MESON_PIN(GPIOX_1, 0),
+	MESON_PIN(GPIOX_2, 0),
+	MESON_PIN(GPIOX_3, 0),
+	MESON_PIN(GPIOX_4, 0),
+	MESON_PIN(GPIOX_5, 0),
+	MESON_PIN(GPIOX_6, 0),
+	MESON_PIN(GPIOX_7, 0),
+	MESON_PIN(GPIOX_8, 0),
+	MESON_PIN(GPIOX_9, 0),
+	MESON_PIN(GPIOX_10, 0),
+	MESON_PIN(GPIOX_11, 0),
+	MESON_PIN(GPIOX_16, 0),
+	MESON_PIN(GPIOX_17, 0),
+	MESON_PIN(GPIOX_18, 0),
+	MESON_PIN(GPIOX_19, 0),
+	MESON_PIN(GPIOX_20, 0),
+	MESON_PIN(GPIOX_21, 0),
+
+	MESON_PIN(GPIOY_0, 0),
+	MESON_PIN(GPIOY_1, 0),
+	MESON_PIN(GPIOY_3, 0),
+	MESON_PIN(GPIOY_6, 0),
+	MESON_PIN(GPIOY_7, 0),
+	MESON_PIN(GPIOY_8, 0),
+	MESON_PIN(GPIOY_9, 0),
+	MESON_PIN(GPIOY_10, 0),
+	MESON_PIN(GPIOY_11, 0),
+	MESON_PIN(GPIOY_12, 0),
+	MESON_PIN(GPIOY_13, 0),
+	MESON_PIN(GPIOY_14, 0),
+
+	MESON_PIN(GPIODV_9, 0),
+	MESON_PIN(GPIODV_24, 0),
+	MESON_PIN(GPIODV_25, 0),
+	MESON_PIN(GPIODV_26, 0),
+	MESON_PIN(GPIODV_27, 0),
+	MESON_PIN(GPIODV_28, 0),
+	MESON_PIN(GPIODV_29, 0),
+
+	MESON_PIN(GPIOH_0, 0),
+	MESON_PIN(GPIOH_1, 0),
+	MESON_PIN(GPIOH_2, 0),
+	MESON_PIN(GPIOH_3, 0),
+	MESON_PIN(GPIOH_4, 0),
+	MESON_PIN(GPIOH_5, 0),
+	MESON_PIN(GPIOH_6, 0),
+	MESON_PIN(GPIOH_7, 0),
+	MESON_PIN(GPIOH_8, 0),
+	MESON_PIN(GPIOH_9, 0),
+
+	MESON_PIN(CARD_0, 0),
+	MESON_PIN(CARD_1, 0),
+	MESON_PIN(CARD_2, 0),
+	MESON_PIN(CARD_3, 0),
+	MESON_PIN(CARD_4, 0),
+	MESON_PIN(CARD_5, 0),
+	MESON_PIN(CARD_6, 0),
+
+	MESON_PIN(BOOT_0, 0),
+	MESON_PIN(BOOT_1, 0),
+	MESON_PIN(BOOT_2, 0),
+	MESON_PIN(BOOT_3, 0),
+	MESON_PIN(BOOT_4, 0),
+	MESON_PIN(BOOT_5, 0),
+	MESON_PIN(BOOT_6, 0),
+	MESON_PIN(BOOT_7, 0),
+	MESON_PIN(BOOT_8, 0),
+	MESON_PIN(BOOT_9, 0),
+	MESON_PIN(BOOT_10, 0),
+	MESON_PIN(BOOT_11, 0),
+	MESON_PIN(BOOT_12, 0),
+	MESON_PIN(BOOT_13, 0),
+	MESON_PIN(BOOT_14, 0),
+	MESON_PIN(BOOT_15, 0),
+	MESON_PIN(BOOT_16, 0),
+	MESON_PIN(BOOT_17, 0),
+	MESON_PIN(BOOT_18, 0),
+
+	MESON_PIN(DIF_0_P, 0),
+	MESON_PIN(DIF_0_N, 0),
+	MESON_PIN(DIF_1_P, 0),
+	MESON_PIN(DIF_1_N, 0),
+	MESON_PIN(DIF_2_P, 0),
+	MESON_PIN(DIF_2_N, 0),
+	MESON_PIN(DIF_3_P, 0),
+	MESON_PIN(DIF_3_N, 0),
+	MESON_PIN(DIF_4_P, 0),
+	MESON_PIN(DIF_4_N, 0),
+
+	MESON_PIN(GPIOAO_0, AO_OFF),
+	MESON_PIN(GPIOAO_1, AO_OFF),
+	MESON_PIN(GPIOAO_2, AO_OFF),
+	MESON_PIN(GPIOAO_3, AO_OFF),
+	MESON_PIN(GPIOAO_4, AO_OFF),
+	MESON_PIN(GPIOAO_5, AO_OFF),
+	MESON_PIN(GPIOAO_6, AO_OFF),
+	MESON_PIN(GPIOAO_7, AO_OFF),
+	MESON_PIN(GPIOAO_8, AO_OFF),
+	MESON_PIN(GPIOAO_9, AO_OFF),
+	MESON_PIN(GPIOAO_10, AO_OFF),
+	MESON_PIN(GPIOAO_11, AO_OFF),
+	MESON_PIN(GPIOAO_12, AO_OFF),
+	MESON_PIN(GPIOAO_13, AO_OFF),
+	MESON_PIN(GPIO_BSD_EN, AO_OFF),
+	MESON_PIN(GPIO_TEST_N, AO_OFF),
+};
+
+/* bank X */
+static const unsigned int sd_d0_a_pins[]	= { PIN(GPIOX_0, 0) };
+static const unsigned int sd_d1_a_pins[]	= { PIN(GPIOX_1, 0) };
+static const unsigned int sd_d2_a_pins[]	= { PIN(GPIOX_2, 0) };
+static const unsigned int sd_d3_a_pins[]	= { PIN(GPIOX_3, 0) };
+static const unsigned int sdxc_d0_0_a_pins[]	= { PIN(GPIOX_4, 0) };
+static const unsigned int sdxc_d47_a_pins[]	= { PIN(GPIOX_4, 0), PIN(GPIOX_5, 0),
+						    PIN(GPIOX_6, 0), PIN(GPIOX_7, 0) };
+static const unsigned int sdxc_d13_0_a_pins[]	= { PIN(GPIOX_5, 0), PIN(GPIOX_6, 0),
+						    PIN(GPIOX_7, 0) };
+static const unsigned int sd_clk_a_pins[]	= { PIN(GPIOX_8, 0) };
+static const unsigned int sd_cmd_a_pins[]	= { PIN(GPIOX_9, 0) };
+static const unsigned int xtal_32k_out_pins[]	= { PIN(GPIOX_10, 0) };
+static const unsigned int xtal_24m_out_pins[]	= { PIN(GPIOX_11, 0) };
+static const unsigned int uart_tx_b0_pins[]	= { PIN(GPIOX_16, 0) };
+static const unsigned int uart_rx_b0_pins[]	= { PIN(GPIOX_17, 0) };
+static const unsigned int uart_cts_b0_pins[]	= { PIN(GPIOX_18, 0) };
+static const unsigned int uart_rts_b0_pins[]	= { PIN(GPIOX_19, 0) };
+
+static const unsigned int sdxc_d0_1_a_pins[]	= { PIN(GPIOX_0, 0) };
+static const unsigned int sdxc_d13_1_a_pins[]	= { PIN(GPIOX_1, 0), PIN(GPIOX_2, 0),
+						    PIN(GPIOX_3, 0) };
+static const unsigned int pcm_out_a_pins[]	= { PIN(GPIOX_4, 0) };
+static const unsigned int pcm_in_a_pins[]	= { PIN(GPIOX_5, 0) };
+static const unsigned int pcm_fs_a_pins[]	= { PIN(GPIOX_6, 0) };
+static const unsigned int pcm_clk_a_pins[]	= { PIN(GPIOX_7, 0) };
+static const unsigned int sdxc_clk_a_pins[]	= { PIN(GPIOX_8, 0) };
+static const unsigned int sdxc_cmd_a_pins[]	= { PIN(GPIOX_9, 0) };
+static const unsigned int pwm_vs_0_pins[]	= { PIN(GPIOX_10, 0) };
+static const unsigned int pwm_e_pins[]		= { PIN(GPIOX_10, 0) };
+static const unsigned int pwm_vs_1_pins[]	= { PIN(GPIOX_11, 0) };
+
+static const unsigned int uart_tx_a_pins[]	= { PIN(GPIOX_4, 0) };
+static const unsigned int uart_rx_a_pins[]	= { PIN(GPIOX_5, 0) };
+static const unsigned int uart_cts_a_pins[]	= { PIN(GPIOX_6, 0) };
+static const unsigned int uart_rts_a_pins[]	= { PIN(GPIOX_7, 0) };
+static const unsigned int uart_tx_b1_pins[]	= { PIN(GPIOX_8, 0) };
+static const unsigned int uart_rx_b1_pins[]	= { PIN(GPIOX_9, 0) };
+static const unsigned int uart_cts_b1_pins[]	= { PIN(GPIOX_10, 0) };
+static const unsigned int uart_rts_b1_pins[]	= { PIN(GPIOX_20, 0) };
+
+static const unsigned int iso7816_0_clk_pins[]	= { PIN(GPIOX_6, 0) };
+static const unsigned int iso7816_0_data_pins[]	= { PIN(GPIOX_7, 0) };
+static const unsigned int spi_sclk_0_pins[]	= { PIN(GPIOX_8, 0) };
+static const unsigned int spi_miso_0_pins[]	= { PIN(GPIOX_9, 0) };
+static const unsigned int spi_mosi_0_pins[]	= { PIN(GPIOX_10, 0) };
+static const unsigned int iso7816_det_pins[]	= { PIN(GPIOX_16, 0) };
+static const unsigned int iso7816_reset_pins[]	= { PIN(GPIOX_17, 0) };
+static const unsigned int iso7816_1_clk_pins[]	= { PIN(GPIOX_18, 0) };
+static const unsigned int iso7816_1_data_pins[]	= { PIN(GPIOX_19, 0) };
+static const unsigned int spi_ss0_0_pins[]	= { PIN(GPIOX_20, 0) };
+
+static const unsigned int tsin_clk_b_pins[]	= { PIN(GPIOX_8, 0) };
+static const unsigned int tsin_sop_b_pins[]	= { PIN(GPIOX_9, 0) };
+static const unsigned int tsin_d0_b_pins[]	= { PIN(GPIOX_10, 0) };
+static const unsigned int pwm_b_pins[]		= { PIN(GPIOX_11, 0) };
+static const unsigned int i2c_sda_d0_pins[]	= { PIN(GPIOX_16, 0) };
+static const unsigned int i2c_sck_d0_pins[]	= { PIN(GPIOX_17, 0) };
+static const unsigned int tsin_d_valid_b_pins[] = { PIN(GPIOX_20, 0) };
+
+/* bank Y */
+static const unsigned int tsin_d_valid_a_pins[] = { PIN(GPIOY_0, 0) };
+static const unsigned int tsin_sop_a_pins[]	= { PIN(GPIOY_1, 0) };
+static const unsigned int tsin_d17_a_pins[]	= { PIN(GPIOY_6, 0), PIN(GPIOY_7, 0),
+						    PIN(GPIOY_10, 0), PIN(GPIOY_11, 0),
+						    PIN(GPIOY_12, 0), PIN(GPIOY_13, 0),
+						    PIN(GPIOY_14, 0) };
+static const unsigned int tsin_clk_a_pins[]	= { PIN(GPIOY_8, 0) };
+static const unsigned int tsin_d0_a_pins[]	= { PIN(GPIOY_9, 0) };
+
+static const unsigned int spdif_out_0_pins[]	= { PIN(GPIOY_3, 0) };
+
+static const unsigned int xtal_24m_pins[]	= { PIN(GPIOY_3, 0) };
+static const unsigned int iso7816_2_clk_pins[]	= { PIN(GPIOY_13, 0) };
+static const unsigned int iso7816_2_data_pins[] = { PIN(GPIOY_14, 0) };
+
+/* bank DV */
+static const unsigned int pwm_d_pins[]		= { PIN(GPIODV_28, 0) };
+static const unsigned int pwm_c0_pins[]		= { PIN(GPIODV_29, 0) };
+
+static const unsigned int pwm_vs_2_pins[]	= { PIN(GPIODV_9, 0) };
+static const unsigned int pwm_vs_3_pins[]	= { PIN(GPIODV_28, 0) };
+static const unsigned int pwm_vs_4_pins[]	= { PIN(GPIODV_29, 0) };
+
+static const unsigned int xtal24_out_pins[]	= { PIN(GPIODV_29, 0) };
+
+static const unsigned int uart_tx_c_pins[]	= { PIN(GPIODV_24, 0) };
+static const unsigned int uart_rx_c_pins[]	= { PIN(GPIODV_25, 0) };
+static const unsigned int uart_cts_c_pins[]	= { PIN(GPIODV_26, 0) };
+static const unsigned int uart_rts_c_pins[]	= { PIN(GPIODV_27, 0) };
+
+static const unsigned int pwm_c1_pins[]		= { PIN(GPIODV_9, 0) };
+
+static const unsigned int i2c_sda_a_pins[]	= { PIN(GPIODV_24, 0) };
+static const unsigned int i2c_sck_a_pins[]	= { PIN(GPIODV_25, 0) };
+static const unsigned int i2c_sda_b0_pins[]	= { PIN(GPIODV_26, 0) };
+static const unsigned int i2c_sck_b0_pins[]	= { PIN(GPIODV_27, 0) };
+static const unsigned int i2c_sda_c0_pins[]	= { PIN(GPIODV_28, 0) };
+static const unsigned int i2c_sck_c0_pins[]	= { PIN(GPIODV_29, 0) };
+
+/* bank H */
+static const unsigned int hdmi_hpd_pins[]	= { PIN(GPIOH_0, 0) };
+static const unsigned int hdmi_sda_pins[]	= { PIN(GPIOH_1, 0) };
+static const unsigned int hdmi_scl_pins[]	= { PIN(GPIOH_2, 0) };
+static const unsigned int hdmi_cec_0_pins[]	= { PIN(GPIOH_3, 0) };
+static const unsigned int eth_txd1_0_pins[]	= { PIN(GPIOH_5, 0) };
+static const unsigned int eth_txd0_0_pins[]	= { PIN(GPIOH_6, 0) };
+static const unsigned int clk_24m_out_pins[]	= { PIN(GPIOH_9, 0) };
+
+static const unsigned int spi_ss1_pins[]	= { PIN(GPIOH_0, 0) };
+static const unsigned int spi_ss2_pins[]	= { PIN(GPIOH_1, 0) };
+static const unsigned int spi_ss0_1_pins[]	= { PIN(GPIOH_3, 0) };
+static const unsigned int spi_miso_1_pins[]	= { PIN(GPIOH_4, 0) };
+static const unsigned int spi_mosi_1_pins[]	= { PIN(GPIOH_5, 0) };
+static const unsigned int spi_sclk_1_pins[]	= { PIN(GPIOH_6, 0) };
+
+static const unsigned int eth_txd3_pins[]	= { PIN(GPIOH_7, 0) };
+static const unsigned int eth_txd2_pins[]	= { PIN(GPIOH_8, 0) };
+static const unsigned int eth_tx_clk_pins[]	= { PIN(GPIOH_9, 0) };
+
+static const unsigned int i2c_sda_b1_pins[]	= { PIN(GPIOH_3, 0) };
+static const unsigned int i2c_sck_b1_pins[]	= { PIN(GPIOH_4, 0) };
+static const unsigned int i2c_sda_c1_pins[]	= { PIN(GPIOH_5, 0) };
+static const unsigned int i2c_sck_c1_pins[]	= { PIN(GPIOH_6, 0) };
+static const unsigned int i2c_sda_d1_pins[]	= { PIN(GPIOH_7, 0) };
+static const unsigned int i2c_sck_d1_pins[]	= { PIN(GPIOH_8, 0) };
+
+/* bank BOOT */
+static const unsigned int nand_io_pins[]	= { PIN(BOOT_0, 0), PIN(BOOT_1, 0),
+						    PIN(BOOT_2, 0), PIN(BOOT_3, 0),
+						    PIN(BOOT_4, 0), PIN(BOOT_5, 0),
+						    PIN(BOOT_6, 0), PIN(BOOT_7, 0) };
+static const unsigned int nand_io_ce0_pins[]	= { PIN(BOOT_8, 0) };
+static const unsigned int nand_io_ce1_pins[]	= { PIN(BOOT_9, 0) };
+static const unsigned int nand_io_rb0_pins[]	= { PIN(BOOT_10, 0) };
+static const unsigned int nand_ale_pins[]	= { PIN(BOOT_11, 0) };
+static const unsigned int nand_cle_pins[]	= { PIN(BOOT_12, 0) };
+static const unsigned int nand_wen_clk_pins[]	= { PIN(BOOT_13, 0) };
+static const unsigned int nand_ren_clk_pins[]	= { PIN(BOOT_14, 0) };
+static const unsigned int nand_dqs_0_pins[]	= { PIN(BOOT_15, 0) };
+static const unsigned int nand_dqs_1_pins[]	= { PIN(BOOT_18, 0) };
+
+static const unsigned int sdxc_d0_c_pins[]	= { PIN(BOOT_0, 0)};
+static const unsigned int sdxc_d13_c_pins[]	= { PIN(BOOT_1, 0), PIN(BOOT_2, 0),
+						    PIN(BOOT_3, 0) };
+static const unsigned int sdxc_d47_c_pins[]	= { PIN(BOOT_4, 0), PIN(BOOT_5, 0),
+						    PIN(BOOT_6, 0), PIN(BOOT_7, 0) };
+static const unsigned int sdxc_clk_c_pins[]	= { PIN(BOOT_8, 0) };
+static const unsigned int sdxc_cmd_c_pins[]	= { PIN(BOOT_10, 0) };
+static const unsigned int nor_d_pins[]		= { PIN(BOOT_11, 0) };
+static const unsigned int nor_q_pins[]		= { PIN(BOOT_12, 0) };
+static const unsigned int nor_c_pins[]		= { PIN(BOOT_13, 0) };
+static const unsigned int nor_cs_pins[]		= { PIN(BOOT_18, 0) };
+
+static const unsigned int sd_d0_c_pins[]	= { PIN(BOOT_0, 0) };
+static const unsigned int sd_d1_c_pins[]	= { PIN(BOOT_1, 0) };
+static const unsigned int sd_d2_c_pins[]	= { PIN(BOOT_2, 0) };
+static const unsigned int sd_d3_c_pins[]	= { PIN(BOOT_3, 0) };
+static const unsigned int sd_cmd_c_pins[]	= { PIN(BOOT_8, 0) };
+static const unsigned int sd_clk_c_pins[]	= { PIN(BOOT_10, 0) };
+
+/* bank CARD */
+static const unsigned int sd_d1_b_pins[]	= { PIN(CARD_0, 0) };
+static const unsigned int sd_d0_b_pins[]	= { PIN(CARD_1, 0) };
+static const unsigned int sd_clk_b_pins[]	= { PIN(CARD_2, 0) };
+static const unsigned int sd_cmd_b_pins[]	= { PIN(CARD_3, 0) };
+static const unsigned int sd_d3_b_pins[]	= { PIN(CARD_4, 0) };
+static const unsigned int sd_d2_b_pins[]	= { PIN(CARD_5, 0) };
+
+static const unsigned int sdxc_d13_b_pins[]	= { PIN(CARD_0, 0), PIN(CARD_4, 0),
+						    PIN(CARD_5, 0) };
+static const unsigned int sdxc_d0_b_pins[]	= { PIN(CARD_1, 0) };
+static const unsigned int sdxc_clk_b_pins[]	= { PIN(CARD_2, 0) };
+static const unsigned int sdxc_cmd_b_pins[]	= { PIN(CARD_3, 0) };
+
+/* bank AO */
+static const unsigned int uart_tx_ao_a_pins[]	= { PIN(GPIOAO_0, AO_OFF) };
+static const unsigned int uart_rx_ao_a_pins[]	= { PIN(GPIOAO_1, AO_OFF) };
+static const unsigned int uart_cts_ao_a_pins[]	= { PIN(GPIOAO_2, AO_OFF) };
+static const unsigned int uart_rts_ao_a_pins[]	= { PIN(GPIOAO_3, AO_OFF) };
+static const unsigned int i2c_mst_sck_ao_pins[] = { PIN(GPIOAO_4, AO_OFF) };
+static const unsigned int i2c_mst_sda_ao_pins[] = { PIN(GPIOAO_5, AO_OFF) };
+static const unsigned int clk_32k_in_out_pins[]	= { PIN(GPIOAO_6, AO_OFF) };
+static const unsigned int remote_input_pins[]	= { PIN(GPIOAO_7, AO_OFF) };
+static const unsigned int hdmi_cec_1_pins[]	= { PIN(GPIOAO_12, AO_OFF) };
+static const unsigned int ir_blaster_pins[]	= { PIN(GPIOAO_13, AO_OFF) };
+
+static const unsigned int pwm_c2_pins[]		= { PIN(GPIOAO_3, AO_OFF) };
+static const unsigned int i2c_sck_ao_pins[]	= { PIN(GPIOAO_4, AO_OFF) };
+static const unsigned int i2c_sda_ao_pins[]	= { PIN(GPIOAO_5, AO_OFF) };
+static const unsigned int ir_remote_out_pins[]	= { PIN(GPIOAO_7, AO_OFF) };
+static const unsigned int i2s_am_clk_out_pins[]	= { PIN(GPIOAO_8, AO_OFF) };
+static const unsigned int i2s_ao_clk_out_pins[]	= { PIN(GPIOAO_9, AO_OFF) };
+static const unsigned int i2s_lr_clk_out_pins[]	= { PIN(GPIOAO_10, AO_OFF) };
+static const unsigned int i2s_out_01_pins[]	= { PIN(GPIOAO_11, AO_OFF) };
+
+static const unsigned int uart_tx_ao_b0_pins[]	= { PIN(GPIOAO_0, AO_OFF) };
+static const unsigned int uart_rx_ao_b0_pins[]	= { PIN(GPIOAO_1, AO_OFF) };
+static const unsigned int uart_cts_ao_b_pins[]	= { PIN(GPIOAO_2, AO_OFF) };
+static const unsigned int uart_rts_ao_b_pins[]	= { PIN(GPIOAO_3, AO_OFF) };
+static const unsigned int uart_tx_ao_b1_pins[]	= { PIN(GPIOAO_4, AO_OFF) };
+static const unsigned int uart_rx_ao_b1_pins[]	= { PIN(GPIOAO_5, AO_OFF) };
+static const unsigned int spdif_out_1_pins[]	= { PIN(GPIOAO_6, AO_OFF) };
+
+static const unsigned int i2s_in_ch01_pins[]	= { PIN(GPIOAO_6, AO_OFF) };
+static const unsigned int i2s_ao_clk_in_pins[]	= { PIN(GPIOAO_9, AO_OFF) };
+static const unsigned int i2s_lr_clk_in_pins[]	= { PIN(GPIOAO_10, AO_OFF) };
+
+/* bank DIF */
+static const unsigned int eth_rxd1_pins[]	= { PIN(DIF_0_P, 0) };
+static const unsigned int eth_rxd0_pins[]	= { PIN(DIF_0_N, 0) };
+static const unsigned int eth_rx_dv_pins[]	= { PIN(DIF_1_P, 0) };
+static const unsigned int eth_rx_clk_pins[]	= { PIN(DIF_1_N, 0) };
+static const unsigned int eth_txd0_1_pins[]	= { PIN(DIF_2_P, 0) };
+static const unsigned int eth_txd1_1_pins[]	= { PIN(DIF_2_N, 0) };
+static const unsigned int eth_tx_en_pins[]	= { PIN(DIF_3_P, 0) };
+static const unsigned int eth_ref_clk_pins[]	= { PIN(DIF_3_N, 0) };
+static const unsigned int eth_mdc_pins[]	= { PIN(DIF_4_P, 0) };
+static const unsigned int eth_mdio_en_pins[]	= { PIN(DIF_4_N, 0) };
+
+static struct meson_pmx_group meson8b_groups[] = {
+	GPIO_GROUP(GPIOX_0, 0),
+	GPIO_GROUP(GPIOX_1, 0),
+	GPIO_GROUP(GPIOX_2, 0),
+	GPIO_GROUP(GPIOX_3, 0),
+	GPIO_GROUP(GPIOX_4, 0),
+	GPIO_GROUP(GPIOX_5, 0),
+	GPIO_GROUP(GPIOX_6, 0),
+	GPIO_GROUP(GPIOX_7, 0),
+	GPIO_GROUP(GPIOX_8, 0),
+	GPIO_GROUP(GPIOX_9, 0),
+	GPIO_GROUP(GPIOX_10, 0),
+	GPIO_GROUP(GPIOX_11, 0),
+	GPIO_GROUP(GPIOX_16, 0),
+	GPIO_GROUP(GPIOX_17, 0),
+	GPIO_GROUP(GPIOX_18, 0),
+	GPIO_GROUP(GPIOX_19, 0),
+	GPIO_GROUP(GPIOX_20, 0),
+	GPIO_GROUP(GPIOX_21, 0),
+
+	GPIO_GROUP(GPIOY_0, 0),
+	GPIO_GROUP(GPIOY_1, 0),
+	GPIO_GROUP(GPIOY_3, 0),
+	GPIO_GROUP(GPIOY_6, 0),
+	GPIO_GROUP(GPIOY_7, 0),
+	GPIO_GROUP(GPIOY_8, 0),
+	GPIO_GROUP(GPIOY_9, 0),
+	GPIO_GROUP(GPIOY_10, 0),
+	GPIO_GROUP(GPIOY_11, 0),
+	GPIO_GROUP(GPIOY_12, 0),
+	GPIO_GROUP(GPIOY_13, 0),
+	GPIO_GROUP(GPIOY_14, 0),
+
+	GPIO_GROUP(GPIODV_9, 0),
+	GPIO_GROUP(GPIODV_24, 0),
+	GPIO_GROUP(GPIODV_25, 0),
+	GPIO_GROUP(GPIODV_26, 0),
+	GPIO_GROUP(GPIODV_27, 0),
+	GPIO_GROUP(GPIODV_28, 0),
+	GPIO_GROUP(GPIODV_29, 0),
+
+	GPIO_GROUP(GPIOH_0, 0),
+	GPIO_GROUP(GPIOH_1, 0),
+	GPIO_GROUP(GPIOH_2, 0),
+	GPIO_GROUP(GPIOH_3, 0),
+	GPIO_GROUP(GPIOH_4, 0),
+	GPIO_GROUP(GPIOH_5, 0),
+	GPIO_GROUP(GPIOH_6, 0),
+	GPIO_GROUP(GPIOH_7, 0),
+	GPIO_GROUP(GPIOH_8, 0),
+	GPIO_GROUP(GPIOH_9, 0),
+
+	GPIO_GROUP(DIF_0_P, 0),
+	GPIO_GROUP(DIF_0_N, 0),
+	GPIO_GROUP(DIF_1_P, 0),
+	GPIO_GROUP(DIF_1_N, 0),
+	GPIO_GROUP(DIF_2_P, 0),
+	GPIO_GROUP(DIF_2_N, 0),
+	GPIO_GROUP(DIF_3_P, 0),
+	GPIO_GROUP(DIF_3_N, 0),
+	GPIO_GROUP(DIF_4_P, 0),
+	GPIO_GROUP(DIF_4_N, 0),
+
+	GPIO_GROUP(GPIOAO_0, AO_OFF),
+	GPIO_GROUP(GPIOAO_1, AO_OFF),
+	GPIO_GROUP(GPIOAO_2, AO_OFF),
+	GPIO_GROUP(GPIOAO_3, AO_OFF),
+	GPIO_GROUP(GPIOAO_4, AO_OFF),
+	GPIO_GROUP(GPIOAO_5, AO_OFF),
+	GPIO_GROUP(GPIOAO_6, AO_OFF),
+	GPIO_GROUP(GPIOAO_7, AO_OFF),
+	GPIO_GROUP(GPIOAO_8, AO_OFF),
+	GPIO_GROUP(GPIOAO_9, AO_OFF),
+	GPIO_GROUP(GPIOAO_10, AO_OFF),
+	GPIO_GROUP(GPIOAO_11, AO_OFF),
+	GPIO_GROUP(GPIOAO_12, AO_OFF),
+	GPIO_GROUP(GPIOAO_13, AO_OFF),
+	GPIO_GROUP(GPIO_BSD_EN, AO_OFF),
+	GPIO_GROUP(GPIO_TEST_N, AO_OFF),
+
+	/* bank X */
+	GROUP(sd_d0_a,		8,	5),
+	GROUP(sd_d1_a,		8,	4),
+	GROUP(sd_d2_a,		8,	3),
+	GROUP(sd_d3_a,		8,	2),
+	GROUP(sdxc_d0_0_a,	5,	29),
+	GROUP(sdxc_d47_a,	5,	12),
+	GROUP(sdxc_d13_0_a,	5,	28),
+	GROUP(sd_clk_a,		8,	1),
+	GROUP(sd_cmd_a,		8,	0),
+	GROUP(xtal_32k_out,	3,	22),
+	GROUP(xtal_24m_out,	3,	20),
+	GROUP(uart_tx_b0,	4,	9),
+	GROUP(uart_rx_b0,	4,	8),
+	GROUP(uart_cts_b0,	4,	7),
+	GROUP(uart_rts_b0,	4,	6),
+	GROUP(sdxc_d0_1_a,	5,	14),
+	GROUP(sdxc_d13_1_a,	5,	13),
+	GROUP(pcm_out_a,	3,	30),
+	GROUP(pcm_in_a,		3,	29),
+	GROUP(pcm_fs_a,		3,	28),
+	GROUP(pcm_clk_a,	3,	27),
+	GROUP(sdxc_clk_a,	5,	11),
+	GROUP(sdxc_cmd_a,	5,	10),
+	GROUP(pwm_vs_0,		7,	31),
+	GROUP(pwm_e,		9,	19),
+	GROUP(pwm_vs_1,		7,	30),
+	GROUP(uart_tx_a,	4,	17),
+	GROUP(uart_rx_a,	4,	16),
+	GROUP(uart_cts_a,	4,	15),
+	GROUP(uart_rts_a,	4,	14),
+	GROUP(uart_tx_b1,	6,	19),
+	GROUP(uart_rx_b1,	6,	18),
+	GROUP(uart_cts_b1,	6,	17),
+	GROUP(uart_rts_b1,	6,	16),
+	GROUP(iso7816_0_clk,	5,	9),
+	GROUP(iso7816_0_data,	5,	8),
+	GROUP(spi_sclk_0,	4,	22),
+	GROUP(spi_miso_0,	4,	24),
+	GROUP(spi_mosi_0,	4,	23),
+	GROUP(iso7816_det,	4,	21),
+	GROUP(iso7816_reset,	4,	20),
+	GROUP(iso7816_1_clk,	4,	19),
+	GROUP(iso7816_1_data,	4,	18),
+	GROUP(spi_ss0_0,	4,	25),
+	GROUP(tsin_clk_b,	3,	6),
+	GROUP(tsin_sop_b,	3,	7),
+	GROUP(tsin_d0_b,	3,	8),
+	GROUP(pwm_b,		2,	3),
+	GROUP(i2c_sda_d0,	4,	5),
+	GROUP(i2c_sck_d0,	4,	4),
+	GROUP(tsin_d_valid_b,	3,	9),
+
+	/* bank Y */
+	GROUP(tsin_d_valid_a,	3,	2),
+	GROUP(tsin_sop_a,	3,	1),
+	GROUP(tsin_d17_a,	3,	5),
+	GROUP(tsin_clk_a,	3,	0),
+	GROUP(tsin_d0_a,	3,	4),
+	GROUP(spdif_out_0,	1,	7),
+	GROUP(xtal_24m,		3,	18),
+	GROUP(iso7816_2_clk,	5,	7),
+	GROUP(iso7816_2_data,	5,	6),
+
+	/* bank DV */
+	GROUP(pwm_d,		3,	26),
+	GROUP(pwm_c0,		3,	25),
+	GROUP(pwm_vs_2,		7,	28),
+	GROUP(pwm_vs_3,		7,	27),
+	GROUP(pwm_vs_4,		7,	26),
+	GROUP(xtal24_out,	7,	25),
+	GROUP(uart_tx_c,	6,	23),
+	GROUP(uart_rx_c,	6,	22),
+	GROUP(uart_cts_c,	6,	21),
+	GROUP(uart_rts_c,	6,	20),
+	GROUP(pwm_c1,		3,	24),
+	GROUP(i2c_sda_a,	9,	31),
+	GROUP(i2c_sck_a,	9,	30),
+	GROUP(i2c_sda_b0,	9,	29),
+	GROUP(i2c_sck_b0,	9,	28),
+	GROUP(i2c_sda_c0,	9,	27),
+	GROUP(i2c_sck_c0,	9,	26),
+
+	/* bank H */
+	GROUP(hdmi_hpd,		1,	26),
+	GROUP(hdmi_sda,		1,	25),
+	GROUP(hdmi_scl,		1,	24),
+	GROUP(hdmi_cec_0,	1,	23),
+	GROUP(eth_txd1_0,	7,	21),
+	GROUP(eth_txd0_0,	7,	20),
+	GROUP(clk_24m_out,	4,	1),
+	GROUP(spi_ss1,		8,	11),
+	GROUP(spi_ss2,		8,	12),
+	GROUP(spi_ss0_1,	9,	13),
+	GROUP(spi_miso_1,	9,	12),
+	GROUP(spi_mosi_1,	9,	11),
+	GROUP(spi_sclk_1,	9,	10),
+	GROUP(eth_txd3,		6,	13),
+	GROUP(eth_txd2,		6,	12),
+	GROUP(eth_tx_clk,	6,	11),
+	GROUP(i2c_sda_b1,	5,	27),
+	GROUP(i2c_sck_b1,	5,	26),
+	GROUP(i2c_sda_c1,	5,	25),
+	GROUP(i2c_sck_c1,	5,	24),
+	GROUP(i2c_sda_d1,	4,	3),
+	GROUP(i2c_sck_d1,	4,	2),
+
+	/* bank BOOT */
+	GROUP(nand_io,		2,	26),
+	GROUP(nand_io_ce0,	2,	25),
+	GROUP(nand_io_ce1,	2,	24),
+	GROUP(nand_io_rb0,	2,	17),
+	GROUP(nand_ale,		2,	21),
+	GROUP(nand_cle,		2,	20),
+	GROUP(nand_wen_clk,	2,	19),
+	GROUP(nand_ren_clk,	2,	18),
+	GROUP(nand_dqs_0,	2,	27),
+	GROUP(nand_dqs_1,	2,	28),
+	GROUP(sdxc_d0_c,	4,	30),
+	GROUP(sdxc_d13_c,	4,	29),
+	GROUP(sdxc_d47_c,	4,	28),
+	GROUP(sdxc_clk_c,	7,	19),
+	GROUP(sdxc_cmd_c,	7,	18),
+	GROUP(nor_d,		5,	1),
+	GROUP(nor_q,		5,	3),
+	GROUP(nor_c,		5,	2),
+	GROUP(nor_cs,		5,	0),
+	GROUP(sd_d0_c,		6,	29),
+	GROUP(sd_d1_c,		6,	28),
+	GROUP(sd_d2_c,		6,	27),
+	GROUP(sd_d3_c,		6,	26),
+	GROUP(sd_cmd_c,		6,	30),
+	GROUP(sd_clk_c,		6,	31),
+
+	/* bank CARD */
+	GROUP(sd_d1_b,		2,	14),
+	GROUP(sd_d0_b,		2,	15),
+	GROUP(sd_clk_b,		2,	11),
+	GROUP(sd_cmd_b,		2,	10),
+	GROUP(sd_d3_b,		2,	12),
+	GROUP(sd_d2_b,		2,	13),
+	GROUP(sdxc_d13_b,	2,	6),
+	GROUP(sdxc_d0_b,	2,	7),
+	GROUP(sdxc_clk_b,	2,	5),
+	GROUP(sdxc_cmd_b,	2,	4),
+
+	/* bank AO */
+	GROUP(uart_tx_ao_a,	0,	12),
+	GROUP(uart_rx_ao_a,	0,	11),
+	GROUP(uart_cts_ao_a,	0,	10),
+	GROUP(uart_rts_ao_a,	0,	9),
+	GROUP(i2c_mst_sck_ao,	0,	6),
+	GROUP(i2c_mst_sda_ao,	0,	5),
+	GROUP(clk_32k_in_out,	0,	18),
+	GROUP(remote_input,	0,	0),
+	GROUP(hdmi_cec_1,	0,	17),
+	GROUP(ir_blaster,	0,	31),
+	GROUP(pwm_c2,		0,	22),
+	GROUP(i2c_sck_ao,	0,	2),
+	GROUP(i2c_sda_ao,	0,	1),
+	GROUP(ir_remote_out,	0,	21),
+	GROUP(i2s_am_clk_out,	0,	30),
+	GROUP(i2s_ao_clk_out,	0,	29),
+	GROUP(i2s_lr_clk_out,	0,	28),
+	GROUP(i2s_out_01,	0,	27),
+	GROUP(uart_tx_ao_b0,	0,	26),
+	GROUP(uart_rx_ao_b0,	0,	25),
+	GROUP(uart_cts_ao_b,	0,	8),
+	GROUP(uart_rts_ao_b,	0,	7),
+	GROUP(uart_tx_ao_b1,	0,	24),
+	GROUP(uart_rx_ao_b1,	0,	23),
+	GROUP(spdif_out_1,	0,	16),
+	GROUP(i2s_in_ch01,	0,	13),
+	GROUP(i2s_ao_clk_in,	0,	15),
+	GROUP(i2s_lr_clk_in,	0,	14),
+
+	/* bank DIF */
+	GROUP(eth_rxd1,		6,	0),
+	GROUP(eth_rxd0,		6,	1),
+	GROUP(eth_rx_dv,	6,	2),
+	GROUP(eth_rx_clk,	6,	3),
+	GROUP(eth_txd0_1,	6,	4),
+	GROUP(eth_txd1_1,	6,	5),
+	GROUP(eth_tx_en,	6,	0),
+	GROUP(eth_ref_clk,	6,	8),
+	GROUP(eth_mdc,		6,	9),
+	GROUP(eth_mdio_en,	6,	10),
+};
+
+static const char * const gpio_groups[] = {
+	"GPIOX_0", "GPIOX_1", "GPIOX_2", "GPIOX_3", "GPIOX_4",
+	"GPIOX_5", "GPIOX_6", "GPIOX_7", "GPIOX_8", "GPIOX_9",
+	"GPIOX_10", "GPIOX_11", "GPIOX_16", "GPIOX_17", "GPIOX_18",
+	"GPIOX_19", "GPIOX_20", "GPIOX_21",
+
+	"GPIOY_0", "GPIOY_1", "GPIOY_3", "GPIOY_6", "GPIOY_7",
+	"GPIOY_8", "GPIOY_9", "GPIOY_10", "GPIOY_11", "GPIOY_12",
+	"GPIOY_13", "GPIOY_14",
+
+	"GPIODV_9", "GPIODV_24", "GPIODV_25", "GPIODV_26",
+	"GPIODV_27", "GPIODV_28", "GPIODV_29",
+
+	"GPIOH_0", "GPIOH_1", "GPIOH_2", "GPIOH_3", "GPIOH_4",
+	"GPIOH_5", "GPIOH_6", "GPIOH_7", "GPIOH_8", "GPIOH_9",
+
+	"CARD_0", "CARD_1", "CARD_2", "CARD_3", "CARD_4",
+	"CARD_5", "CARD_6",
+
+	"BOOT_0", "BOOT_1", "BOOT_2", "BOOT_3", "BOOT_4",
+	"BOOT_5", "BOOT_6", "BOOT_7", "BOOT_8", "BOOT_9",
+	"BOOT_10", "BOOT_11", "BOOT_12", "BOOT_13", "BOOT_14",
+	"BOOT_15", "BOOT_16", "BOOT_17", "BOOT_18",
+
+	"GPIOAO_0", "GPIOAO_1", "GPIOAO_2", "GPIOAO_3",
+	"GPIOAO_4", "GPIOAO_5", "GPIOAO_6", "GPIOAO_7",
+	"GPIOAO_8", "GPIOAO_9", "GPIOAO_10", "GPIOAO_11",
+	"GPIOAO_12", "GPIOAO_13", "GPIO_BSD_EN", "GPIO_TEST_N",
+
+	"DIF_0_P", "DIF_0_N", "DIF_1_P", "DIF_1_N",
+	"DIF_2_P", "DIF_2_N", "DIF_3_P", "DIF_3_N",
+	"DIF_4_P", "DIF_4_N"
+};
+
+static const char * const sd_a_groups[] = {
+	"sd_d0_a", "sd_d1_a", "sd_d2_a", "sd_d3_a", "sd_clk_a",
+	"sd_cmd_a"
+};
+
+static const char * const sdxc_a_groups[] = {
+	"sdxc_d0_0_a", "sdxc_d13_0_a", "sdxc_d47_a", "sdxc_clk_a",
+	"sdxc_cmd_a", "sdxc_d0_1_a", "sdxc_d0_13_1_a"
+};
+
+static const char * const pcm_a_groups[] = {
+	"pcm_out_a", "pcm_in_a", "pcm_fs_a", "pcm_clk_a"
+};
+
+static const char * const uart_a_groups[] = {
+	"uart_tx_a", "uart_rx_a", "uart_cts_a", "uart_rts_a"
+};
+
+static const char * const uart_b_groups[] = {
+	"uart_tx_b0", "uart_rx_b0", "uart_cts_b0", "uart_rts_b0",
+	"uart_tx_b1", "uart_rx_b1", "uart_cts_b1", "uart_rts_b1"
+};
+
+static const char * const iso7816_groups[] = {
+	"iso7816_det", "iso7816_reset", "iso7816_0_clk", "iso7816_0_data",
+	"iso7816_1_clk", "iso7816_1_data", "iso7816_2_clk", "iso7816_2_data"
+};
+
+static const char * const i2c_d_groups[] = {
+	"i2c_sda_d0", "i2c_sck_d0", "i2c_sda_d1", "i2c_sck_d1"
+};
+
+static const char * const xtal_groups[] = {
+	"xtal_32k_out", "xtal_24m_out", "xtal_24m", "xtal24_out"
+};
+
+static const char * const uart_c_groups[] = {
+	"uart_tx_c", "uart_rx_c", "uart_cts_c", "uart_rts_c"
+};
+
+static const char * const i2c_c_groups[] = {
+	"i2c_sda_c0", "i2c_sck_c0", "i2c_sda_c1", "i2c_sck_c1"
+};
+
+static const char * const hdmi_groups[] = {
+	"hdmi_hpd", "hdmi_sda", "hdmi_scl", "hdmi_cec_0",
+	"hdmi_cec_1"
+};
+
+static const char * const spi_groups[] = {
+	"spi_ss0_0", "spi_miso_0", "spi_mosi_0", "spi_sclk_0",
+	"spi_ss0_1", "spi_ss1", "spi_sclk_1", "spi_mosi_1",
+	"spi_miso_1", "spi_ss2"
+};
+
+static const char * const ethernet_groups[] = {
+	"eth_tx_clk", "eth_tx_en", "eth_txd1_0", "eth_txd1_1",
+	"eth_txd0_0", "eth_txd0_1", "eth_rx_clk", "eth_rx_dv",
+	"eth_rxd1", "eth_rxd0", "eth_mdio_en", "eth_mdc", "eth_ref_clk",
+	"eth_txd2", "eth_txd3"
+};
+
+static const char * const i2c_a_groups[] = {
+	"i2c_sda_a", "i2c_sck_a",
+};
+
+static const char * const i2c_b_groups[] = {
+	"i2c_sda_b0", "i2c_sck_b0", "i2c_sda_b1", "i2c_sck_b1"
+};
+
+static const char * const sd_c_groups[] = {
+	"sd_d0_c", "sd_d1_c", "sd_d2_c", "sd_d3_c",
+	"sd_cmd_c", "sd_clk_c"
+};
+
+static const char * const sdxc_c_groups[] = {
+	"sdxc_d0_c", "sdxc_d13_c", "sdxc_d47_c", "sdxc_cmd_c",
+	"sdxc_clk_c"
+};
+
+static const char * const nand_groups[] = {
+	"nand_io", "nand_io_ce0", "nand_io_ce1",
+	"nand_io_rb0", "nand_ale", "nand_cle",
+	"nand_wen_clk", "nand_ren_clk", "nand_dqs0",
+	"nand_dqs1"
+};
+
+static const char * const nor_groups[] = {
+	"nor_d", "nor_q", "nor_c", "nor_cs"
+};
+
+static const char * const sd_b_groups[] = {
+	"sd_d1_b", "sd_d0_b", "sd_clk_b", "sd_cmd_b",
+	"sd_d3_b", "sd_d2_b"
+};
+
+static const char * const sdxc_b_groups[] = {
+	"sdxc_d13_b", "sdxc_d0_b", "sdxc_clk_b", "sdxc_cmd_b"
+};
+
+static const char * const uart_ao_groups[] = {
+	"uart_tx_ao_a", "uart_rx_ao_a", "uart_cts_ao_a", "uart_rts_ao_a"
+};
+
+static const char * const remote_groups[] = {
+	"remote_input", "ir_blaster", "ir_remote_out"
+};
+
+static const char * const i2c_slave_ao_groups[] = {
+	"i2c_sck_ao", "i2c_sda_ao"
+};
+
+static const char * const uart_ao_b_groups[] = {
+	"uart_tx_ao_b0", "uart_rx_ao_b0", "uart_tx_ao_b1", "uart_rx_ao_b1",
+	"uart_cts_ao_b", "uart_rts_ao_b"
+};
+
+static const char * const i2c_mst_ao_groups[] = {
+	"i2c_mst_sck_ao", "i2c_mst_sda_ao"
+};
+
+static const char * const clk_groups[] = {
+	"clk_24m_out", "clk_32k_in_out"
+};
+
+static const char * const spdif_groups[] = {
+	"spdif_out_1", "spdif_out_0"
+};
+
+static const char * const i2s_groups[] = {
+	"i2s_am_clk_out", "i2s_ao_clk_out", "i2s_lr_clk_out",
+	"i2s_out_01", "i2s_in_ch01", "i2s_ao_clk_in",
+	"i2s_lr_clk_in"
+};
+
+static const char * const pwm_b_groups[] = {
+	"pwm_b"
+};
+
+static const char * const pwm_c_groups[] = {
+	"pwm_c0", "pwm_c1", "pwm_c2"
+};
+
+static const char * const pwm_d_groups[] = {
+	"pwm_d"
+};
+
+static const char * const pwm_e_groups[] = {
+	"pwm_e"
+};
+
+static const char * const pwm_vs_groups[] = {
+	"pwm_vs_0", "pwm_vs_1", "pwm_vs_2",
+	"pwm_vs_3", "pwm_vs_4"
+};
+
+static const char * const tsin_a_groups[] = {
+	"tsin_d0_a", "tsin_d17_a", "tsin_clk_a", "tsin_sop_a",
+	"tsin_d_valid_a"
+};
+
+static const char * const tsin_b_groups[] = {
+	"tsin_d0_b", "tsin_clk_b", "tsin_sop_b", "tsin_d_valid_b"
+};
+
+static struct meson_pmx_func meson8b_functions[] = {
+	FUNCTION(gpio),
+	FUNCTION(sd_a),
+	FUNCTION(sdxc_a),
+	FUNCTION(pcm_a),
+	FUNCTION(uart_a),
+	FUNCTION(uart_b),
+	FUNCTION(iso7816),
+	FUNCTION(i2c_d),
+	FUNCTION(xtal),
+	FUNCTION(uart_c),
+	FUNCTION(i2c_c),
+	FUNCTION(hdmi),
+	FUNCTION(spi),
+	FUNCTION(ethernet),
+	FUNCTION(i2c_a),
+	FUNCTION(i2c_b),
+	FUNCTION(sd_c),
+	FUNCTION(sdxc_c),
+	FUNCTION(nand),
+	FUNCTION(nor),
+	FUNCTION(sd_b),
+	FUNCTION(sdxc_b),
+	FUNCTION(uart_ao),
+	FUNCTION(remote),
+	FUNCTION(i2c_slave_ao),
+	FUNCTION(uart_ao_b),
+	FUNCTION(i2c_mst_ao),
+	FUNCTION(clk),
+	FUNCTION(spdif),
+	FUNCTION(i2s),
+	FUNCTION(pwm_b),
+	FUNCTION(pwm_c),
+	FUNCTION(pwm_d),
+	FUNCTION(pwm_e),
+	FUNCTION(pwm_vs),
+	FUNCTION(tsin_a),
+	FUNCTION(tsin_b),
+};
+
+static struct meson_bank meson8b_banks[] = {
+	/*   name    first                      last                   pullen  pull    dir     out     in  */
+	BANK("X",    PIN(GPIOX_0, 0),		PIN(GPIOX_21, 0),      4,  0,  4,  0,  0,  0,  1,  0,  2,  0),
+	BANK("Y",    PIN(GPIOY_0, 0),		PIN(GPIOY_14, 0),      3,  0,  3,  0,  3,  0,  4,  0,  5,  0),
+	BANK("DV",   PIN(GPIODV_9, 0),		PIN(GPIODV_29, 0),     0,  0,  0,  0,  7,  0,  8,  0,  9,  0),
+	BANK("H",    PIN(GPIOH_0, 0),		PIN(GPIOH_9, 0),       1, 16,  1, 16,  9, 19, 10, 19, 11, 19),
+	BANK("CARD", PIN(CARD_0, 0),		PIN(CARD_6, 0),        2, 20,  2, 20,  0, 22,  1, 22,  2, 22),
+	BANK("BOOT", PIN(BOOT_0, 0),		PIN(BOOT_18, 0),       2,  0,  2,  0,  9,  0, 10,  0, 11,  0),
+	BANK("DIF",  PIN(DIF_0_P, 0),		PIN(DIF_4_N, 0),       5,  8,  5,  8, 12, 12, 13, 12, 14, 12),
+};
+
+static struct meson_bank meson8b_ao_banks[] = {
+	/*   name    first                  last                      pullen  pull    dir     out     in  */
+	BANK("AO",   PIN(GPIOAO_0, AO_OFF), PIN(GPIO_TEST_N, AO_OFF), 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
+};
+
+static struct meson_domain_data meson8b_domain_data[] = {
+	{
+		.name		= "banks",
+		.banks		= meson8b_banks,
+		.num_banks	= ARRAY_SIZE(meson8b_banks),
+		.pin_base	= 0,
+		.num_pins	= 83,
+	},
+	{
+		.name		= "ao-bank",
+		.banks		= meson8b_ao_banks,
+		.num_banks	= ARRAY_SIZE(meson8b_ao_banks),
+		.pin_base	= 83,
+		.num_pins	= 16,
+	},
+};
+
+struct meson_pinctrl_data meson8b_pinctrl_data = {
+	.pins		= meson8b_pins,
+	.groups		= meson8b_groups,
+	.funcs		= meson8b_functions,
+	.domain_data	= meson8b_domain_data,
+	.num_pins	= ARRAY_SIZE(meson8b_pins),
+	.num_groups	= ARRAY_SIZE(meson8b_groups),
+	.num_funcs	= ARRAY_SIZE(meson8b_functions),
+	.num_domains	= ARRAY_SIZE(meson8b_domain_data),
+};
diff --git a/drivers/pinctrl/mvebu/Kconfig b/drivers/pinctrl/mvebu/Kconfig
index d6dd8358a6f6..170602407c0d 100644
--- a/drivers/pinctrl/mvebu/Kconfig
+++ b/drivers/pinctrl/mvebu/Kconfig
@@ -26,6 +26,10 @@ config PINCTRL_ARMADA_38X
 	bool
 	select PINCTRL_MVEBU
 
+config PINCTRL_ARMADA_39X
+	bool
+	select PINCTRL_MVEBU
+
 config PINCTRL_ARMADA_XP
 	bool
 	select PINCTRL_MVEBU
diff --git a/drivers/pinctrl/mvebu/Makefile b/drivers/pinctrl/mvebu/Makefile
index a0818e96374b..554d8af14eeb 100644
--- a/drivers/pinctrl/mvebu/Makefile
+++ b/drivers/pinctrl/mvebu/Makefile
@@ -4,5 +4,6 @@ obj-$(CONFIG_PINCTRL_KIRKWOOD)	+= pinctrl-kirkwood.o
 obj-$(CONFIG_PINCTRL_ARMADA_370) += pinctrl-armada-370.o
 obj-$(CONFIG_PINCTRL_ARMADA_375) += pinctrl-armada-375.o
 obj-$(CONFIG_PINCTRL_ARMADA_38X) += pinctrl-armada-38x.o
+obj-$(CONFIG_PINCTRL_ARMADA_39X) += pinctrl-armada-39x.o
 obj-$(CONFIG_PINCTRL_ARMADA_XP)  += pinctrl-armada-xp.o
 obj-$(CONFIG_PINCTRL_ORION)  += pinctrl-orion.o
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-370.c b/drivers/pinctrl/mvebu/pinctrl-armada-370.c
index c4f51d0cd2cc..42f930f70de3 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-370.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-370.c
@@ -379,7 +379,7 @@ static struct mvebu_mpp_mode mv88f6710_mpp_modes[] = {
 
 static struct mvebu_pinctrl_soc_info armada_370_pinctrl_info;
 
-static struct of_device_id armada_370_pinctrl_of_match[] = {
+static const struct of_device_id armada_370_pinctrl_of_match[] = {
 	{ .compatible = "marvell,mv88f6710-pinctrl" },
 	{ },
 };
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-375.c b/drivers/pinctrl/mvebu/pinctrl-armada-375.c
index cd7c8f51f7d9..ca1e7571fedb 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-375.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-375.c
@@ -399,7 +399,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 
 static struct mvebu_pinctrl_soc_info armada_375_pinctrl_info;
 
-static struct of_device_id armada_375_pinctrl_of_match[] = {
+static const struct of_device_id armada_375_pinctrl_of_match[] = {
 	{ .compatible = "marvell,mv88f6720-pinctrl" },
 	{ },
 };
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-38x.c b/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
index 7302f66f4f19..83bbcc72be1f 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
@@ -389,7 +389,7 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 
 static struct mvebu_pinctrl_soc_info armada_38x_pinctrl_info;
 
-static struct of_device_id armada_38x_pinctrl_of_match[] = {
+static const struct of_device_id armada_38x_pinctrl_of_match[] = {
 	{
 		.compatible = "marvell,mv88f6810-pinctrl",
 		.data       = (void *) V_88F6810,
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-39x.c b/drivers/pinctrl/mvebu/pinctrl-armada-39x.c
new file mode 100644
index 000000000000..42491624d660
--- /dev/null
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-39x.c
@@ -0,0 +1,432 @@
+/*
+ * Marvell Armada 39x pinctrl driver based on mvebu pinctrl core
+ *
+ * Copyright (C) 2015 Marvell
+ *
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-mvebu.h"
+
+static void __iomem *mpp_base;
+
+static int armada_39x_mpp_ctrl_get(unsigned pid, unsigned long *config)
+{
+	return default_mpp_ctrl_get(mpp_base, pid, config);
+}
+
+static int armada_39x_mpp_ctrl_set(unsigned pid, unsigned long config)
+{
+	return default_mpp_ctrl_set(mpp_base, pid, config);
+}
+
+enum {
+	V_88F6920 = BIT(0),
+	V_88F6928 = BIT(1),
+	V_88F6920_PLUS = (V_88F6920 | V_88F6928),
+};
+
+static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
+	MPP_MODE(0,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ua0",     "rxd",      V_88F6920_PLUS)),
+	MPP_MODE(1,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ua0",     "txd",      V_88F6920_PLUS)),
+	MPP_MODE(2,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "i2c0",    "sck",      V_88F6920_PLUS)),
+	MPP_MODE(3,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "i2c0",    "sda",      V_88F6920_PLUS)),
+	MPP_MODE(4,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "ua1",     "txd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua0",     "rts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "smi",     "mdc",      V_88F6920_PLUS)),
+	MPP_MODE(5,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "ua1",     "rxd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua0",     "cts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "smi",     "mdio",     V_88F6920_PLUS)),
+	MPP_MODE(6,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "cs3",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "xsmi",    "mdio",     V_88F6920_PLUS)),
+	MPP_MODE(7,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad9",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "xsmi",    "mdc",      V_88F6920_PLUS)),
+	MPP_MODE(8,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad10",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ptp",     "trig",     V_88F6920_PLUS)),
+	MPP_MODE(9,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad11",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ptp",     "clk",      V_88F6920_PLUS)),
+	MPP_MODE(10,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad12",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ptp",     "event",    V_88F6920_PLUS)),
+	MPP_MODE(11,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad13",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "led",     "clk",      V_88F6920_PLUS)),
+	MPP_MODE(12,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad14",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "led",     "stb",      V_88F6920_PLUS)),
+	MPP_MODE(13,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad15",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "led",     "data",     V_88F6920_PLUS)),
+	MPP_MODE(14,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "m",       "vtt",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "wen1",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ua1",     "txd",      V_88F6920_PLUS)),
+	MPP_MODE(15,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi0",    "mosi",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "i2c1",    "sck",      V_88F6920_PLUS)),
+	MPP_MODE(16,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi0",    "miso",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "i2c1",    "sda",      V_88F6920_PLUS)),
+	MPP_MODE(17,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua1",     "rxd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi0",    "sck",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "smi",     "mdio",     V_88F6920_PLUS)),
+	MPP_MODE(18,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua1",     "txd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi0",    "cs0",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "i2c2",    "sck",      V_88F6920_PLUS)),
+	MPP_MODE(19,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(5, "ua0",     "cts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "i2c2",    "sda",      V_88F6920_PLUS)),
+	MPP_MODE(20,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(5, "ua0",     "rts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "smi",     "mdc",      V_88F6920_PLUS)),
+	MPP_MODE(21,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "spi0",    "cs1",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(4, "sd",      "cmd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "bootcs",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "rxd0",     V_88F6920_PLUS)),
+	MPP_MODE(22,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "spi0",    "mosi",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad0",      V_88F6920_PLUS)),
+	MPP_MODE(23,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "spi0",    "sck",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad2",      V_88F6920_PLUS)),
+	MPP_MODE(24,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "spi0",    "miso",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "ua0",     "cts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua1",     "rxd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d4",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "readyn",   V_88F6920_PLUS)),
+	MPP_MODE(25,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "spi0",    "cs0",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "ua0",     "rts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua1",     "txd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d5",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "cs0",      V_88F6920_PLUS)),
+	MPP_MODE(26,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "spi0",    "cs2",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "i2c1",    "sck",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d6",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "cs1",      V_88F6920_PLUS)),
+	MPP_MODE(27,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "spi0",    "cs3",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "i2c1",    "sda",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d7",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "cs2",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "txclkout", V_88F6920_PLUS)),
+	MPP_MODE(28,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad5",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "txd0",     V_88F6920_PLUS)),
+	MPP_MODE(29,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ale0",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "txd1",     V_88F6920_PLUS)),
+	MPP_MODE(30,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "oen",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "txd2",     V_88F6920_PLUS)),
+	MPP_MODE(31,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ale1",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "txd3",     V_88F6920_PLUS)),
+	MPP_MODE(32,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "wen0",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "txctl",    V_88F6920_PLUS)),
+	MPP_MODE(33,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad3",      V_88F6920_PLUS)),
+	MPP_MODE(34,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad1",      V_88F6920_PLUS)),
+	MPP_MODE(35,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ref",     "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "a1",       V_88F6920_PLUS)),
+	MPP_MODE(36,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "a0",       V_88F6920_PLUS)),
+	MPP_MODE(37,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d3",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad8",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "rxclk",    V_88F6920_PLUS)),
+	MPP_MODE(38,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ref",     "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d0",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad4",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "rxd1",     V_88F6920_PLUS)),
+	MPP_MODE(39,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "i2c1",    "sck",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua0",     "cts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d1",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "a2",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "rxd2",     V_88F6920_PLUS)),
+	MPP_MODE(40,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "i2c1",    "sda",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua0",     "rts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd",      "d2",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad6",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "rxd3",     V_88F6920_PLUS)),
+	MPP_MODE(41,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ua1",     "rxd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua0",     "cts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",    "cs3",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "burstn",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "nd",      "rbn0",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(8, "ge",      "rxctl",    V_88F6920_PLUS)),
+	MPP_MODE(42,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ua1",     "txd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ua0",     "rts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ad7",      V_88F6920_PLUS)),
+	MPP_MODE(43,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "pcie0",   "clkreq",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "m",       "vtt",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",    "cs2",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "clkout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "nd",      "rbn1",     V_88F6920_PLUS)),
+	MPP_MODE(44,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(2, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(7, "led",     "clk",      V_88F6920_PLUS)),
+	MPP_MODE(45,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ref",     "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS)),
+	MPP_MODE(46,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ref",     "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "led",     "stb",      V_88F6920_PLUS)),
+	MPP_MODE(47,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(2, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(7, "led",     "data",     V_88F6920_PLUS)),
+	MPP_MODE(48,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(2, "m",       "vtt",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",     "pclk",     V_88F6928),
+		 MPP_VAR_FUNCTION(4, "audio",   "mclk",     V_88F6928),
+		 MPP_VAR_FUNCTION(5, "sd",      "d4",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie0",   "clkreq",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ua1",     "txd",      V_88F6920_PLUS)),
+	MPP_MODE(49,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",     "fsync",    V_88F6928),
+		 MPP_VAR_FUNCTION(4, "audio",   "lrclk",    V_88F6928),
+		 MPP_VAR_FUNCTION(5, "sd",      "d5",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ua2",     "rxd",      V_88F6920_PLUS)),
+	MPP_MODE(50,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",     "drx",      V_88F6928),
+		 MPP_VAR_FUNCTION(4, "audio",   "extclk",   V_88F6928),
+		 MPP_VAR_FUNCTION(5, "sd",      "cmd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ua2",     "rxd",      V_88F6920_PLUS)),
+	MPP_MODE(51,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",     "dtx",      V_88F6928),
+		 MPP_VAR_FUNCTION(4, "audio",   "sdo",      V_88F6928),
+		 MPP_VAR_FUNCTION(5, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ua2",     "txd",      V_88F6920_PLUS)),
+	MPP_MODE(52,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",     "intn",     V_88F6928),
+		 MPP_VAR_FUNCTION(4, "audio",   "sdi",      V_88F6928),
+		 MPP_VAR_FUNCTION(5, "sd",      "d6",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "i2c3",    "sck",      V_88F6920_PLUS)),
+	MPP_MODE(53,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(2, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(3, "tdm",     "rstn",     V_88F6928),
+		 MPP_VAR_FUNCTION(4, "audio",   "bclk",     V_88F6928),
+		 MPP_VAR_FUNCTION(5, "sd",      "d7",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "i2c3",    "sda",      V_88F6920_PLUS)),
+	MPP_MODE(54,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(2, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(3, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd",      "d3",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ua3",     "txd",      V_88F6920_PLUS)),
+	MPP_MODE(55,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ua1",     "cts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",    "cs1",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd",      "d0",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(7, "ua3",     "rxd",      V_88F6920_PLUS)),
+	MPP_MODE(56,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ua1",     "rts",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",    "mosi",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS)),
+	MPP_MODE(57,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",    "sck",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd",      "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS)),
+	MPP_MODE(58,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "i2c1",    "sck",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "pcie2",   "clkreq",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",    "miso",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd",      "d1",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS)),
+	MPP_MODE(59,
+		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "pcie0",   "rstout",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "i2c1",    "sda",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",    "cs0",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd",      "d2",       V_88F6920_PLUS)),
+};
+
+static struct mvebu_pinctrl_soc_info armada_39x_pinctrl_info;
+
+static const struct of_device_id armada_39x_pinctrl_of_match[] = {
+	{
+		.compatible = "marvell,mv88f6920-pinctrl",
+		.data       = (void *) V_88F6920,
+	},
+	{
+		.compatible = "marvell,mv88f6928-pinctrl",
+		.data       = (void *) V_88F6928,
+	},
+	{ },
+};
+
+static struct mvebu_mpp_ctrl armada_39x_mpp_controls[] = {
+	MPP_FUNC_CTRL(0, 59, NULL, armada_39x_mpp_ctrl),
+};
+
+static struct pinctrl_gpio_range armada_39x_mpp_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0,   0,  0, 32),
+	MPP_GPIO_RANGE(1,  32, 32, 27),
+};
+
+static int armada_39x_pinctrl_probe(struct platform_device *pdev)
+{
+	struct mvebu_pinctrl_soc_info *soc = &armada_39x_pinctrl_info;
+	const struct of_device_id *match =
+		of_match_device(armada_39x_pinctrl_of_match, &pdev->dev);
+	struct resource *res;
+
+	if (!match)
+		return -ENODEV;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mpp_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mpp_base))
+		return PTR_ERR(mpp_base);
+
+	soc->variant = (unsigned) match->data & 0xff;
+	soc->controls = armada_39x_mpp_controls;
+	soc->ncontrols = ARRAY_SIZE(armada_39x_mpp_controls);
+	soc->gpioranges = armada_39x_mpp_gpio_ranges;
+	soc->ngpioranges = ARRAY_SIZE(armada_39x_mpp_gpio_ranges);
+	soc->modes = armada_39x_mpp_modes;
+	soc->nmodes = armada_39x_mpp_controls[0].npins;
+
+	pdev->dev.platform_data = soc;
+
+	return mvebu_pinctrl_probe(pdev);
+}
+
+static int armada_39x_pinctrl_remove(struct platform_device *pdev)
+{
+	return mvebu_pinctrl_remove(pdev);
+}
+
+static struct platform_driver armada_39x_pinctrl_driver = {
+	.driver = {
+		.name = "armada-39x-pinctrl",
+		.of_match_table = of_match_ptr(armada_39x_pinctrl_of_match),
+	},
+	.probe = armada_39x_pinctrl_probe,
+	.remove = armada_39x_pinctrl_remove,
+};
+
+module_platform_driver(armada_39x_pinctrl_driver);
+
+MODULE_AUTHOR("Thomas Petazzoni <thomas.petazzoni@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Armada 39x pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-xp.c b/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
index fc3376147c18..578db9f033b2 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
@@ -34,6 +34,7 @@
 #include "pinctrl-mvebu.h"
 
 static void __iomem *mpp_base;
+static u32 *mpp_saved_regs;
 
 static int armada_xp_mpp_ctrl_get(unsigned pid, unsigned long *config)
 {
@@ -361,7 +362,7 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 
 static struct mvebu_pinctrl_soc_info armada_xp_pinctrl_info;
 
-static struct of_device_id armada_xp_pinctrl_of_match[] = {
+static const struct of_device_id armada_xp_pinctrl_of_match[] = {
 	{
 		.compatible = "marvell,mv78230-pinctrl",
 		.data       = (void *) V_MV78230,
@@ -406,12 +407,42 @@ static struct pinctrl_gpio_range mv78460_mpp_gpio_ranges[] = {
 	MPP_GPIO_RANGE(2,  64, 64,  3),
 };
 
+static int armada_xp_pinctrl_suspend(struct platform_device *pdev,
+				     pm_message_t state)
+{
+	struct mvebu_pinctrl_soc_info *soc =
+		platform_get_drvdata(pdev);
+	int i, nregs;
+
+	nregs = DIV_ROUND_UP(soc->nmodes, MVEBU_MPPS_PER_REG);
+
+	for (i = 0; i < nregs; i++)
+		mpp_saved_regs[i] = readl(mpp_base + i * 4);
+
+	return 0;
+}
+
+static int armada_xp_pinctrl_resume(struct platform_device *pdev)
+{
+	struct mvebu_pinctrl_soc_info *soc =
+		platform_get_drvdata(pdev);
+	int i, nregs;
+
+	nregs = DIV_ROUND_UP(soc->nmodes, MVEBU_MPPS_PER_REG);
+
+	for (i = 0; i < nregs; i++)
+		writel(mpp_saved_regs[i], mpp_base + i * 4);
+
+	return 0;
+}
+
 static int armada_xp_pinctrl_probe(struct platform_device *pdev)
 {
 	struct mvebu_pinctrl_soc_info *soc = &armada_xp_pinctrl_info;
 	const struct of_device_id *match =
 		of_match_device(armada_xp_pinctrl_of_match, &pdev->dev);
 	struct resource *res;
+	int nregs;
 
 	if (!match)
 		return -ENODEV;
@@ -459,6 +490,13 @@ static int armada_xp_pinctrl_probe(struct platform_device *pdev)
 		break;
 	}
 
+	nregs = DIV_ROUND_UP(soc->nmodes, MVEBU_MPPS_PER_REG);
+
+	mpp_saved_regs = devm_kmalloc(&pdev->dev, nregs * sizeof(u32),
+				      GFP_KERNEL);
+	if (!mpp_saved_regs)
+		return -ENOMEM;
+
 	pdev->dev.platform_data = soc;
 
 	return mvebu_pinctrl_probe(pdev);
@@ -476,6 +514,8 @@ static struct platform_driver armada_xp_pinctrl_driver = {
 	},
 	.probe = armada_xp_pinctrl_probe,
 	.remove = armada_xp_pinctrl_remove,
+	.suspend = armada_xp_pinctrl_suspend,
+	.resume = armada_xp_pinctrl_resume,
 };
 
 module_platform_driver(armada_xp_pinctrl_driver);
diff --git a/drivers/pinctrl/mvebu/pinctrl-kirkwood.c b/drivers/pinctrl/mvebu/pinctrl-kirkwood.c
index dbc673cf7131..0f07dc554a1d 100644
--- a/drivers/pinctrl/mvebu/pinctrl-kirkwood.c
+++ b/drivers/pinctrl/mvebu/pinctrl-kirkwood.c
@@ -456,7 +456,7 @@ static struct mvebu_pinctrl_soc_info mv98dx4122_info = {
 	.ngpioranges = ARRAY_SIZE(mv88f628x_gpio_ranges),
 };
 
-static struct of_device_id kirkwood_pinctrl_of_match[] = {
+static const struct of_device_id kirkwood_pinctrl_of_match[] = {
 	{ .compatible = "marvell,88f6180-pinctrl", .data = &mv88f6180_info },
 	{ .compatible = "marvell,88f6190-pinctrl", .data = &mv88f6190_info },
 	{ .compatible = "marvell,88f6192-pinctrl", .data = &mv88f6192_info },
diff --git a/drivers/pinctrl/mvebu/pinctrl-orion.c b/drivers/pinctrl/mvebu/pinctrl-orion.c
index 3a632efb56bb..3b7122d826e4 100644
--- a/drivers/pinctrl/mvebu/pinctrl-orion.c
+++ b/drivers/pinctrl/mvebu/pinctrl-orion.c
@@ -211,7 +211,7 @@ static struct mvebu_pinctrl_soc_info mv88f5281_info = {
  * There are multiple variants of the Orion SoCs, but in terms of pin
  * muxing, they are identical.
  */
-static struct of_device_id orion_pinctrl_of_match[] = {
+static const struct of_device_id orion_pinctrl_of_match[] = {
 	{ .compatible = "marvell,88f5181l-pinctrl", .data = &mv88f5181l_info },
 	{ .compatible = "marvell,88f5182-pinctrl", .data = &mv88f5182_info },
 	{ .compatible = "marvell,88f5281-pinctrl", .data = &mv88f5281_info },
diff --git a/drivers/pinctrl/nomadik/pinctrl-abx500.c b/drivers/pinctrl/nomadik/pinctrl-abx500.c
index 1806b24faa14..23db4c9ac76c 100644
--- a/drivers/pinctrl/nomadik/pinctrl-abx500.c
+++ b/drivers/pinctrl/nomadik/pinctrl-abx500.c
@@ -466,7 +466,7 @@ static int abx500_set_mode(struct pinctrl_dev *pctldev, struct gpio_chip *chip,
 		break;
 
 	default:
-		dev_dbg(pct->dev, "unknow alt_setting %d\n", alt_setting);
+		dev_dbg(pct->dev, "unknown alt_setting %d\n", alt_setting);
 
 		return -EINVAL;
 	}
diff --git a/drivers/pinctrl/pinconf-generic.c b/drivers/pinctrl/pinconf-generic.c
index 4db92f64b4de..e63ad9fbd388 100644
--- a/drivers/pinctrl/pinconf-generic.c
+++ b/drivers/pinctrl/pinconf-generic.c
@@ -283,23 +283,40 @@ int pinconf_generic_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 	struct device *dev = pctldev->dev;
 	unsigned long *configs = NULL;
 	unsigned num_configs = 0;
-	unsigned reserve;
+	unsigned reserve, strings_count;
 	struct property *prop;
 	const char *group;
 	const char *subnode_target_type = "pins";
 
+	ret = of_property_count_strings(np, "pins");
+	if (ret < 0) {
+		ret = of_property_count_strings(np, "groups");
+		if (ret < 0)
+			/* skip this node; may contain config child nodes */
+			return 0;
+		if (type == PIN_MAP_TYPE_INVALID)
+			type = PIN_MAP_TYPE_CONFIGS_GROUP;
+		subnode_target_type = "groups";
+	} else {
+		if (type == PIN_MAP_TYPE_INVALID)
+			type = PIN_MAP_TYPE_CONFIGS_PIN;
+	}
+	strings_count = ret;
+
 	ret = of_property_read_string(np, "function", &function);
 	if (ret < 0) {
 		/* EINVAL=missing, which is fine since it's optional */
 		if (ret != -EINVAL)
-			dev_err(dev, "could not parse property function\n");
+			dev_err(dev, "%s: could not parse property function\n",
+				of_node_full_name(np));
 		function = NULL;
 	}
 
 	ret = pinconf_generic_parse_dt_config(np, pctldev, &configs,
 					      &num_configs);
 	if (ret < 0) {
-		dev_err(dev, "could not parse node property\n");
+		dev_err(dev, "%s: could not parse node property\n",
+			of_node_full_name(np));
 		return ret;
 	}
 
@@ -309,21 +326,7 @@ int pinconf_generic_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 	if (num_configs)
 		reserve++;
 
-	ret = of_property_count_strings(np, "pins");
-	if (ret < 0) {
-		ret = of_property_count_strings(np, "groups");
-		if (ret < 0) {
-			dev_err(dev, "could not parse property pins/groups\n");
-			goto exit;
-		}
-		if (type == PIN_MAP_TYPE_INVALID)
-			type = PIN_MAP_TYPE_CONFIGS_GROUP;
-		subnode_target_type = "groups";
-	} else {
-		if (type == PIN_MAP_TYPE_INVALID)
-			type = PIN_MAP_TYPE_CONFIGS_PIN;
-	}
-	reserve *= ret;
+	reserve *= strings_count;
 
 	ret = pinctrl_utils_reserve_map(pctldev, map, reserved_maps,
 			num_maps, reserve);
@@ -367,15 +370,22 @@ int pinconf_generic_dt_node_to_map(struct pinctrl_dev *pctldev,
 	*map = NULL;
 	*num_maps = 0;
 
+	ret = pinconf_generic_dt_subnode_to_map(pctldev, np_config, map,
+						&reserved_maps, num_maps, type);
+	if (ret < 0)
+		goto exit;
+
 	for_each_child_of_node(np_config, np) {
 		ret = pinconf_generic_dt_subnode_to_map(pctldev, np, map,
 					&reserved_maps, num_maps, type);
-		if (ret < 0) {
-			pinctrl_utils_dt_free_map(pctldev, *map, *num_maps);
-			return ret;
-		}
+		if (ret < 0)
+			goto exit;
 	}
 	return 0;
+
+exit:
+	pinctrl_utils_dt_free_map(pctldev, *map, *num_maps);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(pinconf_generic_dt_node_to_map);
 
diff --git a/drivers/pinctrl/pinctrl-amd.c b/drivers/pinctrl/pinctrl-amd.c
new file mode 100644
index 000000000000..7de3b64bf142
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-amd.c
@@ -0,0 +1,869 @@
+/*
+ * GPIO driver for AMD
+ *
+ * Copyright (c) 2014,2015 AMD Corporation.
+ * Authors: Ken Xue <Ken.Xue@amd.com>
+ *      Wu, Jeff <Jeff.Wu@amd.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/bug.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/compiler.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/log2.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/acpi.h>
+#include <linux/seq_file.h>
+#include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/bitops.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+
+#include "pinctrl-utils.h"
+#include "pinctrl-amd.h"
+
+static inline struct amd_gpio *to_amd_gpio(struct gpio_chip *gc)
+{
+	return container_of(gc, struct amd_gpio, gc);
+}
+
+static int amd_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
+{
+	unsigned long flags;
+	u32 pin_reg;
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + offset * 4);
+	/*
+	 * Suppose BIOS or Bootloader sets specific debounce for the
+	 * GPIO. if not, set debounce to be  2.75ms and remove glitch.
+	*/
+	if ((pin_reg & DB_TMR_OUT_MASK) == 0) {
+		pin_reg |= 0xf;
+		pin_reg |= BIT(DB_TMR_OUT_UNIT_OFF);
+		pin_reg |= DB_TYPE_REMOVE_GLITCH << DB_CNTRL_OFF;
+		pin_reg &= ~BIT(DB_TMR_LARGE_OFF);
+	}
+
+	pin_reg &= ~BIT(OUTPUT_ENABLE_OFF);
+	writel(pin_reg, gpio_dev->base + offset * 4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	return 0;
+}
+
+static int amd_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
+		int value)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + offset * 4);
+	pin_reg |= BIT(OUTPUT_ENABLE_OFF);
+	if (value)
+		pin_reg |= BIT(OUTPUT_VALUE_OFF);
+	else
+		pin_reg &= ~BIT(OUTPUT_VALUE_OFF);
+	writel(pin_reg, gpio_dev->base + offset * 4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	return 0;
+}
+
+static int amd_gpio_get_value(struct gpio_chip *gc, unsigned offset)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + offset * 4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	return !!(pin_reg & BIT(PIN_STS_OFF));
+}
+
+static void amd_gpio_set_value(struct gpio_chip *gc, unsigned offset, int value)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + offset * 4);
+	if (value)
+		pin_reg |= BIT(OUTPUT_VALUE_OFF);
+	else
+		pin_reg &= ~BIT(OUTPUT_VALUE_OFF);
+	writel(pin_reg, gpio_dev->base + offset * 4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+}
+
+static int amd_gpio_set_debounce(struct gpio_chip *gc, unsigned offset,
+		unsigned debounce)
+{
+	u32 time;
+	u32 pin_reg;
+	int ret = 0;
+	unsigned long flags;
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + offset * 4);
+
+	if (debounce) {
+		pin_reg |= DB_TYPE_REMOVE_GLITCH << DB_CNTRL_OFF;
+		pin_reg &= ~DB_TMR_OUT_MASK;
+		/*
+		Debounce	Debounce	Timer	Max
+		TmrLarge	TmrOutUnit	Unit	Debounce
+							Time
+		0	0	61 usec (2 RtcClk)	976 usec
+		0	1	244 usec (8 RtcClk)	3.9 msec
+		1	0	15.6 msec (512 RtcClk)	250 msec
+		1	1	62.5 msec (2048 RtcClk)	1 sec
+		*/
+
+		if (debounce < 61) {
+			pin_reg |= 1;
+			pin_reg &= ~BIT(DB_TMR_OUT_UNIT_OFF);
+			pin_reg &= ~BIT(DB_TMR_LARGE_OFF);
+		} else if (debounce < 976) {
+			time = debounce / 61;
+			pin_reg |= time & DB_TMR_OUT_MASK;
+			pin_reg &= ~BIT(DB_TMR_OUT_UNIT_OFF);
+			pin_reg &= ~BIT(DB_TMR_LARGE_OFF);
+		} else if (debounce < 3900) {
+			time = debounce / 244;
+			pin_reg |= time & DB_TMR_OUT_MASK;
+			pin_reg |= BIT(DB_TMR_OUT_UNIT_OFF);
+			pin_reg &= ~BIT(DB_TMR_LARGE_OFF);
+		} else if (debounce < 250000) {
+			time = debounce / 15600;
+			pin_reg |= time & DB_TMR_OUT_MASK;
+			pin_reg &= ~BIT(DB_TMR_OUT_UNIT_OFF);
+			pin_reg |= BIT(DB_TMR_LARGE_OFF);
+		} else if (debounce < 1000000) {
+			time = debounce / 62500;
+			pin_reg |= time & DB_TMR_OUT_MASK;
+			pin_reg |= BIT(DB_TMR_OUT_UNIT_OFF);
+			pin_reg |= BIT(DB_TMR_LARGE_OFF);
+		} else {
+			pin_reg &= ~DB_CNTRl_MASK;
+			ret = -EINVAL;
+		}
+	} else {
+		pin_reg &= ~BIT(DB_TMR_OUT_UNIT_OFF);
+		pin_reg &= ~BIT(DB_TMR_LARGE_OFF);
+		pin_reg &= ~DB_TMR_OUT_MASK;
+		pin_reg &= ~DB_CNTRl_MASK;
+	}
+	writel(pin_reg, gpio_dev->base + offset * 4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	return ret;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void amd_gpio_dbg_show(struct seq_file *s, struct gpio_chip *gc)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	unsigned int bank, i, pin_num;
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	char *level_trig;
+	char *active_level;
+	char *interrupt_enable;
+	char *interrupt_mask;
+	char *wake_cntrl0;
+	char *wake_cntrl1;
+	char *wake_cntrl2;
+	char *pin_sts;
+	char *pull_up_sel;
+	char *pull_up_enable;
+	char *pull_down_enable;
+	char *output_value;
+	char *output_enable;
+
+	for (bank = 0; bank < AMD_GPIO_TOTAL_BANKS; bank++) {
+		seq_printf(s, "GPIO bank%d\t", bank);
+
+		switch (bank) {
+		case 0:
+			i = 0;
+			pin_num = AMD_GPIO_PINS_BANK0;
+			break;
+		case 1:
+			i = 64;
+			pin_num = AMD_GPIO_PINS_BANK1 + i;
+			break;
+		case 2:
+			i = 128;
+			pin_num = AMD_GPIO_PINS_BANK2 + i;
+			break;
+		}
+
+		for (; i < pin_num; i++) {
+			seq_printf(s, "pin%d\t", i);
+			spin_lock_irqsave(&gpio_dev->lock, flags);
+			pin_reg = readl(gpio_dev->base + i * 4);
+			spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+			if (pin_reg & BIT(INTERRUPT_ENABLE_OFF)) {
+				interrupt_enable = "interrupt is enabled|";
+
+				if (!(pin_reg & BIT(ACTIVE_LEVEL_OFF))
+				&& !(pin_reg & BIT(ACTIVE_LEVEL_OFF+1)))
+					active_level = "Active low|";
+				else if (pin_reg & BIT(ACTIVE_LEVEL_OFF)
+				&& !(pin_reg & BIT(ACTIVE_LEVEL_OFF+1)))
+					active_level = "Active high|";
+				else if (!(pin_reg & BIT(ACTIVE_LEVEL_OFF))
+					&& pin_reg & BIT(ACTIVE_LEVEL_OFF+1))
+					active_level = "Active on both|";
+				else
+					active_level = "Unknow Active level|";
+
+				if (pin_reg & BIT(LEVEL_TRIG_OFF))
+					level_trig = "Level trigger|";
+				else
+					level_trig = "Edge trigger|";
+
+			} else {
+				interrupt_enable =
+					"interrupt is disabled|";
+				active_level = " ";
+				level_trig = " ";
+			}
+
+			if (pin_reg & BIT(INTERRUPT_MASK_OFF))
+				interrupt_mask =
+					"interrupt is unmasked|";
+			else
+				interrupt_mask =
+					"interrupt is masked|";
+
+			if (pin_reg & BIT(WAKE_CNTRL_OFF))
+				wake_cntrl0 = "enable wakeup in S0i3 state|";
+			else
+				wake_cntrl0 = "disable wakeup in S0i3 state|";
+
+			if (pin_reg & BIT(WAKE_CNTRL_OFF))
+				wake_cntrl1 = "enable wakeup in S3 state|";
+			else
+				wake_cntrl1 = "disable wakeup in S3 state|";
+
+			if (pin_reg & BIT(WAKE_CNTRL_OFF))
+				wake_cntrl2 = "enable wakeup in S4/S5 state|";
+			else
+				wake_cntrl2 = "disable wakeup in S4/S5 state|";
+
+			if (pin_reg & BIT(PULL_UP_ENABLE_OFF)) {
+				pull_up_enable = "pull-up is enabled|";
+				if (pin_reg & BIT(PULL_UP_SEL_OFF))
+					pull_up_sel = "8k pull-up|";
+				else
+					pull_up_sel = "4k pull-up|";
+			} else {
+				pull_up_enable = "pull-up is disabled|";
+				pull_up_sel = " ";
+			}
+
+			if (pin_reg & BIT(PULL_DOWN_ENABLE_OFF))
+				pull_down_enable = "pull-down is enabled|";
+			else
+				pull_down_enable = "Pull-down is disabled|";
+
+			if (pin_reg & BIT(OUTPUT_ENABLE_OFF)) {
+				pin_sts = " ";
+				output_enable = "output is enabled|";
+				if (pin_reg & BIT(OUTPUT_VALUE_OFF))
+					output_value = "output is high|";
+				else
+					output_value = "output is low|";
+			} else {
+				output_enable = "output is disabled|";
+				output_value = " ";
+
+				if (pin_reg & BIT(PIN_STS_OFF))
+					pin_sts = "input is high|";
+				else
+					pin_sts = "input is low|";
+			}
+
+			seq_printf(s, "%s %s %s %s %s %s\n"
+				" %s %s %s %s %s %s %s 0x%x\n",
+				level_trig, active_level, interrupt_enable,
+				interrupt_mask, wake_cntrl0, wake_cntrl1,
+				wake_cntrl2, pin_sts, pull_up_sel,
+				pull_up_enable, pull_down_enable,
+				output_value, output_enable, pin_reg);
+		}
+	}
+}
+#else
+#define amd_gpio_dbg_show NULL
+#endif
+
+static void amd_gpio_irq_enable(struct irq_data *d)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
+	/*
+		Suppose BIOS or Bootloader sets specific debounce for the
+		GPIO. if not, set debounce to be  2.75ms.
+	*/
+	if ((pin_reg & DB_TMR_OUT_MASK) == 0) {
+		pin_reg |= 0xf;
+		pin_reg |= BIT(DB_TMR_OUT_UNIT_OFF);
+		pin_reg &= ~BIT(DB_TMR_LARGE_OFF);
+	}
+	pin_reg |= BIT(INTERRUPT_ENABLE_OFF);
+	pin_reg |= BIT(INTERRUPT_MASK_OFF);
+	writel(pin_reg, gpio_dev->base + (d->hwirq)*4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+}
+
+static void amd_gpio_irq_disable(struct irq_data *d)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
+	pin_reg &= ~BIT(INTERRUPT_ENABLE_OFF);
+	pin_reg &= ~BIT(INTERRUPT_MASK_OFF);
+	writel(pin_reg, gpio_dev->base + (d->hwirq)*4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+}
+
+static void amd_gpio_irq_mask(struct irq_data *d)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
+	pin_reg &= ~BIT(INTERRUPT_MASK_OFF);
+	writel(pin_reg, gpio_dev->base + (d->hwirq)*4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+}
+
+static void amd_gpio_irq_unmask(struct irq_data *d)
+{
+	u32 pin_reg;
+	unsigned long flags;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
+	pin_reg |= BIT(INTERRUPT_MASK_OFF);
+	writel(pin_reg, gpio_dev->base + (d->hwirq)*4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+}
+
+static void amd_gpio_irq_eoi(struct irq_data *d)
+{
+	u32 reg;
+	unsigned long flags;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	reg = readl(gpio_dev->base + WAKE_INT_MASTER_REG);
+	reg |= EOI_MASK;
+	writel(reg, gpio_dev->base + WAKE_INT_MASTER_REG);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+}
+
+static int amd_gpio_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	int ret = 0;
+	u32 pin_reg;
+	unsigned long flags;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
+
+	switch (type & IRQ_TYPE_SENSE_MASK) {
+	case IRQ_TYPE_EDGE_RISING:
+		pin_reg &= ~BIT(LEVEL_TRIG_OFF);
+		pin_reg &= ~(ACTIVE_LEVEL_MASK << ACTIVE_LEVEL_OFF);
+		pin_reg |= ACTIVE_HIGH << ACTIVE_LEVEL_OFF;
+		pin_reg |= DB_TYPE_REMOVE_GLITCH << DB_CNTRL_OFF;
+		__irq_set_handler_locked(d->irq, handle_edge_irq);
+		break;
+
+	case IRQ_TYPE_EDGE_FALLING:
+		pin_reg &= ~BIT(LEVEL_TRIG_OFF);
+		pin_reg &= ~(ACTIVE_LEVEL_MASK << ACTIVE_LEVEL_OFF);
+		pin_reg |= ACTIVE_LOW << ACTIVE_LEVEL_OFF;
+		pin_reg |= DB_TYPE_REMOVE_GLITCH << DB_CNTRL_OFF;
+		__irq_set_handler_locked(d->irq, handle_edge_irq);
+		break;
+
+	case IRQ_TYPE_EDGE_BOTH:
+		pin_reg &= ~BIT(LEVEL_TRIG_OFF);
+		pin_reg &= ~(ACTIVE_LEVEL_MASK << ACTIVE_LEVEL_OFF);
+		pin_reg |= BOTH_EADGE << ACTIVE_LEVEL_OFF;
+		pin_reg |= DB_TYPE_REMOVE_GLITCH << DB_CNTRL_OFF;
+		__irq_set_handler_locked(d->irq, handle_edge_irq);
+		break;
+
+	case IRQ_TYPE_LEVEL_HIGH:
+		pin_reg |= LEVEL_TRIGGER << LEVEL_TRIG_OFF;
+		pin_reg &= ~(ACTIVE_LEVEL_MASK << ACTIVE_LEVEL_OFF);
+		pin_reg |= ACTIVE_HIGH << ACTIVE_LEVEL_OFF;
+		pin_reg &= ~(DB_CNTRl_MASK << DB_CNTRL_OFF);
+		pin_reg |= DB_TYPE_PRESERVE_LOW_GLITCH << DB_CNTRL_OFF;
+		__irq_set_handler_locked(d->irq, handle_level_irq);
+		break;
+
+	case IRQ_TYPE_LEVEL_LOW:
+		pin_reg |= LEVEL_TRIGGER << LEVEL_TRIG_OFF;
+		pin_reg &= ~(ACTIVE_LEVEL_MASK << ACTIVE_LEVEL_OFF);
+		pin_reg |= ACTIVE_LOW << ACTIVE_LEVEL_OFF;
+		pin_reg &= ~(DB_CNTRl_MASK << DB_CNTRL_OFF);
+		pin_reg |= DB_TYPE_PRESERVE_HIGH_GLITCH << DB_CNTRL_OFF;
+		__irq_set_handler_locked(d->irq, handle_level_irq);
+		break;
+
+	case IRQ_TYPE_NONE:
+		break;
+
+	default:
+		dev_err(&gpio_dev->pdev->dev, "Invalid type value\n");
+		ret = -EINVAL;
+	}
+
+	pin_reg |= CLR_INTR_STAT << INTERRUPT_STS_OFF;
+	writel(pin_reg, gpio_dev->base + (d->hwirq)*4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	return ret;
+}
+
+static void amd_irq_ack(struct irq_data *d)
+{
+	/*
+	 * based on HW design,there is no need to ack HW
+	 * before handle current irq. But this routine is
+	 * necessary for handle_edge_irq
+	*/
+}
+
+static struct irq_chip amd_gpio_irqchip = {
+	.name         = "amd_gpio",
+	.irq_ack      = amd_irq_ack,
+	.irq_enable   = amd_gpio_irq_enable,
+	.irq_disable  = amd_gpio_irq_disable,
+	.irq_mask     = amd_gpio_irq_mask,
+	.irq_unmask   = amd_gpio_irq_unmask,
+	.irq_eoi      = amd_gpio_irq_eoi,
+	.irq_set_type = amd_gpio_irq_set_type,
+};
+
+static void amd_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
+{
+	u32 i;
+	u32 off;
+	u32 reg;
+	u32 pin_reg;
+	u64 reg64;
+	int handled = 0;
+	unsigned long flags;
+	struct irq_chip *chip = irq_get_chip(irq);
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+
+	chained_irq_enter(chip, desc);
+	/*enable GPIO interrupt again*/
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	reg = readl(gpio_dev->base + WAKE_INT_STATUS_REG1);
+	reg64 = reg;
+	reg64 = reg64 << 32;
+
+	reg = readl(gpio_dev->base + WAKE_INT_STATUS_REG0);
+	reg64 |= reg;
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	/*
+	 * first 46 bits indicates interrupt status.
+	 * one bit represents four interrupt sources.
+	*/
+	for (off = 0; off < 46 ; off++) {
+		if (reg64 & BIT(off)) {
+			for (i = 0; i < 4; i++) {
+				pin_reg = readl(gpio_dev->base +
+						(off * 4 + i) * 4);
+				if ((pin_reg & BIT(INTERRUPT_STS_OFF)) ||
+					(pin_reg & BIT(WAKE_STS_OFF))) {
+					irq = irq_find_mapping(gc->irqdomain,
+								off * 4 + i);
+					generic_handle_irq(irq);
+					writel(pin_reg,
+						gpio_dev->base
+						+ (off * 4 + i) * 4);
+					handled++;
+				}
+			}
+		}
+	}
+
+	if (handled == 0)
+		handle_bad_irq(irq, desc);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	reg = readl(gpio_dev->base + WAKE_INT_MASTER_REG);
+	reg |= EOI_MASK;
+	writel(reg, gpio_dev->base + WAKE_INT_MASTER_REG);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	chained_irq_exit(chip, desc);
+}
+
+static int amd_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctldev);
+
+	return gpio_dev->ngroups;
+}
+
+static const char *amd_get_group_name(struct pinctrl_dev *pctldev,
+				      unsigned group)
+{
+	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctldev);
+
+	return gpio_dev->groups[group].name;
+}
+
+static int amd_get_group_pins(struct pinctrl_dev *pctldev,
+			      unsigned group,
+			      const unsigned **pins,
+			      unsigned *num_pins)
+{
+	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = gpio_dev->groups[group].pins;
+	*num_pins = gpio_dev->groups[group].npins;
+	return 0;
+}
+
+static const struct pinctrl_ops amd_pinctrl_ops = {
+	.get_groups_count	= amd_get_groups_count,
+	.get_group_name		= amd_get_group_name,
+	.get_group_pins		= amd_get_group_pins,
+#ifdef CONFIG_OF
+	.dt_node_to_map		= pinconf_generic_dt_node_to_map_group,
+	.dt_free_map		= pinctrl_utils_dt_free_map,
+#endif
+};
+
+static int amd_pinconf_get(struct pinctrl_dev *pctldev,
+			  unsigned int pin,
+			  unsigned long *config)
+{
+	u32 pin_reg;
+	unsigned arg;
+	unsigned long flags;
+	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	pin_reg = readl(gpio_dev->base + pin*4);
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+	switch (param) {
+	case PIN_CONFIG_INPUT_DEBOUNCE:
+		arg = pin_reg & DB_TMR_OUT_MASK;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		arg = (pin_reg >> PULL_DOWN_ENABLE_OFF) & BIT(0);
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		arg = (pin_reg >> PULL_UP_SEL_OFF) & (BIT(0) | BIT(1));
+		break;
+
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		arg = (pin_reg >> DRV_STRENGTH_SEL_OFF) & DRV_STRENGTH_SEL_MASK;
+		break;
+
+	default:
+		dev_err(&gpio_dev->pdev->dev, "Invalid config param %04x\n",
+			param);
+		return -ENOTSUPP;
+	}
+
+	*config = pinconf_to_config_packed(param, arg);
+
+	return 0;
+}
+
+static int amd_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
+				unsigned long *configs, unsigned num_configs)
+{
+	int i;
+	u32 arg;
+	int ret = 0;
+	u32 pin_reg;
+	unsigned long flags;
+	enum pin_config_param param;
+	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctldev);
+
+	spin_lock_irqsave(&gpio_dev->lock, flags);
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		arg = pinconf_to_config_argument(configs[i]);
+		pin_reg = readl(gpio_dev->base + pin*4);
+
+		switch (param) {
+		case PIN_CONFIG_INPUT_DEBOUNCE:
+			pin_reg &= ~DB_TMR_OUT_MASK;
+			pin_reg |= arg & DB_TMR_OUT_MASK;
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			pin_reg &= ~BIT(PULL_DOWN_ENABLE_OFF);
+			pin_reg |= (arg & BIT(0)) << PULL_DOWN_ENABLE_OFF;
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_UP:
+			pin_reg &= ~BIT(PULL_UP_SEL_OFF);
+			pin_reg |= (arg & BIT(0)) << PULL_UP_SEL_OFF;
+			pin_reg &= ~BIT(PULL_UP_ENABLE_OFF);
+			pin_reg |= ((arg>>1) & BIT(0)) << PULL_UP_ENABLE_OFF;
+			break;
+
+		case PIN_CONFIG_DRIVE_STRENGTH:
+			pin_reg &= ~(DRV_STRENGTH_SEL_MASK
+					<< DRV_STRENGTH_SEL_OFF);
+			pin_reg |= (arg & DRV_STRENGTH_SEL_MASK)
+					<< DRV_STRENGTH_SEL_OFF;
+			break;
+
+		default:
+			dev_err(&gpio_dev->pdev->dev,
+				"Invalid config param %04x\n", param);
+			ret = -ENOTSUPP;
+		}
+
+		writel(pin_reg, gpio_dev->base + pin*4);
+	}
+	spin_unlock_irqrestore(&gpio_dev->lock, flags);
+
+	return ret;
+}
+
+static int amd_pinconf_group_get(struct pinctrl_dev *pctldev,
+				unsigned int group,
+				unsigned long *config)
+{
+	const unsigned *pins;
+	unsigned npins;
+	int ret;
+
+	ret = amd_get_group_pins(pctldev, group, &pins, &npins);
+	if (ret)
+		return ret;
+
+	if (amd_pinconf_get(pctldev, pins[0], config))
+			return -ENOTSUPP;
+
+	return 0;
+}
+
+static int amd_pinconf_group_set(struct pinctrl_dev *pctldev,
+				unsigned group, unsigned long *configs,
+				unsigned num_configs)
+{
+	const unsigned *pins;
+	unsigned npins;
+	int i, ret;
+
+	ret = amd_get_group_pins(pctldev, group, &pins, &npins);
+	if (ret)
+		return ret;
+	for (i = 0; i < npins; i++) {
+		if (amd_pinconf_set(pctldev, pins[i], configs, num_configs))
+			return -ENOTSUPP;
+	}
+	return 0;
+}
+
+static const struct pinconf_ops amd_pinconf_ops = {
+	.pin_config_get		= amd_pinconf_get,
+	.pin_config_set		= amd_pinconf_set,
+	.pin_config_group_get = amd_pinconf_group_get,
+	.pin_config_group_set = amd_pinconf_group_set,
+};
+
+static struct pinctrl_desc amd_pinctrl_desc = {
+	.pins	= kerncz_pins,
+	.npins = ARRAY_SIZE(kerncz_pins),
+	.pctlops = &amd_pinctrl_ops,
+	.confops = &amd_pinconf_ops,
+	.owner = THIS_MODULE,
+};
+
+static int amd_gpio_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int irq_base;
+	struct resource *res;
+	struct amd_gpio *gpio_dev;
+
+	gpio_dev = devm_kzalloc(&pdev->dev,
+				sizeof(struct amd_gpio), GFP_KERNEL);
+	if (!gpio_dev)
+		return -ENOMEM;
+
+	spin_lock_init(&gpio_dev->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get gpio io resource.\n");
+		return -EINVAL;
+	}
+
+	gpio_dev->base = devm_ioremap_nocache(&pdev->dev, res->start,
+						resource_size(res));
+	if (IS_ERR(gpio_dev->base))
+		return PTR_ERR(gpio_dev->base);
+
+	irq_base = platform_get_irq(pdev, 0);
+	if (irq_base < 0) {
+		dev_err(&pdev->dev, "Failed to get gpio IRQ.\n");
+		return -EINVAL;
+	}
+
+	gpio_dev->pdev = pdev;
+	gpio_dev->gc.direction_input	= amd_gpio_direction_input;
+	gpio_dev->gc.direction_output	= amd_gpio_direction_output;
+	gpio_dev->gc.get			= amd_gpio_get_value;
+	gpio_dev->gc.set			= amd_gpio_set_value;
+	gpio_dev->gc.set_debounce	= amd_gpio_set_debounce;
+	gpio_dev->gc.dbg_show		= amd_gpio_dbg_show;
+
+	gpio_dev->gc.base			= 0;
+	gpio_dev->gc.label			= pdev->name;
+	gpio_dev->gc.owner			= THIS_MODULE;
+	gpio_dev->gc.dev			= &pdev->dev;
+	gpio_dev->gc.ngpio			= TOTAL_NUMBER_OF_PINS;
+#if defined(CONFIG_OF_GPIO)
+	gpio_dev->gc.of_node			= pdev->dev.of_node;
+#endif
+
+	gpio_dev->groups = kerncz_groups;
+	gpio_dev->ngroups = ARRAY_SIZE(kerncz_groups);
+
+	amd_pinctrl_desc.name = dev_name(&pdev->dev);
+	gpio_dev->pctrl = pinctrl_register(&amd_pinctrl_desc,
+					&pdev->dev, gpio_dev);
+	if (!gpio_dev->pctrl) {
+		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
+		return -ENODEV;
+	}
+
+	ret = gpiochip_add(&gpio_dev->gc);
+	if (ret)
+		goto out1;
+
+	ret = gpiochip_add_pin_range(&gpio_dev->gc, dev_name(&pdev->dev),
+				0, 0, TOTAL_NUMBER_OF_PINS);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to add pin range\n");
+		goto out2;
+	}
+
+	ret = gpiochip_irqchip_add(&gpio_dev->gc,
+				&amd_gpio_irqchip,
+				0,
+				handle_simple_irq,
+				IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(&pdev->dev, "could not add irqchip\n");
+		ret = -ENODEV;
+		goto out2;
+	}
+
+	gpiochip_set_chained_irqchip(&gpio_dev->gc,
+				 &amd_gpio_irqchip,
+				 irq_base,
+				 amd_gpio_irq_handler);
+
+	platform_set_drvdata(pdev, gpio_dev);
+
+	dev_dbg(&pdev->dev, "amd gpio driver loaded\n");
+	return ret;
+
+out2:
+	gpiochip_remove(&gpio_dev->gc);
+
+out1:
+	pinctrl_unregister(gpio_dev->pctrl);
+	return ret;
+}
+
+static int amd_gpio_remove(struct platform_device *pdev)
+{
+	struct amd_gpio *gpio_dev;
+
+	gpio_dev = platform_get_drvdata(pdev);
+
+	gpiochip_remove(&gpio_dev->gc);
+	pinctrl_unregister(gpio_dev->pctrl);
+
+	return 0;
+}
+
+static const struct acpi_device_id amd_gpio_acpi_match[] = {
+	{ "AMD0030", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, amd_gpio_acpi_match);
+
+static struct platform_driver amd_gpio_driver = {
+	.driver		= {
+		.name	= "amd_gpio",
+		.owner	= THIS_MODULE,
+		.acpi_match_table = ACPI_PTR(amd_gpio_acpi_match),
+	},
+	.probe		= amd_gpio_probe,
+	.remove		= amd_gpio_remove,
+};
+
+module_platform_driver(amd_gpio_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Ken Xue <Ken.Xue@amd.com>, Jeff Wu <Jeff.Wu@amd.com>");
+MODULE_DESCRIPTION("AMD GPIO pinctrl driver");
diff --git a/drivers/pinctrl/pinctrl-amd.h b/drivers/pinctrl/pinctrl-amd.h
new file mode 100644
index 000000000000..7bfea47dbb47
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-amd.h
@@ -0,0 +1,261 @@
+/*
+ * GPIO driver for AMD
+ *
+ * Copyright (c) 2014,2015 Ken Xue <Ken.Xue@amd.com>
+ *		Jeff Wu <Jeff.Wu@amd.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ */
+
+#ifndef _PINCTRL_AMD_H
+#define _PINCTRL_AMD_H
+
+#define TOTAL_NUMBER_OF_PINS	192
+#define AMD_GPIO_PINS_PER_BANK  64
+#define AMD_GPIO_TOTAL_BANKS    3
+
+#define AMD_GPIO_PINS_BANK0     63
+#define AMD_GPIO_PINS_BANK1     64
+#define AMD_GPIO_PINS_BANK2     56
+
+#define WAKE_INT_MASTER_REG 0xfc
+#define EOI_MASK (1 << 29)
+
+#define WAKE_INT_STATUS_REG0 0x2f8
+#define WAKE_INT_STATUS_REG1 0x2fc
+
+#define DB_TMR_OUT_OFF			0
+#define DB_TMR_OUT_UNIT_OFF		4
+#define DB_CNTRL_OFF			5
+#define DB_TMR_LARGE_OFF		7
+#define LEVEL_TRIG_OFF			8
+#define ACTIVE_LEVEL_OFF		9
+#define INTERRUPT_ENABLE_OFF		11
+#define INTERRUPT_MASK_OFF		12
+#define WAKE_CNTRL_OFF			13
+#define PIN_STS_OFF			16
+#define DRV_STRENGTH_SEL_OFF		17
+#define PULL_UP_SEL_OFF			19
+#define PULL_UP_ENABLE_OFF		20
+#define PULL_DOWN_ENABLE_OFF		21
+#define OUTPUT_VALUE_OFF		22
+#define OUTPUT_ENABLE_OFF		23
+#define SW_CNTRL_IN_OFF			24
+#define SW_CNTRL_EN_OFF			25
+#define INTERRUPT_STS_OFF		28
+#define WAKE_STS_OFF			29
+
+#define DB_TMR_OUT_MASK	0xFUL
+#define DB_CNTRl_MASK	0x3UL
+#define ACTIVE_LEVEL_MASK	0x3UL
+#define DRV_STRENGTH_SEL_MASK	0x3UL
+
+#define DB_TYPE_NO_DEBOUNCE               0x0UL
+#define DB_TYPE_PRESERVE_LOW_GLITCH       0x1UL
+#define DB_TYPE_PRESERVE_HIGH_GLITCH      0x2UL
+#define DB_TYPE_REMOVE_GLITCH             0x3UL
+
+#define EDGE_TRAGGER	0x0UL
+#define LEVEL_TRIGGER	0x1UL
+
+#define ACTIVE_HIGH	0x0UL
+#define ACTIVE_LOW	0x1UL
+#define BOTH_EADGE	0x2UL
+
+#define ENABLE_INTERRUPT	0x1UL
+#define DISABLE_INTERRUPT	0x0UL
+
+#define ENABLE_INTERRUPT_MASK	0x0UL
+#define DISABLE_INTERRUPT_MASK	0x1UL
+
+#define CLR_INTR_STAT	0x1UL
+
+struct amd_pingroup {
+	const char *name;
+	const unsigned *pins;
+	unsigned npins;
+};
+
+struct amd_function {
+	const char *name;
+	const char * const *groups;
+	unsigned ngroups;
+};
+
+struct amd_gpio {
+	spinlock_t              lock;
+	void __iomem            *base;
+
+	const struct amd_pingroup *groups;
+	u32 ngroups;
+	struct pinctrl_dev *pctrl;
+	struct gpio_chip        gc;
+	struct resource         *res;
+	struct platform_device  *pdev;
+};
+
+/*  KERNCZ configuration*/
+static const struct pinctrl_pin_desc kerncz_pins[] = {
+	PINCTRL_PIN(0, "GPIO_0"),
+	PINCTRL_PIN(1, "GPIO_1"),
+	PINCTRL_PIN(2, "GPIO_2"),
+	PINCTRL_PIN(3, "GPIO_3"),
+	PINCTRL_PIN(4, "GPIO_4"),
+	PINCTRL_PIN(5, "GPIO_5"),
+	PINCTRL_PIN(6, "GPIO_6"),
+	PINCTRL_PIN(7, "GPIO_7"),
+	PINCTRL_PIN(8, "GPIO_8"),
+	PINCTRL_PIN(9, "GPIO_9"),
+	PINCTRL_PIN(10, "GPIO_10"),
+	PINCTRL_PIN(11, "GPIO_11"),
+	PINCTRL_PIN(12, "GPIO_12"),
+	PINCTRL_PIN(13, "GPIO_13"),
+	PINCTRL_PIN(14, "GPIO_14"),
+	PINCTRL_PIN(15, "GPIO_15"),
+	PINCTRL_PIN(16, "GPIO_16"),
+	PINCTRL_PIN(17, "GPIO_17"),
+	PINCTRL_PIN(18, "GPIO_18"),
+	PINCTRL_PIN(19, "GPIO_19"),
+	PINCTRL_PIN(20, "GPIO_20"),
+	PINCTRL_PIN(23, "GPIO_23"),
+	PINCTRL_PIN(24, "GPIO_24"),
+	PINCTRL_PIN(25, "GPIO_25"),
+	PINCTRL_PIN(26, "GPIO_26"),
+	PINCTRL_PIN(39, "GPIO_39"),
+	PINCTRL_PIN(40, "GPIO_40"),
+	PINCTRL_PIN(43, "GPIO_42"),
+	PINCTRL_PIN(46, "GPIO_46"),
+	PINCTRL_PIN(47, "GPIO_47"),
+	PINCTRL_PIN(48, "GPIO_48"),
+	PINCTRL_PIN(49, "GPIO_49"),
+	PINCTRL_PIN(50, "GPIO_50"),
+	PINCTRL_PIN(51, "GPIO_51"),
+	PINCTRL_PIN(52, "GPIO_52"),
+	PINCTRL_PIN(53, "GPIO_53"),
+	PINCTRL_PIN(54, "GPIO_54"),
+	PINCTRL_PIN(55, "GPIO_55"),
+	PINCTRL_PIN(56, "GPIO_56"),
+	PINCTRL_PIN(57, "GPIO_57"),
+	PINCTRL_PIN(58, "GPIO_58"),
+	PINCTRL_PIN(59, "GPIO_59"),
+	PINCTRL_PIN(60, "GPIO_60"),
+	PINCTRL_PIN(61, "GPIO_61"),
+	PINCTRL_PIN(62, "GPIO_62"),
+	PINCTRL_PIN(64, "GPIO_64"),
+	PINCTRL_PIN(65, "GPIO_65"),
+	PINCTRL_PIN(66, "GPIO_66"),
+	PINCTRL_PIN(68, "GPIO_68"),
+	PINCTRL_PIN(69, "GPIO_69"),
+	PINCTRL_PIN(70, "GPIO_70"),
+	PINCTRL_PIN(71, "GPIO_71"),
+	PINCTRL_PIN(72, "GPIO_72"),
+	PINCTRL_PIN(74, "GPIO_74"),
+	PINCTRL_PIN(75, "GPIO_75"),
+	PINCTRL_PIN(76, "GPIO_76"),
+	PINCTRL_PIN(84, "GPIO_84"),
+	PINCTRL_PIN(85, "GPIO_85"),
+	PINCTRL_PIN(86, "GPIO_86"),
+	PINCTRL_PIN(87, "GPIO_87"),
+	PINCTRL_PIN(88, "GPIO_88"),
+	PINCTRL_PIN(89, "GPIO_89"),
+	PINCTRL_PIN(90, "GPIO_90"),
+	PINCTRL_PIN(91, "GPIO_91"),
+	PINCTRL_PIN(92, "GPIO_92"),
+	PINCTRL_PIN(93, "GPIO_93"),
+	PINCTRL_PIN(95, "GPIO_95"),
+	PINCTRL_PIN(96, "GPIO_96"),
+	PINCTRL_PIN(97, "GPIO_97"),
+	PINCTRL_PIN(98, "GPIO_98"),
+	PINCTRL_PIN(99, "GPIO_99"),
+	PINCTRL_PIN(100, "GPIO_100"),
+	PINCTRL_PIN(101, "GPIO_101"),
+	PINCTRL_PIN(102, "GPIO_102"),
+	PINCTRL_PIN(113, "GPIO_113"),
+	PINCTRL_PIN(114, "GPIO_114"),
+	PINCTRL_PIN(115, "GPIO_115"),
+	PINCTRL_PIN(116, "GPIO_116"),
+	PINCTRL_PIN(117, "GPIO_117"),
+	PINCTRL_PIN(118, "GPIO_118"),
+	PINCTRL_PIN(119, "GPIO_119"),
+	PINCTRL_PIN(120, "GPIO_120"),
+	PINCTRL_PIN(121, "GPIO_121"),
+	PINCTRL_PIN(122, "GPIO_122"),
+	PINCTRL_PIN(126, "GPIO_126"),
+	PINCTRL_PIN(129, "GPIO_129"),
+	PINCTRL_PIN(130, "GPIO_130"),
+	PINCTRL_PIN(131, "GPIO_131"),
+	PINCTRL_PIN(132, "GPIO_132"),
+	PINCTRL_PIN(133, "GPIO_133"),
+	PINCTRL_PIN(135, "GPIO_135"),
+	PINCTRL_PIN(136, "GPIO_136"),
+	PINCTRL_PIN(137, "GPIO_137"),
+	PINCTRL_PIN(138, "GPIO_138"),
+	PINCTRL_PIN(139, "GPIO_139"),
+	PINCTRL_PIN(140, "GPIO_140"),
+	PINCTRL_PIN(141, "GPIO_141"),
+	PINCTRL_PIN(142, "GPIO_142"),
+	PINCTRL_PIN(143, "GPIO_143"),
+	PINCTRL_PIN(144, "GPIO_144"),
+	PINCTRL_PIN(145, "GPIO_145"),
+	PINCTRL_PIN(146, "GPIO_146"),
+	PINCTRL_PIN(147, "GPIO_147"),
+	PINCTRL_PIN(148, "GPIO_148"),
+	PINCTRL_PIN(166, "GPIO_166"),
+	PINCTRL_PIN(167, "GPIO_167"),
+	PINCTRL_PIN(168, "GPIO_168"),
+	PINCTRL_PIN(169, "GPIO_169"),
+	PINCTRL_PIN(170, "GPIO_170"),
+	PINCTRL_PIN(171, "GPIO_171"),
+	PINCTRL_PIN(172, "GPIO_172"),
+	PINCTRL_PIN(173, "GPIO_173"),
+	PINCTRL_PIN(174, "GPIO_174"),
+	PINCTRL_PIN(175, "GPIO_175"),
+	PINCTRL_PIN(176, "GPIO_176"),
+	PINCTRL_PIN(177, "GPIO_177"),
+};
+
+static const unsigned i2c0_pins[] = {145, 146};
+static const unsigned i2c1_pins[] = {147, 148};
+static const unsigned i2c2_pins[] = {113, 114};
+static const unsigned i2c3_pins[] = {19, 20};
+
+static const unsigned uart0_pins[] = {135, 136, 137, 138, 139};
+static const unsigned uart1_pins[] = {140, 141, 142, 143, 144};
+
+static const struct amd_pingroup kerncz_groups[] = {
+	{
+		.name = "i2c0",
+		.pins = i2c0_pins,
+		.npins = 2,
+	},
+	{
+		.name = "i2c1",
+		.pins = i2c1_pins,
+		.npins = 2,
+	},
+	{
+		.name = "i2c2",
+		.pins = i2c2_pins,
+		.npins = 2,
+	},
+	{
+		.name = "i2c3",
+		.pins = i2c3_pins,
+		.npins = 2,
+	},
+	{
+		.name = "uart0",
+		.pins = uart0_pins,
+		.npins = 9,
+	},
+	{
+		.name = "uart1",
+		.pins = uart1_pins,
+		.npins = 5,
+	},
+};
+
+#endif
diff --git a/drivers/pinctrl/pinctrl-as3722.c b/drivers/pinctrl/pinctrl-as3722.c
index 169b1bfa00c8..db0571ffbe99 100644
--- a/drivers/pinctrl/pinctrl-as3722.c
+++ b/drivers/pinctrl/pinctrl-as3722.c
@@ -625,7 +625,7 @@ static int as3722_pinctrl_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id as3722_pinctrl_of_match[] = {
+static const struct of_device_id as3722_pinctrl_of_match[] = {
 	{ .compatible = "ams,as3722-pinctrl", },
 	{ },
 };
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index f4cd0b9b2438..2f797cb7e205 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -451,18 +451,18 @@ static enum at91_mux at91_mux_get_periph(void __iomem *pio, unsigned mask)
 
 static bool at91_mux_get_deglitch(void __iomem *pio, unsigned pin)
 {
-	return (__raw_readl(pio + PIO_IFSR) >> pin) & 0x1;
+	return (readl_relaxed(pio + PIO_IFSR) >> pin) & 0x1;
 }
 
 static void at91_mux_set_deglitch(void __iomem *pio, unsigned mask, bool is_on)
 {
-	__raw_writel(mask, pio + (is_on ? PIO_IFER : PIO_IFDR));
+	writel_relaxed(mask, pio + (is_on ? PIO_IFER : PIO_IFDR));
 }
 
 static bool at91_mux_pio3_get_deglitch(void __iomem *pio, unsigned pin)
 {
-	if ((__raw_readl(pio + PIO_IFSR) >> pin) & 0x1)
-		return !((__raw_readl(pio + PIO_IFSCSR) >> pin) & 0x1);
+	if ((readl_relaxed(pio + PIO_IFSR) >> pin) & 0x1)
+		return !((readl_relaxed(pio + PIO_IFSCSR) >> pin) & 0x1);
 
 	return false;
 }
@@ -470,55 +470,55 @@ static bool at91_mux_pio3_get_deglitch(void __iomem *pio, unsigned pin)
 static void at91_mux_pio3_set_deglitch(void __iomem *pio, unsigned mask, bool is_on)
 {
 	if (is_on)
-		__raw_writel(mask, pio + PIO_IFSCDR);
+		writel_relaxed(mask, pio + PIO_IFSCDR);
 	at91_mux_set_deglitch(pio, mask, is_on);
 }
 
 static bool at91_mux_pio3_get_debounce(void __iomem *pio, unsigned pin, u32 *div)
 {
-	*div = __raw_readl(pio + PIO_SCDR);
+	*div = readl_relaxed(pio + PIO_SCDR);
 
-	return ((__raw_readl(pio + PIO_IFSR) >> pin) & 0x1) &&
-	       ((__raw_readl(pio + PIO_IFSCSR) >> pin) & 0x1);
+	return ((readl_relaxed(pio + PIO_IFSR) >> pin) & 0x1) &&
+	       ((readl_relaxed(pio + PIO_IFSCSR) >> pin) & 0x1);
 }
 
 static void at91_mux_pio3_set_debounce(void __iomem *pio, unsigned mask,
 				bool is_on, u32 div)
 {
 	if (is_on) {
-		__raw_writel(mask, pio + PIO_IFSCER);
-		__raw_writel(div & PIO_SCDR_DIV, pio + PIO_SCDR);
-		__raw_writel(mask, pio + PIO_IFER);
+		writel_relaxed(mask, pio + PIO_IFSCER);
+		writel_relaxed(div & PIO_SCDR_DIV, pio + PIO_SCDR);
+		writel_relaxed(mask, pio + PIO_IFER);
 	} else
-		__raw_writel(mask, pio + PIO_IFSCDR);
+		writel_relaxed(mask, pio + PIO_IFSCDR);
 }
 
 static bool at91_mux_pio3_get_pulldown(void __iomem *pio, unsigned pin)
 {
-	return !((__raw_readl(pio + PIO_PPDSR) >> pin) & 0x1);
+	return !((readl_relaxed(pio + PIO_PPDSR) >> pin) & 0x1);
 }
 
 static void at91_mux_pio3_set_pulldown(void __iomem *pio, unsigned mask, bool is_on)
 {
 	if (is_on)
-		__raw_writel(mask, pio + PIO_PUDR);
+		writel_relaxed(mask, pio + PIO_PUDR);
 
-	__raw_writel(mask, pio + (is_on ? PIO_PPDER : PIO_PPDDR));
+	writel_relaxed(mask, pio + (is_on ? PIO_PPDER : PIO_PPDDR));
 }
 
 static void at91_mux_pio3_disable_schmitt_trig(void __iomem *pio, unsigned mask)
 {
-	__raw_writel(__raw_readl(pio + PIO_SCHMITT) | mask, pio + PIO_SCHMITT);
+	writel_relaxed(readl_relaxed(pio + PIO_SCHMITT) | mask, pio + PIO_SCHMITT);
 }
 
 static bool at91_mux_pio3_get_schmitt_trig(void __iomem *pio, unsigned pin)
 {
-	return (__raw_readl(pio + PIO_SCHMITT) >> pin) & 0x1;
+	return (readl_relaxed(pio + PIO_SCHMITT) >> pin) & 0x1;
 }
 
 static inline u32 read_drive_strength(void __iomem *reg, unsigned pin)
 {
-	unsigned tmp = __raw_readl(reg);
+	unsigned tmp = readl_relaxed(reg);
 
 	tmp = tmp >> two_bit_pin_value_shift_amount(pin);
 
@@ -554,13 +554,13 @@ static unsigned at91_mux_sam9x5_get_drivestrength(void __iomem *pio,
 
 static void set_drive_strength(void __iomem *reg, unsigned pin, u32 strength)
 {
-	unsigned tmp = __raw_readl(reg);
+	unsigned tmp = readl_relaxed(reg);
 	unsigned shift = two_bit_pin_value_shift_amount(pin);
 
 	tmp &= ~(DRIVE_STRENGTH_MASK  <<  shift);
 	tmp |= strength << shift;
 
-	__raw_writel(tmp, reg);
+	writel_relaxed(tmp, reg);
 }
 
 static void at91_mux_sama5d3_set_drivestrength(void __iomem *pio, unsigned pin,
@@ -1114,7 +1114,7 @@ static int at91_pinctrl_parse_functions(struct device_node *np,
 	return 0;
 }
 
-static struct of_device_id at91_pinctrl_of_match[] = {
+static const struct of_device_id at91_pinctrl_of_match[] = {
 	{ .compatible = "atmel,sama5d3-pinctrl", .data = &sama5d3_ops },
 	{ .compatible = "atmel,at91sam9x5-pinctrl", .data = &at91sam9x5_ops },
 	{ .compatible = "atmel,at91rm9200-pinctrl", .data = &at91rm9200_ops },
@@ -1240,8 +1240,7 @@ static int at91_pinctrl_probe(struct platform_device *pdev)
 
 	if (!info->pctl) {
 		dev_err(&pdev->dev, "could not register AT91 pinctrl driver\n");
-		ret = -EINVAL;
-		goto err;
+		return -EINVAL;
 	}
 
 	/* We will handle a range of GPIO pins */
@@ -1252,9 +1251,6 @@ static int at91_pinctrl_probe(struct platform_device *pdev)
 	dev_info(&pdev->dev, "initialized AT91 pinctrl driver\n");
 
 	return 0;
-
-err:
-	return ret;
 }
 
 static int at91_pinctrl_remove(struct platform_device *pdev)
@@ -1477,28 +1473,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);
 }
 
@@ -1538,9 +1531,9 @@ void at91_pinctrl_gpio_suspend(void)
 
 		pio = gpio_chips[i]->regbase;
 
-		backups[i] = __raw_readl(pio + PIO_IMR);
-		__raw_writel(backups[i], pio + PIO_IDR);
-		__raw_writel(wakeups[i], pio + PIO_IER);
+		backups[i] = readl_relaxed(pio + PIO_IMR);
+		writel_relaxed(backups[i], pio + PIO_IDR);
+		writel_relaxed(wakeups[i], pio + PIO_IER);
 
 		if (!wakeups[i])
 			clk_disable_unprepare(gpio_chips[i]->clock);
@@ -1565,8 +1558,8 @@ void at91_pinctrl_gpio_resume(void)
 		if (!wakeups[i])
 			clk_prepare_enable(gpio_chips[i]->clock);
 
-		__raw_writel(wakeups[i], pio + PIO_IDR);
-		__raw_writel(backups[i], pio + PIO_IER);
+		writel_relaxed(wakeups[i], pio + PIO_IDR);
+		writel_relaxed(backups[i], pio + PIO_IER);
 	}
 }
 
@@ -1577,8 +1570,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,
@@ -1697,7 +1690,7 @@ static struct gpio_chip at91_gpio_template = {
 	.ngpio			= MAX_NB_GPIO_PER_BANK,
 };
 
-static struct of_device_id at91_gpio_of_match[] = {
+static const struct of_device_id at91_gpio_of_match[] = {
 	{ .compatible = "atmel,at91sam9x5-gpio", .data = &at91sam9x5_ops, },
 	{ .compatible = "atmel,at91rm9200-gpio", .data = &at91rm9200_ops },
 	{ /* sentinel */ }
diff --git a/drivers/pinctrl/pinctrl-lantiq.h b/drivers/pinctrl/pinctrl-lantiq.h
index c7cfad5527d7..eb89ba045228 100644
--- a/drivers/pinctrl/pinctrl-lantiq.h
+++ b/drivers/pinctrl/pinctrl-lantiq.h
@@ -193,4 +193,4 @@ enum ltq_pin {
 extern int ltq_pinctrl_register(struct platform_device *pdev,
 				   struct ltq_pinmux_info *info);
 extern int ltq_pinctrl_unregister(struct platform_device *pdev);
-#endif	/* __PINCTRL_PXA3XX_H */
+#endif	/* __PINCTRL_LANTIQ_H */
diff --git a/drivers/pinctrl/pinctrl-palmas.c b/drivers/pinctrl/pinctrl-palmas.c
index 26461e30f0ae..2631df0504bd 100644
--- a/drivers/pinctrl/pinctrl-palmas.c
+++ b/drivers/pinctrl/pinctrl-palmas.c
@@ -987,7 +987,7 @@ static struct palmas_pinctrl_data tps80036_pinctrl_data = {
 	.num_pin_groups = ARRAY_SIZE(tps80036_pingroups),
 };
 
-static struct of_device_id palmas_pinctrl_of_match[] = {
+static const struct of_device_id palmas_pinctrl_of_match[] = {
 	{ .compatible = "ti,palmas-pinctrl", .data = &tps65913_pinctrl_data},
 	{ .compatible = "ti,tps65913-pinctrl", .data = &tps65913_pinctrl_data},
 	{ .compatible = "ti,tps80036-pinctrl", .data = &tps80036_pinctrl_data},
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index 69e84427f913..13b45f297727 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -1501,7 +1501,7 @@ static void pcs_free_resources(struct pcs_device *pcs)
 		}							\
 	} while (0);
 
-static struct of_device_id pcs_of_match[];
+static const struct of_device_id pcs_of_match[];
 
 static int pcs_add_gpio_func(struct device_node *node, struct pcs_device *pcs)
 {
@@ -2000,7 +2000,7 @@ static const struct pcs_soc_data pinconf_single = {
 	.flags = PCS_FEAT_PINCONF,
 };
 
-static struct of_device_id pcs_of_match[] = {
+static const struct of_device_id pcs_of_match[] = {
 	{ .compatible = "ti,omap3-padconf", .data = &pinctrl_single_omap_wkup },
 	{ .compatible = "ti,omap4-padconf", .data = &pinctrl_single_omap_wkup },
 	{ .compatible = "ti,omap5-padconf", .data = &pinctrl_single_omap_wkup },
diff --git a/drivers/pinctrl/pinctrl-st.c b/drivers/pinctrl/pinctrl-st.c
index 9e5ec00084bb..65bf73b70e34 100644
--- a/drivers/pinctrl/pinctrl-st.c
+++ b/drivers/pinctrl/pinctrl-st.c
@@ -206,6 +206,8 @@
 #define gpio_chip_to_bank(chip) \
 		container_of(chip, struct st_gpio_bank, gpio_chip)
 
+#define pc_to_bank(pc) \
+		container_of(pc, struct st_gpio_bank, pc)
 
 enum st_retime_style {
 	st_retime_style_none,
@@ -398,6 +400,16 @@ static const struct st_pctl_data stih407_flashdata = {
 	.rt = 100,
 };
 
+static struct st_pio_control *st_get_pio_control(
+			struct pinctrl_dev *pctldev, int pin)
+{
+	struct pinctrl_gpio_range *range =
+			 pinctrl_find_gpio_range_from_pin(pctldev, pin);
+	struct st_gpio_bank *bank = gpio_range_to_bank(range);
+
+	return &bank->pc;
+}
+
 /* Low level functions.. */
 static inline int st_gpio_bank(int gpio)
 {
@@ -460,6 +472,20 @@ static void st_pctl_set_function(struct st_pio_control *pc,
 	regmap_field_write(alt, val);
 }
 
+static unsigned int st_pctl_get_pin_function(struct st_pio_control *pc, int pin)
+{
+	struct regmap_field *alt = pc->alt;
+	unsigned int val;
+	int offset = pin * 4;
+
+	if (!alt)
+		return 0;
+
+	regmap_field_read(alt, &val);
+
+	return (val >> offset) & 0xf;
+}
+
 static unsigned long st_pinconf_delay_to_bit(unsigned int delay,
 	const struct st_pctl_data *data, unsigned long config)
 {
@@ -757,6 +783,35 @@ static int st_gpio_direction_output(struct gpio_chip *chip,
 	return 0;
 }
 
+static int st_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct st_gpio_bank *bank = gpio_chip_to_bank(chip);
+	struct st_pio_control pc = bank->pc;
+	unsigned long config;
+	unsigned int direction = 0;
+	unsigned int function;
+	unsigned int value;
+	int i = 0;
+
+	/* Alternate function direction is handled by Pinctrl */
+	function = st_pctl_get_pin_function(&pc, offset);
+	if (function) {
+		st_pinconf_get_direction(&pc, offset, &config);
+		return !ST_PINCONF_UNPACK_OE(config);
+	}
+
+	/*
+	 * GPIO direction is handled differently
+	 * - See st_gpio_direction() above for an explanation
+	 */
+	for (i = 0; i <= 2; i++) {
+		value = readl(bank->base + REG_PIO_PC(i));
+		direction |= ((value >> offset) & 0x1) << i;
+	}
+
+	return (direction == ST_GPIO_DIRECTION_IN);
+}
+
 static int st_gpio_xlate(struct gpio_chip *gc,
 			const struct of_phandle_args *gpiospec, u32 *flags)
 {
@@ -904,16 +959,6 @@ static int st_pmx_get_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static struct st_pio_control *st_get_pio_control(
-			struct pinctrl_dev *pctldev, int pin)
-{
-	struct pinctrl_gpio_range *range =
-			 pinctrl_find_gpio_range_from_pin(pctldev, pin);
-	struct st_gpio_bank *bank = gpio_range_to_bank(range);
-
-	return &bank->pc;
-}
-
 static int st_pmx_set_mux(struct pinctrl_dev *pctldev, unsigned fselector,
 			unsigned group)
 {
@@ -1011,17 +1056,30 @@ static int st_pinconf_get(struct pinctrl_dev *pctldev,
 static void st_pinconf_dbg_show(struct pinctrl_dev *pctldev,
 				   struct seq_file *s, unsigned pin_id)
 {
+	struct st_pio_control *pc;
 	unsigned long config;
+	unsigned int function;
+	int offset = st_gpio_pin(pin_id);
+	char f[16];
 
 	mutex_unlock(&pctldev->mutex);
+	pc = st_get_pio_control(pctldev, pin_id);
 	st_pinconf_get(pctldev, pin_id, &config);
 	mutex_lock(&pctldev->mutex);
-	seq_printf(s, "[OE:%ld,PU:%ld,OD:%ld]\n"
+
+	function = st_pctl_get_pin_function(pc, offset);
+	if (function)
+		snprintf(f, 10, "Alt Fn %d", function);
+	else
+		snprintf(f, 5, "GPIO");
+
+	seq_printf(s, "[OE:%d,PU:%ld,OD:%ld]\t%s\n"
 		"\t\t[retime:%ld,invclk:%ld,clknotdat:%ld,"
 		"de:%ld,rt-clk:%ld,rt-delay:%ld]",
-		ST_PINCONF_UNPACK_OE(config),
+		!st_gpio_get_direction(&pc_to_bank(pc)->gpio_chip, offset),
 		ST_PINCONF_UNPACK_PU(config),
 		ST_PINCONF_UNPACK_OD(config),
+		f,
 		ST_PINCONF_UNPACK_RT(config),
 		ST_PINCONF_UNPACK_RT_INVERTCLK(config),
 		ST_PINCONF_UNPACK_RT_CLKNOTDATA(config),
@@ -1438,6 +1496,7 @@ static struct gpio_chip st_gpio_template = {
 	.set			= st_gpio_set,
 	.direction_input	= st_gpio_direction_input,
 	.direction_output	= st_gpio_direction_output,
+	.get_direction		= st_gpio_get_direction,
 	.ngpio			= ST_GPIO_PINS_PER_BANK,
 	.of_gpio_n_cells	= 1,
 	.of_xlate		= st_gpio_xlate,
@@ -1531,7 +1590,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 	return 0;
 }
 
-static struct of_device_id st_pctl_of_match[] = {
+static const struct of_device_id st_pctl_of_match[] = {
 	{ .compatible = "st,stih415-sbc-pinctrl", .data = &stih415_sbc_data },
 	{ .compatible = "st,stih415-rear-pinctrl", .data = &stih415_rear_data },
 	{ .compatible = "st,stih415-left-pinctrl", .data = &stih415_left_data },
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index e5949d51bc52..4c95c2024a1c 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -103,6 +103,7 @@ static const struct cfg_param {
 	{"nvidia,lock",			TEGRA_PINCONF_PARAM_LOCK},
 	{"nvidia,io-reset",		TEGRA_PINCONF_PARAM_IORESET},
 	{"nvidia,rcv-sel",		TEGRA_PINCONF_PARAM_RCV_SEL},
+	{"nvidia,io-hv",		TEGRA_PINCONF_PARAM_RCV_SEL},
 	{"nvidia,high-speed-mode",	TEGRA_PINCONF_PARAM_HIGH_SPEED_MODE},
 	{"nvidia,schmitt",		TEGRA_PINCONF_PARAM_SCHMITT},
 	{"nvidia,low-power-mode",	TEGRA_PINCONF_PARAM_LOW_POWER_MODE},
@@ -348,14 +349,24 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_HIGH_SPEED_MODE:
-		*bank = g->drv_bank;
-		*reg = g->drv_reg;
+		if (pmx->soc->hsm_in_mux) {
+			*bank = g->mux_bank;
+			*reg = g->mux_reg;
+		} else {
+			*bank = g->drv_bank;
+			*reg = g->drv_reg;
+		}
 		*bit = g->hsm_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_SCHMITT:
-		*bank = g->drv_bank;
-		*reg = g->drv_reg;
+		if (pmx->soc->schmitt_in_mux) {
+			*bank = g->mux_bank;
+			*reg = g->mux_reg;
+		} else {
+			*bank = g->drv_bank;
+			*reg = g->drv_reg;
+		}
 		*bit = g->schmitt_bit;
 		*width = 1;
 		break;
@@ -390,8 +401,13 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		*width = g->slwr_width;
 		break;
 	case TEGRA_PINCONF_PARAM_DRIVE_TYPE:
-		*bank = g->drv_bank;
-		*reg = g->drv_reg;
+		if (pmx->soc->drvtype_in_mux) {
+			*bank = g->mux_bank;
+			*reg = g->mux_reg;
+		} else {
+			*bank = g->drv_bank;
+			*reg = g->drv_reg;
+		}
 		*bit = g->drvtype_bit;
 		*width = 2;
 		break;
diff --git a/drivers/pinctrl/pinctrl-tegra.h b/drivers/pinctrl/pinctrl-tegra.h
index 8d94d1332e7b..1615db7e3a4b 100644
--- a/drivers/pinctrl/pinctrl-tegra.h
+++ b/drivers/pinctrl/pinctrl-tegra.h
@@ -139,26 +139,26 @@ struct tegra_pingroup {
 	u32 pupd_bank:2;
 	u32 tri_bank:2;
 	u32 drv_bank:2;
-	u32 mux_bit:6;
-	u32 pupd_bit:6;
-	u32 tri_bit:6;
-	u32 einput_bit:6;
-	u32 odrain_bit:6;
-	u32 lock_bit:6;
-	u32 ioreset_bit:6;
-	u32 rcv_sel_bit:6;
-	u32 hsm_bit:6;
-	u32 schmitt_bit:6;
-	u32 lpmd_bit:6;
-	u32 drvdn_bit:6;
-	u32 drvup_bit:6;
-	u32 slwr_bit:6;
-	u32 slwf_bit:6;
-	u32 drvtype_bit:6;
-	u32 drvdn_width:6;
-	u32 drvup_width:6;
-	u32 slwr_width:6;
-	u32 slwf_width:6;
+	s32 mux_bit:6;
+	s32 pupd_bit:6;
+	s32 tri_bit:6;
+	s32 einput_bit:6;
+	s32 odrain_bit:6;
+	s32 lock_bit:6;
+	s32 ioreset_bit:6;
+	s32 rcv_sel_bit:6;
+	s32 hsm_bit:6;
+	s32 schmitt_bit:6;
+	s32 lpmd_bit:6;
+	s32 drvdn_bit:6;
+	s32 drvup_bit:6;
+	s32 slwr_bit:6;
+	s32 slwf_bit:6;
+	s32 drvtype_bit:6;
+	s32 drvdn_width:6;
+	s32 drvup_width:6;
+	s32 slwr_width:6;
+	s32 slwf_width:6;
 };
 
 /**
@@ -182,6 +182,9 @@ struct tegra_pinctrl_soc_data {
 	unsigned nfunctions;
 	const struct tegra_pingroup *groups;
 	unsigned ngroups;
+	bool hsm_in_mux;
+	bool schmitt_in_mux;
+	bool drvtype_in_mux;
 };
 
 int tegra_pinctrl_probe(struct platform_device *pdev,
diff --git a/drivers/pinctrl/pinctrl-tegra114.c b/drivers/pinctrl/pinctrl-tegra114.c
index 52e4ec6386b4..05e49d5137ab 100644
--- a/drivers/pinctrl/pinctrl-tegra114.c
+++ b/drivers/pinctrl/pinctrl-tegra114.c
@@ -1547,6 +1547,7 @@ static struct tegra_function tegra114_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 #define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
 #define PINGROUP_BIT_Y(b)		(b)
@@ -1572,20 +1573,17 @@ static struct tegra_function tegra114_functions[] = {
 		.tri_reg = PINGROUP_REG(r),				\
 		.tri_bank = 1,						\
 		.tri_bit = 4,						\
-		.einput_bit = PINGROUP_BIT_Y(5),			\
+		.einput_bit = 5,					\
 		.odrain_bit = PINGROUP_BIT_##od(6),			\
-		.lock_bit = PINGROUP_BIT_Y(7),				\
+		.lock_bit = 7,						\
 		.ioreset_bit = PINGROUP_BIT_##ior(8),			\
 		.rcv_sel_bit = PINGROUP_BIT_##rcv_sel(9),		\
 		.drv_reg = -1,						\
 	}
 
-#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
-
-#define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,		\
-		     drvdn_b, drvdn_w, drvup_b, drvup_w,		\
-		     slwr_b, slwr_w, slwf_b, slwf_w,			\
-		     drvtype)						\
+#define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b,	\
+		     drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w,		\
+		     slwf_b, slwf_w, drvtype)				\
 	{								\
 		.name = "drive_" #pg_name,				\
 		.pins = drive_##pg_name##_pins,				\
@@ -1843,6 +1841,9 @@ static const struct tegra_pinctrl_soc_data tegra114_pinctrl = {
 	.nfunctions = ARRAY_SIZE(tegra114_functions),
 	.groups = tegra114_groups,
 	.ngroups = ARRAY_SIZE(tegra114_groups),
+	.hsm_in_mux = false,
+	.schmitt_in_mux = false,
+	.drvtype_in_mux = false,
 };
 
 static int tegra114_pinctrl_probe(struct platform_device *pdev)
diff --git a/drivers/pinctrl/pinctrl-tegra124.c b/drivers/pinctrl/pinctrl-tegra124.c
index 2b20906c5356..7cd44c7c296d 100644
--- a/drivers/pinctrl/pinctrl-tegra124.c
+++ b/drivers/pinctrl/pinctrl-tegra124.c
@@ -1536,6 +1536,7 @@ enum tegra_mux {
 	TEGRA_MUX_CLK,
 	TEGRA_MUX_CLK12,
 	TEGRA_MUX_CPU,
+	TEGRA_MUX_CSI,
 	TEGRA_MUX_DAP,
 	TEGRA_MUX_DAP1,
 	TEGRA_MUX_DAP2,
@@ -1544,6 +1545,7 @@ enum tegra_mux {
 	TEGRA_MUX_DISPLAYA_ALT,
 	TEGRA_MUX_DISPLAYB,
 	TEGRA_MUX_DP,
+	TEGRA_MUX_DSI_B,
 	TEGRA_MUX_DTV,
 	TEGRA_MUX_EXTPERIPH1,
 	TEGRA_MUX_EXTPERIPH2,
@@ -1613,8 +1615,6 @@ enum tegra_mux {
 	TEGRA_MUX_VI_ALT3,
 	TEGRA_MUX_VIMCLK2,
 	TEGRA_MUX_VIMCLK2_ALT,
-	TEGRA_MUX_CSI,
-	TEGRA_MUX_DSI_B,
 };
 
 #define FUNCTION(fname)					\
@@ -1630,6 +1630,7 @@ static struct tegra_function tegra124_functions[] = {
 	FUNCTION(clk),
 	FUNCTION(clk12),
 	FUNCTION(cpu),
+	FUNCTION(csi),
 	FUNCTION(dap),
 	FUNCTION(dap1),
 	FUNCTION(dap2),
@@ -1638,6 +1639,7 @@ static struct tegra_function tegra124_functions[] = {
 	FUNCTION(displaya_alt),
 	FUNCTION(displayb),
 	FUNCTION(dp),
+	FUNCTION(dsi_b),
 	FUNCTION(dtv),
 	FUNCTION(extperiph1),
 	FUNCTION(extperiph2),
@@ -1707,15 +1709,15 @@ static struct tegra_function tegra124_functions[] = {
 	FUNCTION(vi_alt3),
 	FUNCTION(vimclk2),
 	FUNCTION(vimclk2_alt),
-	FUNCTION(csi),
-	FUNCTION(dsi_b),
 };
 
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 #define MIPI_PAD_CTRL_PINGROUP_REG_A	0x820	/* bank 2 */
 
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 #define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
+#define MIPI_PAD_CTRL_PINGROUP_REG_Y(r)	((r) - MIPI_PAD_CTRL_PINGROUP_REG_A)
 
 #define PINGROUP_BIT_Y(b)		(b)
 #define PINGROUP_BIT_N(b)		(-1)
@@ -1740,20 +1742,17 @@ static struct tegra_function tegra124_functions[] = {
 		.tri_reg = PINGROUP_REG(r),				\
 		.tri_bank = 1,						\
 		.tri_bit = 4,						\
-		.einput_bit = PINGROUP_BIT_Y(5),			\
+		.einput_bit = 5,					\
 		.odrain_bit = PINGROUP_BIT_##od(6),			\
-		.lock_bit = PINGROUP_BIT_Y(7),				\
+		.lock_bit = 7,						\
 		.ioreset_bit = PINGROUP_BIT_##ior(8),			\
 		.rcv_sel_bit = PINGROUP_BIT_##rcv_sel(9),		\
 		.drv_reg = -1,						\
 	}
 
-#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
-
-#define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,		\
-		     drvdn_b, drvdn_w, drvup_b, drvup_w,		\
-		     slwr_b, slwr_w, slwf_b, slwf_w,			\
-		     drvtype)						\
+#define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b,	\
+		     drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w,		\
+		     slwf_b, slwf_w, drvtype)				\
 	{								\
 		.name = "drive_" #pg_name,				\
 		.pins = drive_##pg_name##_pins,				\
@@ -1782,8 +1781,6 @@ static struct tegra_function tegra124_functions[] = {
 		.drvtype_bit = PINGROUP_BIT_##drvtype(6),		\
 	}
 
-#define MIPI_PAD_CTRL_PINGROUP_REG_Y(r)	((r) - MIPI_PAD_CTRL_PINGROUP_REG_A)
-
 #define MIPI_PAD_CTRL_PINGROUP(pg_name, r, b, f0, f1)			\
 	{								\
 		.name = "mipi_pad_ctrl_" #pg_name,			\
@@ -2044,8 +2041,8 @@ static const struct tegra_pingroup tegra124_groups[] = {
 	DRV_PINGROUP(sdio4,       0x9c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
 	DRV_PINGROUP(ao4,         0x9c8,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
 
-	/*		       pg_name, r      b  f0,  f1 */
-	MIPI_PAD_CTRL_PINGROUP(dsi_b,   0x820, 1, CSI, DSI_B)
+	/*                     pg_name, r,     b, f0,  f1 */
+	MIPI_PAD_CTRL_PINGROUP(dsi_b,   0x820, 1, CSI, DSI_B),
 };
 
 static const struct tegra_pinctrl_soc_data tegra124_pinctrl = {
@@ -2056,6 +2053,9 @@ static const struct tegra_pinctrl_soc_data tegra124_pinctrl = {
 	.nfunctions = ARRAY_SIZE(tegra124_functions),
 	.groups = tegra124_groups,
 	.ngroups = ARRAY_SIZE(tegra124_groups),
+	.hsm_in_mux = false,
+	.schmitt_in_mux = false,
+	.drvtype_in_mux = false,
 };
 
 static int tegra124_pinctrl_probe(struct platform_device *pdev)
diff --git a/drivers/pinctrl/pinctrl-tegra20.c b/drivers/pinctrl/pinctrl-tegra20.c
index d3a5722e4acb..4833db4433d9 100644
--- a/drivers/pinctrl/pinctrl-tegra20.c
+++ b/drivers/pinctrl/pinctrl-tegra20.c
@@ -2221,6 +2221,9 @@ static const struct tegra_pinctrl_soc_data tegra20_pinctrl = {
 	.nfunctions = ARRAY_SIZE(tegra20_functions),
 	.groups = tegra20_groups,
 	.ngroups = ARRAY_SIZE(tegra20_groups),
+	.hsm_in_mux = false,
+	.schmitt_in_mux = false,
+	.drvtype_in_mux = false,
 };
 
 static int tegra20_pinctrl_probe(struct platform_device *pdev)
diff --git a/drivers/pinctrl/pinctrl-tegra210.c b/drivers/pinctrl/pinctrl-tegra210.c
new file mode 100644
index 000000000000..252b464901c0
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-tegra210.c
@@ -0,0 +1,1588 @@
+/*
+ * Pinctrl data for the NVIDIA Tegra210 pinmux
+ *
+ * Copyright (c) 2015, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+
+#include "pinctrl-tegra.h"
+
+/*
+ * Most pins affected by the pinmux can also be GPIOs. Define these first.
+ * These must match how the GPIO driver names/numbers its pins.
+ */
+#define _GPIO(offset)				(offset)
+
+#define TEGRA_PIN_PEX_L0_RST_N_PA0		_GPIO(0)
+#define TEGRA_PIN_PEX_L0_CLKREQ_N_PA1		_GPIO(1)
+#define TEGRA_PIN_PEX_WAKE_N_PA2		_GPIO(2)
+#define TEGRA_PIN_PEX_L1_RST_N_PA3		_GPIO(3)
+#define TEGRA_PIN_PEX_L1_CLKREQ_N_PA4		_GPIO(4)
+#define TEGRA_PIN_SATA_LED_ACTIVE_PA5		_GPIO(5)
+#define TEGRA_PIN_PA6				_GPIO(6)
+#define TEGRA_PIN_DAP1_FS_PB0			_GPIO(8)
+#define TEGRA_PIN_DAP1_DIN_PB1			_GPIO(9)
+#define TEGRA_PIN_DAP1_DOUT_PB2			_GPIO(10)
+#define TEGRA_PIN_DAP1_SCLK_PB3			_GPIO(11)
+#define TEGRA_PIN_SPI2_MOSI_PB4			_GPIO(12)
+#define TEGRA_PIN_SPI2_MISO_PB5			_GPIO(13)
+#define TEGRA_PIN_SPI2_SCK_PB6			_GPIO(14)
+#define TEGRA_PIN_SPI2_CS0_PB7			_GPIO(15)
+#define TEGRA_PIN_SPI1_MOSI_PC0			_GPIO(16)
+#define TEGRA_PIN_SPI1_MISO_PC1			_GPIO(17)
+#define TEGRA_PIN_SPI1_SCK_PC2			_GPIO(18)
+#define TEGRA_PIN_SPI1_CS0_PC3			_GPIO(19)
+#define TEGRA_PIN_SPI1_CS1_PC4			_GPIO(20)
+#define TEGRA_PIN_SPI4_SCK_PC5			_GPIO(21)
+#define TEGRA_PIN_SPI4_CS0_PC6			_GPIO(22)
+#define TEGRA_PIN_SPI4_MOSI_PC7			_GPIO(23)
+#define TEGRA_PIN_SPI4_MISO_PD0			_GPIO(24)
+#define TEGRA_PIN_UART3_TX_PD1			_GPIO(25)
+#define TEGRA_PIN_UART3_RX_PD2			_GPIO(26)
+#define TEGRA_PIN_UART3_RTS_PD3			_GPIO(27)
+#define TEGRA_PIN_UART3_CTS_PD4			_GPIO(28)
+#define TEGRA_PIN_DMIC1_CLK_PE0			_GPIO(32)
+#define TEGRA_PIN_DMIC1_DAT_PE1			_GPIO(33)
+#define TEGRA_PIN_DMIC2_CLK_PE2			_GPIO(34)
+#define TEGRA_PIN_DMIC2_DAT_PE3			_GPIO(35)
+#define TEGRA_PIN_DMIC3_CLK_PE4			_GPIO(36)
+#define TEGRA_PIN_DMIC3_DAT_PE5			_GPIO(37)
+#define TEGRA_PIN_PE6				_GPIO(38)
+#define TEGRA_PIN_PE7				_GPIO(39)
+#define TEGRA_PIN_GEN3_I2C_SCL_PF0		_GPIO(40)
+#define TEGRA_PIN_GEN3_I2C_SDA_PF1		_GPIO(41)
+#define TEGRA_PIN_UART2_TX_PG0			_GPIO(48)
+#define TEGRA_PIN_UART2_RX_PG1			_GPIO(49)
+#define TEGRA_PIN_UART2_RTS_PG2			_GPIO(50)
+#define TEGRA_PIN_UART2_CTS_PG3			_GPIO(51)
+#define TEGRA_PIN_WIFI_EN_PH0			_GPIO(56)
+#define TEGRA_PIN_WIFI_RST_PH1			_GPIO(57)
+#define TEGRA_PIN_WIFI_WAKE_AP_PH2		_GPIO(58)
+#define TEGRA_PIN_AP_WAKE_BT_PH3		_GPIO(59)
+#define TEGRA_PIN_BT_RST_PH4			_GPIO(60)
+#define TEGRA_PIN_BT_WAKE_AP_PH5		_GPIO(61)
+#define TEGRA_PIN_PH6				_GPIO(62)
+#define TEGRA_PIN_AP_WAKE_NFC_PH7		_GPIO(63)
+#define TEGRA_PIN_NFC_EN_PI0			_GPIO(64)
+#define TEGRA_PIN_NFC_INT_PI1			_GPIO(65)
+#define TEGRA_PIN_GPS_EN_PI2			_GPIO(66)
+#define TEGRA_PIN_GPS_RST_PI3			_GPIO(67)
+#define TEGRA_PIN_UART4_TX_PI4			_GPIO(68)
+#define TEGRA_PIN_UART4_RX_PI5			_GPIO(69)
+#define TEGRA_PIN_UART4_RTS_PI6			_GPIO(70)
+#define TEGRA_PIN_UART4_CTS_PI7			_GPIO(71)
+#define TEGRA_PIN_GEN1_I2C_SDA_PJ0		_GPIO(72)
+#define TEGRA_PIN_GEN1_I2C_SCL_PJ1		_GPIO(73)
+#define TEGRA_PIN_GEN2_I2C_SCL_PJ2		_GPIO(74)
+#define TEGRA_PIN_GEN2_I2C_SDA_PJ3		_GPIO(75)
+#define TEGRA_PIN_DAP4_FS_PJ4			_GPIO(76)
+#define TEGRA_PIN_DAP4_DIN_PJ5			_GPIO(77)
+#define TEGRA_PIN_DAP4_DOUT_PJ6			_GPIO(78)
+#define TEGRA_PIN_DAP4_SCLK_PJ7			_GPIO(79)
+#define TEGRA_PIN_PK0				_GPIO(80)
+#define TEGRA_PIN_PK1				_GPIO(81)
+#define TEGRA_PIN_PK2				_GPIO(82)
+#define TEGRA_PIN_PK3				_GPIO(83)
+#define TEGRA_PIN_PK4				_GPIO(84)
+#define TEGRA_PIN_PK5				_GPIO(85)
+#define TEGRA_PIN_PK6				_GPIO(86)
+#define TEGRA_PIN_PK7				_GPIO(87)
+#define TEGRA_PIN_PL0				_GPIO(88)
+#define TEGRA_PIN_PL1				_GPIO(89)
+#define TEGRA_PIN_SDMMC1_CLK_PM0		_GPIO(96)
+#define TEGRA_PIN_SDMMC1_CMD_PM1		_GPIO(97)
+#define TEGRA_PIN_SDMMC1_DAT3_PM2		_GPIO(98)
+#define TEGRA_PIN_SDMMC1_DAT2_PM3		_GPIO(99)
+#define TEGRA_PIN_SDMMC1_DAT1_PM4		_GPIO(100)
+#define TEGRA_PIN_SDMMC1_DAT0_PM5		_GPIO(101)
+#define TEGRA_PIN_SDMMC3_CLK_PP0		_GPIO(120)
+#define TEGRA_PIN_SDMMC3_CMD_PP1		_GPIO(121)
+#define TEGRA_PIN_SDMMC3_DAT3_PP2		_GPIO(122)
+#define TEGRA_PIN_SDMMC3_DAT2_PP3		_GPIO(123)
+#define TEGRA_PIN_SDMMC3_DAT1_PP4		_GPIO(124)
+#define TEGRA_PIN_SDMMC3_DAT0_PP5		_GPIO(125)
+#define TEGRA_PIN_CAM1_MCLK_PS0			_GPIO(144)
+#define TEGRA_PIN_CAM2_MCLK_PS1			_GPIO(145)
+#define TEGRA_PIN_CAM_I2C_SCL_PS2		_GPIO(146)
+#define TEGRA_PIN_CAM_I2C_SDA_PS3		_GPIO(147)
+#define TEGRA_PIN_CAM_RST_PS4			_GPIO(148)
+#define TEGRA_PIN_CAM_AF_EN_PS5			_GPIO(149)
+#define TEGRA_PIN_CAM_FLASH_EN_PS6		_GPIO(150)
+#define TEGRA_PIN_CAM1_PWDN_PS7			_GPIO(151)
+#define TEGRA_PIN_CAM2_PWDN_PT0			_GPIO(152)
+#define TEGRA_PIN_CAM1_STROBE_PT1		_GPIO(153)
+#define TEGRA_PIN_UART1_TX_PU0			_GPIO(160)
+#define TEGRA_PIN_UART1_RX_PU1			_GPIO(161)
+#define TEGRA_PIN_UART1_RTS_PU2			_GPIO(162)
+#define TEGRA_PIN_UART1_CTS_PU3			_GPIO(163)
+#define TEGRA_PIN_LCD_BL_PWM_PV0		_GPIO(168)
+#define TEGRA_PIN_LCD_BL_EN_PV1			_GPIO(169)
+#define TEGRA_PIN_LCD_RST_PV2			_GPIO(170)
+#define TEGRA_PIN_LCD_GPIO1_PV3			_GPIO(171)
+#define TEGRA_PIN_LCD_GPIO2_PV4			_GPIO(172)
+#define TEGRA_PIN_AP_READY_PV5			_GPIO(173)
+#define TEGRA_PIN_TOUCH_RST_PV6			_GPIO(174)
+#define TEGRA_PIN_TOUCH_CLK_PV7			_GPIO(175)
+#define TEGRA_PIN_MODEM_WAKE_AP_PX0		_GPIO(184)
+#define TEGRA_PIN_TOUCH_INT_PX1			_GPIO(185)
+#define TEGRA_PIN_MOTION_INT_PX2		_GPIO(186)
+#define TEGRA_PIN_ALS_PROX_INT_PX3		_GPIO(187)
+#define TEGRA_PIN_TEMP_ALERT_PX4		_GPIO(188)
+#define TEGRA_PIN_BUTTON_POWER_ON_PX5		_GPIO(189)
+#define TEGRA_PIN_BUTTON_VOL_UP_PX6		_GPIO(190)
+#define TEGRA_PIN_BUTTON_VOL_DOWN_PX7		_GPIO(191)
+#define TEGRA_PIN_BUTTON_SLIDE_SW_PY0		_GPIO(192)
+#define TEGRA_PIN_BUTTON_HOME_PY1		_GPIO(193)
+#define TEGRA_PIN_LCD_TE_PY2			_GPIO(194)
+#define TEGRA_PIN_PWR_I2C_SCL_PY3		_GPIO(195)
+#define TEGRA_PIN_PWR_I2C_SDA_PY4		_GPIO(196)
+#define TEGRA_PIN_CLK_32K_OUT_PY5		_GPIO(197)
+#define TEGRA_PIN_PZ0				_GPIO(200)
+#define TEGRA_PIN_PZ1				_GPIO(201)
+#define TEGRA_PIN_PZ2				_GPIO(202)
+#define TEGRA_PIN_PZ3				_GPIO(203)
+#define TEGRA_PIN_PZ4				_GPIO(204)
+#define TEGRA_PIN_PZ5				_GPIO(205)
+#define TEGRA_PIN_DAP2_FS_PAA0			_GPIO(208)
+#define TEGRA_PIN_DAP2_SCLK_PAA1		_GPIO(209)
+#define TEGRA_PIN_DAP2_DIN_PAA2			_GPIO(210)
+#define TEGRA_PIN_DAP2_DOUT_PAA3		_GPIO(211)
+#define TEGRA_PIN_AUD_MCLK_PBB0			_GPIO(216)
+#define TEGRA_PIN_DVFS_PWM_PBB1			_GPIO(217)
+#define TEGRA_PIN_DVFS_CLK_PBB2			_GPIO(218)
+#define TEGRA_PIN_GPIO_X1_AUD_PBB3		_GPIO(219)
+#define TEGRA_PIN_GPIO_X3_AUD_PBB4		_GPIO(220)
+#define TEGRA_PIN_HDMI_CEC_PCC0			_GPIO(224)
+#define TEGRA_PIN_HDMI_INT_DP_HPD_PCC1		_GPIO(225)
+#define TEGRA_PIN_SPDIF_OUT_PCC2		_GPIO(226)
+#define TEGRA_PIN_SPDIF_IN_PCC3			_GPIO(227)
+#define TEGRA_PIN_USB_VBUS_EN0_PCC4		_GPIO(228)
+#define TEGRA_PIN_USB_VBUS_EN1_PCC5		_GPIO(229)
+#define TEGRA_PIN_DP_HPD0_PCC6			_GPIO(230)
+#define TEGRA_PIN_PCC7				_GPIO(231)
+#define TEGRA_PIN_SPI2_CS1_PDD0			_GPIO(232)
+#define TEGRA_PIN_QSPI_SCK_PEE0			_GPIO(240)
+#define TEGRA_PIN_QSPI_CS_N_PEE1		_GPIO(241)
+#define TEGRA_PIN_QSPI_IO0_PEE2			_GPIO(242)
+#define TEGRA_PIN_QSPI_IO1_PEE3			_GPIO(243)
+#define TEGRA_PIN_QSPI_IO2_PEE4			_GPIO(244)
+#define TEGRA_PIN_QSPI_IO3_PEE5			_GPIO(245)
+
+/* All non-GPIO pins follow */
+#define NUM_GPIOS				(TEGRA_PIN_QSPI_IO3_PEE5 + 1)
+#define _PIN(offset)				(NUM_GPIOS + (offset))
+
+/* Non-GPIO pins */
+#define TEGRA_PIN_CORE_PWR_REQ			_PIN(0)
+#define TEGRA_PIN_CPU_PWR_REQ			_PIN(1)
+#define TEGRA_PIN_PWR_INT_N			_PIN(2)
+#define TEGRA_PIN_CLK_32K_IN			_PIN(3)
+#define TEGRA_PIN_JTAG_RTCK			_PIN(4)
+#define TEGRA_PIN_BATT_BCL			_PIN(5)
+#define TEGRA_PIN_CLK_REQ			_PIN(6)
+#define TEGRA_PIN_SHUTDOWN			_PIN(7)
+
+static const struct pinctrl_pin_desc tegra210_pins[] = {
+	PINCTRL_PIN(TEGRA_PIN_PEX_L0_RST_N_PA0, "PEX_L0_RST_N PA0"),
+	PINCTRL_PIN(TEGRA_PIN_PEX_L0_CLKREQ_N_PA1, "PEX_L0_CLKREQ_N PA1"),
+	PINCTRL_PIN(TEGRA_PIN_PEX_WAKE_N_PA2, "PEX_WAKE_N PA2"),
+	PINCTRL_PIN(TEGRA_PIN_PEX_L1_RST_N_PA3, "PEX_L1_RST_N PA3"),
+	PINCTRL_PIN(TEGRA_PIN_PEX_L1_CLKREQ_N_PA4, "PEX_L1_CLKREQ_N PA4"),
+	PINCTRL_PIN(TEGRA_PIN_SATA_LED_ACTIVE_PA5, "SATA_LED_ACTIVE PA5"),
+	PINCTRL_PIN(TEGRA_PIN_PA6, "PA6"),
+	PINCTRL_PIN(TEGRA_PIN_DAP1_FS_PB0, "DAP1_FS PB0"),
+	PINCTRL_PIN(TEGRA_PIN_DAP1_DIN_PB1, "DAP1_DIN PB1"),
+	PINCTRL_PIN(TEGRA_PIN_DAP1_DOUT_PB2, "DAP1_DOUT PB2"),
+	PINCTRL_PIN(TEGRA_PIN_DAP1_SCLK_PB3, "DAP1_SCLK PB3"),
+	PINCTRL_PIN(TEGRA_PIN_SPI2_MOSI_PB4, "SPI2_MOSI PB4"),
+	PINCTRL_PIN(TEGRA_PIN_SPI2_MISO_PB5, "SPI2_MISO PB5"),
+	PINCTRL_PIN(TEGRA_PIN_SPI2_SCK_PB6, "SPI2_SCK PB6"),
+	PINCTRL_PIN(TEGRA_PIN_SPI2_CS0_PB7, "SPI2_CS0 PB7"),
+	PINCTRL_PIN(TEGRA_PIN_SPI1_MOSI_PC0, "SPI1_MOSI PC0"),
+	PINCTRL_PIN(TEGRA_PIN_SPI1_MISO_PC1, "SPI1_MISO PC1"),
+	PINCTRL_PIN(TEGRA_PIN_SPI1_SCK_PC2, "SPI1_SCK PC2"),
+	PINCTRL_PIN(TEGRA_PIN_SPI1_CS0_PC3, "SPI1_CS0 PC3"),
+	PINCTRL_PIN(TEGRA_PIN_SPI1_CS1_PC4, "SPI1_CS1 PC4"),
+	PINCTRL_PIN(TEGRA_PIN_SPI4_SCK_PC5, "SPI4_SCK PC5"),
+	PINCTRL_PIN(TEGRA_PIN_SPI4_CS0_PC6, "SPI4_CS0 PC6"),
+	PINCTRL_PIN(TEGRA_PIN_SPI4_MOSI_PC7, "SPI4_MOSI PC7"),
+	PINCTRL_PIN(TEGRA_PIN_SPI4_MISO_PD0, "SPI4_MISO PD0"),
+	PINCTRL_PIN(TEGRA_PIN_UART3_TX_PD1, "UART3_TX PD1"),
+	PINCTRL_PIN(TEGRA_PIN_UART3_RX_PD2, "UART3_RX PD2"),
+	PINCTRL_PIN(TEGRA_PIN_UART3_RTS_PD3, "UART3_RTS PD3"),
+	PINCTRL_PIN(TEGRA_PIN_UART3_CTS_PD4, "UART3_CTS PD4"),
+	PINCTRL_PIN(TEGRA_PIN_DMIC1_CLK_PE0, "DMIC1_CLK PE0"),
+	PINCTRL_PIN(TEGRA_PIN_DMIC1_DAT_PE1, "DMIC1_DAT PE1"),
+	PINCTRL_PIN(TEGRA_PIN_DMIC2_CLK_PE2, "DMIC2_CLK PE2"),
+	PINCTRL_PIN(TEGRA_PIN_DMIC2_DAT_PE3, "DMIC2_DAT PE3"),
+	PINCTRL_PIN(TEGRA_PIN_DMIC3_CLK_PE4, "DMIC3_CLK PE4"),
+	PINCTRL_PIN(TEGRA_PIN_DMIC3_DAT_PE5, "DMIC3_DAT PE5"),
+	PINCTRL_PIN(TEGRA_PIN_PE6, "PE6"),
+	PINCTRL_PIN(TEGRA_PIN_PE7, "PE7"),
+	PINCTRL_PIN(TEGRA_PIN_GEN3_I2C_SCL_PF0, "GEN3_I2C_SCL PF0"),
+	PINCTRL_PIN(TEGRA_PIN_GEN3_I2C_SDA_PF1, "GEN3_I2C_SDA PF1"),
+	PINCTRL_PIN(TEGRA_PIN_UART2_TX_PG0, "UART2_TX PG0"),
+	PINCTRL_PIN(TEGRA_PIN_UART2_RX_PG1, "UART2_RX PG1"),
+	PINCTRL_PIN(TEGRA_PIN_UART2_RTS_PG2, "UART2_RTS PG2"),
+	PINCTRL_PIN(TEGRA_PIN_UART2_CTS_PG3, "UART2_CTS PG3"),
+	PINCTRL_PIN(TEGRA_PIN_WIFI_EN_PH0, "WIFI_EN PH0"),
+	PINCTRL_PIN(TEGRA_PIN_WIFI_RST_PH1, "WIFI_RST PH1"),
+	PINCTRL_PIN(TEGRA_PIN_WIFI_WAKE_AP_PH2, "WIFI_WAKE_AP PH2"),
+	PINCTRL_PIN(TEGRA_PIN_AP_WAKE_BT_PH3, "AP_WAKE_BT PH3"),
+	PINCTRL_PIN(TEGRA_PIN_BT_RST_PH4, "BT_RST PH4"),
+	PINCTRL_PIN(TEGRA_PIN_BT_WAKE_AP_PH5, "BT_WAKE_AP PH5"),
+	PINCTRL_PIN(TEGRA_PIN_PH6, "PH6"),
+	PINCTRL_PIN(TEGRA_PIN_AP_WAKE_NFC_PH7, "AP_WAKE_NFC PH7"),
+	PINCTRL_PIN(TEGRA_PIN_NFC_EN_PI0, "NFC_EN PI0"),
+	PINCTRL_PIN(TEGRA_PIN_NFC_INT_PI1, "NFC_INT PI1"),
+	PINCTRL_PIN(TEGRA_PIN_GPS_EN_PI2, "GPS_EN PI2"),
+	PINCTRL_PIN(TEGRA_PIN_GPS_RST_PI3, "GPS_RST PI3"),
+	PINCTRL_PIN(TEGRA_PIN_UART4_TX_PI4, "UART4_TX PI4"),
+	PINCTRL_PIN(TEGRA_PIN_UART4_RX_PI5, "UART4_RX PI5"),
+	PINCTRL_PIN(TEGRA_PIN_UART4_RTS_PI6, "UART4_RTS PI6"),
+	PINCTRL_PIN(TEGRA_PIN_UART4_CTS_PI7, "UART4_CTS PI7"),
+	PINCTRL_PIN(TEGRA_PIN_GEN1_I2C_SDA_PJ0, "GEN1_I2C_SDA PJ0"),
+	PINCTRL_PIN(TEGRA_PIN_GEN1_I2C_SCL_PJ1, "GEN1_I2C_SCL PJ1"),
+	PINCTRL_PIN(TEGRA_PIN_GEN2_I2C_SCL_PJ2, "GEN2_I2C_SCL PJ2"),
+	PINCTRL_PIN(TEGRA_PIN_GEN2_I2C_SDA_PJ3, "GEN2_I2C_SDA PJ3"),
+	PINCTRL_PIN(TEGRA_PIN_DAP4_FS_PJ4, "DAP4_FS PJ4"),
+	PINCTRL_PIN(TEGRA_PIN_DAP4_DIN_PJ5, "DAP4_DIN PJ5"),
+	PINCTRL_PIN(TEGRA_PIN_DAP4_DOUT_PJ6, "DAP4_DOUT PJ6"),
+	PINCTRL_PIN(TEGRA_PIN_DAP4_SCLK_PJ7, "DAP4_SCLK PJ7"),
+	PINCTRL_PIN(TEGRA_PIN_PK0, "PK0"),
+	PINCTRL_PIN(TEGRA_PIN_PK1, "PK1"),
+	PINCTRL_PIN(TEGRA_PIN_PK2, "PK2"),
+	PINCTRL_PIN(TEGRA_PIN_PK3, "PK3"),
+	PINCTRL_PIN(TEGRA_PIN_PK4, "PK4"),
+	PINCTRL_PIN(TEGRA_PIN_PK5, "PK5"),
+	PINCTRL_PIN(TEGRA_PIN_PK6, "PK6"),
+	PINCTRL_PIN(TEGRA_PIN_PK7, "PK7"),
+	PINCTRL_PIN(TEGRA_PIN_PL0, "PL0"),
+	PINCTRL_PIN(TEGRA_PIN_PL1, "PL1"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC1_CLK_PM0, "SDMMC1_CLK PM0"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC1_CMD_PM1, "SDMMC1_CMD PM1"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC1_DAT3_PM2, "SDMMC1_DAT3 PM2"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC1_DAT2_PM3, "SDMMC1_DAT2 PM3"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC1_DAT1_PM4, "SDMMC1_DAT1 PM4"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC1_DAT0_PM5, "SDMMC1_DAT0 PM5"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC3_CLK_PP0, "SDMMC3_CLK PP0"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC3_CMD_PP1, "SDMMC3_CMD PP1"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC3_DAT3_PP2, "SDMMC3_DAT3 PP2"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC3_DAT2_PP3, "SDMMC3_DAT2 PP3"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC3_DAT1_PP4, "SDMMC3_DAT1 PP4"),
+	PINCTRL_PIN(TEGRA_PIN_SDMMC3_DAT0_PP5, "SDMMC3_DAT0 PP5"),
+	PINCTRL_PIN(TEGRA_PIN_CAM1_MCLK_PS0, "CAM1_MCLK PS0"),
+	PINCTRL_PIN(TEGRA_PIN_CAM2_MCLK_PS1, "CAM2_MCLK PS1"),
+	PINCTRL_PIN(TEGRA_PIN_CAM_I2C_SCL_PS2, "CAM_I2C_SCL PS2"),
+	PINCTRL_PIN(TEGRA_PIN_CAM_I2C_SDA_PS3, "CAM_I2C_SDA PS3"),
+	PINCTRL_PIN(TEGRA_PIN_CAM_RST_PS4, "CAM_RST PS4"),
+	PINCTRL_PIN(TEGRA_PIN_CAM_AF_EN_PS5, "CAM_AF_EN PS5"),
+	PINCTRL_PIN(TEGRA_PIN_CAM_FLASH_EN_PS6, "CAM_FLASH_EN PS6"),
+	PINCTRL_PIN(TEGRA_PIN_CAM1_PWDN_PS7, "CAM1_PWDN PS7"),
+	PINCTRL_PIN(TEGRA_PIN_CAM2_PWDN_PT0, "CAM2_PWDN PT0"),
+	PINCTRL_PIN(TEGRA_PIN_CAM1_STROBE_PT1, "CAM1_STROBE PT1"),
+	PINCTRL_PIN(TEGRA_PIN_UART1_TX_PU0, "UART1_TX PU0"),
+	PINCTRL_PIN(TEGRA_PIN_UART1_RX_PU1, "UART1_RX PU1"),
+	PINCTRL_PIN(TEGRA_PIN_UART1_RTS_PU2, "UART1_RTS PU2"),
+	PINCTRL_PIN(TEGRA_PIN_UART1_CTS_PU3, "UART1_CTS PU3"),
+	PINCTRL_PIN(TEGRA_PIN_LCD_BL_PWM_PV0, "LCD_BL_PWM PV0"),
+	PINCTRL_PIN(TEGRA_PIN_LCD_BL_EN_PV1, "LCD_BL_EN PV1"),
+	PINCTRL_PIN(TEGRA_PIN_LCD_RST_PV2, "LCD_RST PV2"),
+	PINCTRL_PIN(TEGRA_PIN_LCD_GPIO1_PV3, "LCD_GPIO1 PV3"),
+	PINCTRL_PIN(TEGRA_PIN_LCD_GPIO2_PV4, "LCD_GPIO2 PV4"),
+	PINCTRL_PIN(TEGRA_PIN_AP_READY_PV5, "AP_READY PV5"),
+	PINCTRL_PIN(TEGRA_PIN_TOUCH_RST_PV6, "TOUCH_RST PV6"),
+	PINCTRL_PIN(TEGRA_PIN_TOUCH_CLK_PV7, "TOUCH_CLK PV7"),
+	PINCTRL_PIN(TEGRA_PIN_MODEM_WAKE_AP_PX0, "MODEM_WAKE_AP PX0"),
+	PINCTRL_PIN(TEGRA_PIN_TOUCH_INT_PX1, "TOUCH_INT PX1"),
+	PINCTRL_PIN(TEGRA_PIN_MOTION_INT_PX2, "MOTION_INT PX2"),
+	PINCTRL_PIN(TEGRA_PIN_ALS_PROX_INT_PX3, "ALS_PROX_INT PX3"),
+	PINCTRL_PIN(TEGRA_PIN_TEMP_ALERT_PX4, "TEMP_ALERT PX4"),
+	PINCTRL_PIN(TEGRA_PIN_BUTTON_POWER_ON_PX5, "BUTTON_POWER_ON PX5"),
+	PINCTRL_PIN(TEGRA_PIN_BUTTON_VOL_UP_PX6, "BUTTON_VOL_UP PX6"),
+	PINCTRL_PIN(TEGRA_PIN_BUTTON_VOL_DOWN_PX7, "BUTTON_VOL_DOWN PX7"),
+	PINCTRL_PIN(TEGRA_PIN_BUTTON_SLIDE_SW_PY0, "BUTTON_SLIDE_SW PY0"),
+	PINCTRL_PIN(TEGRA_PIN_BUTTON_HOME_PY1, "BUTTON_HOME PY1"),
+	PINCTRL_PIN(TEGRA_PIN_LCD_TE_PY2, "LCD_TE PY2"),
+	PINCTRL_PIN(TEGRA_PIN_PWR_I2C_SCL_PY3, "PWR_I2C_SCL PY3"),
+	PINCTRL_PIN(TEGRA_PIN_PWR_I2C_SDA_PY4, "PWR_I2C_SDA PY4"),
+	PINCTRL_PIN(TEGRA_PIN_CLK_32K_OUT_PY5, "CLK_32K_OUT PY5"),
+	PINCTRL_PIN(TEGRA_PIN_PZ0, "PZ0"),
+	PINCTRL_PIN(TEGRA_PIN_PZ1, "PZ1"),
+	PINCTRL_PIN(TEGRA_PIN_PZ2, "PZ2"),
+	PINCTRL_PIN(TEGRA_PIN_PZ3, "PZ3"),
+	PINCTRL_PIN(TEGRA_PIN_PZ4, "PZ4"),
+	PINCTRL_PIN(TEGRA_PIN_PZ5, "PZ5"),
+	PINCTRL_PIN(TEGRA_PIN_DAP2_FS_PAA0, "DAP2_FS PAA0"),
+	PINCTRL_PIN(TEGRA_PIN_DAP2_SCLK_PAA1, "DAP2_SCLK PAA1"),
+	PINCTRL_PIN(TEGRA_PIN_DAP2_DIN_PAA2, "DAP2_DIN PAA2"),
+	PINCTRL_PIN(TEGRA_PIN_DAP2_DOUT_PAA3, "DAP2_DOUT PAA3"),
+	PINCTRL_PIN(TEGRA_PIN_AUD_MCLK_PBB0, "AUD_MCLK PBB0"),
+	PINCTRL_PIN(TEGRA_PIN_DVFS_PWM_PBB1, "DVFS_PWM PBB1"),
+	PINCTRL_PIN(TEGRA_PIN_DVFS_CLK_PBB2, "DVFS_CLK PBB2"),
+	PINCTRL_PIN(TEGRA_PIN_GPIO_X1_AUD_PBB3, "GPIO_X1_AUD PBB3"),
+	PINCTRL_PIN(TEGRA_PIN_GPIO_X3_AUD_PBB4, "GPIO_X3_AUD PBB4"),
+	PINCTRL_PIN(TEGRA_PIN_HDMI_CEC_PCC0, "HDMI_CEC PCC0"),
+	PINCTRL_PIN(TEGRA_PIN_HDMI_INT_DP_HPD_PCC1, "HDMI_INT_DP_HPD PCC1"),
+	PINCTRL_PIN(TEGRA_PIN_SPDIF_OUT_PCC2, "SPDIF_OUT PCC2"),
+	PINCTRL_PIN(TEGRA_PIN_SPDIF_IN_PCC3, "SPDIF_IN PCC3"),
+	PINCTRL_PIN(TEGRA_PIN_USB_VBUS_EN0_PCC4, "USB_VBUS_EN0 PCC4"),
+	PINCTRL_PIN(TEGRA_PIN_USB_VBUS_EN1_PCC5, "USB_VBUS_EN1 PCC5"),
+	PINCTRL_PIN(TEGRA_PIN_DP_HPD0_PCC6, "DP_HPD0 PCC6"),
+	PINCTRL_PIN(TEGRA_PIN_PCC7, "PCC7"),
+	PINCTRL_PIN(TEGRA_PIN_SPI2_CS1_PDD0, "SPI2_CS1 PDD0"),
+	PINCTRL_PIN(TEGRA_PIN_QSPI_SCK_PEE0, "QSPI_SCK PEE0"),
+	PINCTRL_PIN(TEGRA_PIN_QSPI_CS_N_PEE1, "QSPI_CS_N PEE1"),
+	PINCTRL_PIN(TEGRA_PIN_QSPI_IO0_PEE2, "QSPI_IO0 PEE2"),
+	PINCTRL_PIN(TEGRA_PIN_QSPI_IO1_PEE3, "QSPI_IO1 PEE3"),
+	PINCTRL_PIN(TEGRA_PIN_QSPI_IO2_PEE4, "QSPI_IO2 PEE4"),
+	PINCTRL_PIN(TEGRA_PIN_QSPI_IO3_PEE5, "QSPI_IO3 PEE5"),
+	PINCTRL_PIN(TEGRA_PIN_CORE_PWR_REQ, "CORE_PWR_REQ"),
+	PINCTRL_PIN(TEGRA_PIN_CPU_PWR_REQ, "CPU_PWR_REQ"),
+	PINCTRL_PIN(TEGRA_PIN_PWR_INT_N, "PWR_INT_N"),
+	PINCTRL_PIN(TEGRA_PIN_CLK_32K_IN, "CLK_32K_IN"),
+	PINCTRL_PIN(TEGRA_PIN_JTAG_RTCK, "JTAG_RTCK"),
+	PINCTRL_PIN(TEGRA_PIN_BATT_BCL, "BATT_BCL"),
+	PINCTRL_PIN(TEGRA_PIN_CLK_REQ, "CLK_REQ"),
+	PINCTRL_PIN(TEGRA_PIN_SHUTDOWN, "SHUTDOWN"),
+};
+
+static const unsigned pex_l0_rst_n_pa0_pins[] = {
+	TEGRA_PIN_PEX_L0_RST_N_PA0,
+};
+
+static const unsigned pex_l0_clkreq_n_pa1_pins[] = {
+	TEGRA_PIN_PEX_L0_CLKREQ_N_PA1,
+};
+
+static const unsigned pex_wake_n_pa2_pins[] = {
+	TEGRA_PIN_PEX_WAKE_N_PA2,
+};
+
+static const unsigned pex_l1_rst_n_pa3_pins[] = {
+	TEGRA_PIN_PEX_L1_RST_N_PA3,
+};
+
+static const unsigned pex_l1_clkreq_n_pa4_pins[] = {
+	TEGRA_PIN_PEX_L1_CLKREQ_N_PA4,
+};
+
+static const unsigned sata_led_active_pa5_pins[] = {
+	TEGRA_PIN_SATA_LED_ACTIVE_PA5,
+};
+
+static const unsigned pa6_pins[] = {
+	TEGRA_PIN_PA6,
+};
+
+static const unsigned dap1_fs_pb0_pins[] = {
+	TEGRA_PIN_DAP1_FS_PB0,
+};
+
+static const unsigned dap1_din_pb1_pins[] = {
+	TEGRA_PIN_DAP1_DIN_PB1,
+};
+
+static const unsigned dap1_dout_pb2_pins[] = {
+	TEGRA_PIN_DAP1_DOUT_PB2,
+};
+
+static const unsigned dap1_sclk_pb3_pins[] = {
+	TEGRA_PIN_DAP1_SCLK_PB3,
+};
+
+static const unsigned spi2_mosi_pb4_pins[] = {
+	TEGRA_PIN_SPI2_MOSI_PB4,
+};
+
+static const unsigned spi2_miso_pb5_pins[] = {
+	TEGRA_PIN_SPI2_MISO_PB5,
+};
+
+static const unsigned spi2_sck_pb6_pins[] = {
+	TEGRA_PIN_SPI2_SCK_PB6,
+};
+
+static const unsigned spi2_cs0_pb7_pins[] = {
+	TEGRA_PIN_SPI2_CS0_PB7,
+};
+
+static const unsigned spi1_mosi_pc0_pins[] = {
+	TEGRA_PIN_SPI1_MOSI_PC0,
+};
+
+static const unsigned spi1_miso_pc1_pins[] = {
+	TEGRA_PIN_SPI1_MISO_PC1,
+};
+
+static const unsigned spi1_sck_pc2_pins[] = {
+	TEGRA_PIN_SPI1_SCK_PC2,
+};
+
+static const unsigned spi1_cs0_pc3_pins[] = {
+	TEGRA_PIN_SPI1_CS0_PC3,
+};
+
+static const unsigned spi1_cs1_pc4_pins[] = {
+	TEGRA_PIN_SPI1_CS1_PC4,
+};
+
+static const unsigned spi4_sck_pc5_pins[] = {
+	TEGRA_PIN_SPI4_SCK_PC5,
+};
+
+static const unsigned spi4_cs0_pc6_pins[] = {
+	TEGRA_PIN_SPI4_CS0_PC6,
+};
+
+static const unsigned spi4_mosi_pc7_pins[] = {
+	TEGRA_PIN_SPI4_MOSI_PC7,
+};
+
+static const unsigned spi4_miso_pd0_pins[] = {
+	TEGRA_PIN_SPI4_MISO_PD0,
+};
+
+static const unsigned uart3_tx_pd1_pins[] = {
+	TEGRA_PIN_UART3_TX_PD1,
+};
+
+static const unsigned uart3_rx_pd2_pins[] = {
+	TEGRA_PIN_UART3_RX_PD2,
+};
+
+static const unsigned uart3_rts_pd3_pins[] = {
+	TEGRA_PIN_UART3_RTS_PD3,
+};
+
+static const unsigned uart3_cts_pd4_pins[] = {
+	TEGRA_PIN_UART3_CTS_PD4,
+};
+
+static const unsigned dmic1_clk_pe0_pins[] = {
+	TEGRA_PIN_DMIC1_CLK_PE0,
+};
+
+static const unsigned dmic1_dat_pe1_pins[] = {
+	TEGRA_PIN_DMIC1_DAT_PE1,
+};
+
+static const unsigned dmic2_clk_pe2_pins[] = {
+	TEGRA_PIN_DMIC2_CLK_PE2,
+};
+
+static const unsigned dmic2_dat_pe3_pins[] = {
+	TEGRA_PIN_DMIC2_DAT_PE3,
+};
+
+static const unsigned dmic3_clk_pe4_pins[] = {
+	TEGRA_PIN_DMIC3_CLK_PE4,
+};
+
+static const unsigned dmic3_dat_pe5_pins[] = {
+	TEGRA_PIN_DMIC3_DAT_PE5,
+};
+
+static const unsigned pe6_pins[] = {
+	TEGRA_PIN_PE6,
+};
+
+static const unsigned pe7_pins[] = {
+	TEGRA_PIN_PE7,
+};
+
+static const unsigned gen3_i2c_scl_pf0_pins[] = {
+	TEGRA_PIN_GEN3_I2C_SCL_PF0,
+};
+
+static const unsigned gen3_i2c_sda_pf1_pins[] = {
+	TEGRA_PIN_GEN3_I2C_SDA_PF1,
+};
+
+static const unsigned uart2_tx_pg0_pins[] = {
+	TEGRA_PIN_UART2_TX_PG0,
+};
+
+static const unsigned uart2_rx_pg1_pins[] = {
+	TEGRA_PIN_UART2_RX_PG1,
+};
+
+static const unsigned uart2_rts_pg2_pins[] = {
+	TEGRA_PIN_UART2_RTS_PG2,
+};
+
+static const unsigned uart2_cts_pg3_pins[] = {
+	TEGRA_PIN_UART2_CTS_PG3,
+};
+
+static const unsigned wifi_en_ph0_pins[] = {
+	TEGRA_PIN_WIFI_EN_PH0,
+};
+
+static const unsigned wifi_rst_ph1_pins[] = {
+	TEGRA_PIN_WIFI_RST_PH1,
+};
+
+static const unsigned wifi_wake_ap_ph2_pins[] = {
+	TEGRA_PIN_WIFI_WAKE_AP_PH2,
+};
+
+static const unsigned ap_wake_bt_ph3_pins[] = {
+	TEGRA_PIN_AP_WAKE_BT_PH3,
+};
+
+static const unsigned bt_rst_ph4_pins[] = {
+	TEGRA_PIN_BT_RST_PH4,
+};
+
+static const unsigned bt_wake_ap_ph5_pins[] = {
+	TEGRA_PIN_BT_WAKE_AP_PH5,
+};
+
+static const unsigned ph6_pins[] = {
+	TEGRA_PIN_PH6,
+};
+
+static const unsigned ap_wake_nfc_ph7_pins[] = {
+	TEGRA_PIN_AP_WAKE_NFC_PH7,
+};
+
+static const unsigned nfc_en_pi0_pins[] = {
+	TEGRA_PIN_NFC_EN_PI0,
+};
+
+static const unsigned nfc_int_pi1_pins[] = {
+	TEGRA_PIN_NFC_INT_PI1,
+};
+
+static const unsigned gps_en_pi2_pins[] = {
+	TEGRA_PIN_GPS_EN_PI2,
+};
+
+static const unsigned gps_rst_pi3_pins[] = {
+	TEGRA_PIN_GPS_RST_PI3,
+};
+
+static const unsigned uart4_tx_pi4_pins[] = {
+	TEGRA_PIN_UART4_TX_PI4,
+};
+
+static const unsigned uart4_rx_pi5_pins[] = {
+	TEGRA_PIN_UART4_RX_PI5,
+};
+
+static const unsigned uart4_rts_pi6_pins[] = {
+	TEGRA_PIN_UART4_RTS_PI6,
+};
+
+static const unsigned uart4_cts_pi7_pins[] = {
+	TEGRA_PIN_UART4_CTS_PI7,
+};
+
+static const unsigned gen1_i2c_sda_pj0_pins[] = {
+	TEGRA_PIN_GEN1_I2C_SDA_PJ0,
+};
+
+static const unsigned gen1_i2c_scl_pj1_pins[] = {
+	TEGRA_PIN_GEN1_I2C_SCL_PJ1,
+};
+
+static const unsigned gen2_i2c_scl_pj2_pins[] = {
+	TEGRA_PIN_GEN2_I2C_SCL_PJ2,
+};
+
+static const unsigned gen2_i2c_sda_pj3_pins[] = {
+	TEGRA_PIN_GEN2_I2C_SDA_PJ3,
+};
+
+static const unsigned dap4_fs_pj4_pins[] = {
+	TEGRA_PIN_DAP4_FS_PJ4,
+};
+
+static const unsigned dap4_din_pj5_pins[] = {
+	TEGRA_PIN_DAP4_DIN_PJ5,
+};
+
+static const unsigned dap4_dout_pj6_pins[] = {
+	TEGRA_PIN_DAP4_DOUT_PJ6,
+};
+
+static const unsigned dap4_sclk_pj7_pins[] = {
+	TEGRA_PIN_DAP4_SCLK_PJ7,
+};
+
+static const unsigned pk0_pins[] = {
+	TEGRA_PIN_PK0,
+};
+
+static const unsigned pk1_pins[] = {
+	TEGRA_PIN_PK1,
+};
+
+static const unsigned pk2_pins[] = {
+	TEGRA_PIN_PK2,
+};
+
+static const unsigned pk3_pins[] = {
+	TEGRA_PIN_PK3,
+};
+
+static const unsigned pk4_pins[] = {
+	TEGRA_PIN_PK4,
+};
+
+static const unsigned pk5_pins[] = {
+	TEGRA_PIN_PK5,
+};
+
+static const unsigned pk6_pins[] = {
+	TEGRA_PIN_PK6,
+};
+
+static const unsigned pk7_pins[] = {
+	TEGRA_PIN_PK7,
+};
+
+static const unsigned pl0_pins[] = {
+	TEGRA_PIN_PL0,
+};
+
+static const unsigned pl1_pins[] = {
+	TEGRA_PIN_PL1,
+};
+
+static const unsigned sdmmc1_clk_pm0_pins[] = {
+	TEGRA_PIN_SDMMC1_CLK_PM0,
+};
+
+static const unsigned sdmmc1_cmd_pm1_pins[] = {
+	TEGRA_PIN_SDMMC1_CMD_PM1,
+};
+
+static const unsigned sdmmc1_dat3_pm2_pins[] = {
+	TEGRA_PIN_SDMMC1_DAT3_PM2,
+};
+
+static const unsigned sdmmc1_dat2_pm3_pins[] = {
+	TEGRA_PIN_SDMMC1_DAT2_PM3,
+};
+
+static const unsigned sdmmc1_dat1_pm4_pins[] = {
+	TEGRA_PIN_SDMMC1_DAT1_PM4,
+};
+
+static const unsigned sdmmc1_dat0_pm5_pins[] = {
+	TEGRA_PIN_SDMMC1_DAT0_PM5,
+};
+
+static const unsigned sdmmc3_clk_pp0_pins[] = {
+	TEGRA_PIN_SDMMC3_CLK_PP0,
+};
+
+static const unsigned sdmmc3_cmd_pp1_pins[] = {
+	TEGRA_PIN_SDMMC3_CMD_PP1,
+};
+
+static const unsigned sdmmc3_dat3_pp2_pins[] = {
+	TEGRA_PIN_SDMMC3_DAT3_PP2,
+};
+
+static const unsigned sdmmc3_dat2_pp3_pins[] = {
+	TEGRA_PIN_SDMMC3_DAT2_PP3,
+};
+
+static const unsigned sdmmc3_dat1_pp4_pins[] = {
+	TEGRA_PIN_SDMMC3_DAT1_PP4,
+};
+
+static const unsigned sdmmc3_dat0_pp5_pins[] = {
+	TEGRA_PIN_SDMMC3_DAT0_PP5,
+};
+
+static const unsigned cam1_mclk_ps0_pins[] = {
+	TEGRA_PIN_CAM1_MCLK_PS0,
+};
+
+static const unsigned cam2_mclk_ps1_pins[] = {
+	TEGRA_PIN_CAM2_MCLK_PS1,
+};
+
+static const unsigned cam_i2c_scl_ps2_pins[] = {
+	TEGRA_PIN_CAM_I2C_SCL_PS2,
+};
+
+static const unsigned cam_i2c_sda_ps3_pins[] = {
+	TEGRA_PIN_CAM_I2C_SDA_PS3,
+};
+
+static const unsigned cam_rst_ps4_pins[] = {
+	TEGRA_PIN_CAM_RST_PS4,
+};
+
+static const unsigned cam_af_en_ps5_pins[] = {
+	TEGRA_PIN_CAM_AF_EN_PS5,
+};
+
+static const unsigned cam_flash_en_ps6_pins[] = {
+	TEGRA_PIN_CAM_FLASH_EN_PS6,
+};
+
+static const unsigned cam1_pwdn_ps7_pins[] = {
+	TEGRA_PIN_CAM1_PWDN_PS7,
+};
+
+static const unsigned cam2_pwdn_pt0_pins[] = {
+	TEGRA_PIN_CAM2_PWDN_PT0,
+};
+
+static const unsigned cam1_strobe_pt1_pins[] = {
+	TEGRA_PIN_CAM1_STROBE_PT1,
+};
+
+static const unsigned uart1_tx_pu0_pins[] = {
+	TEGRA_PIN_UART1_TX_PU0,
+};
+
+static const unsigned uart1_rx_pu1_pins[] = {
+	TEGRA_PIN_UART1_RX_PU1,
+};
+
+static const unsigned uart1_rts_pu2_pins[] = {
+	TEGRA_PIN_UART1_RTS_PU2,
+};
+
+static const unsigned uart1_cts_pu3_pins[] = {
+	TEGRA_PIN_UART1_CTS_PU3,
+};
+
+static const unsigned lcd_bl_pwm_pv0_pins[] = {
+	TEGRA_PIN_LCD_BL_PWM_PV0,
+};
+
+static const unsigned lcd_bl_en_pv1_pins[] = {
+	TEGRA_PIN_LCD_BL_EN_PV1,
+};
+
+static const unsigned lcd_rst_pv2_pins[] = {
+	TEGRA_PIN_LCD_RST_PV2,
+};
+
+static const unsigned lcd_gpio1_pv3_pins[] = {
+	TEGRA_PIN_LCD_GPIO1_PV3,
+};
+
+static const unsigned lcd_gpio2_pv4_pins[] = {
+	TEGRA_PIN_LCD_GPIO2_PV4,
+};
+
+static const unsigned ap_ready_pv5_pins[] = {
+	TEGRA_PIN_AP_READY_PV5,
+};
+
+static const unsigned touch_rst_pv6_pins[] = {
+	TEGRA_PIN_TOUCH_RST_PV6,
+};
+
+static const unsigned touch_clk_pv7_pins[] = {
+	TEGRA_PIN_TOUCH_CLK_PV7,
+};
+
+static const unsigned modem_wake_ap_px0_pins[] = {
+	TEGRA_PIN_MODEM_WAKE_AP_PX0,
+};
+
+static const unsigned touch_int_px1_pins[] = {
+	TEGRA_PIN_TOUCH_INT_PX1,
+};
+
+static const unsigned motion_int_px2_pins[] = {
+	TEGRA_PIN_MOTION_INT_PX2,
+};
+
+static const unsigned als_prox_int_px3_pins[] = {
+	TEGRA_PIN_ALS_PROX_INT_PX3,
+};
+
+static const unsigned temp_alert_px4_pins[] = {
+	TEGRA_PIN_TEMP_ALERT_PX4,
+};
+
+static const unsigned button_power_on_px5_pins[] = {
+	TEGRA_PIN_BUTTON_POWER_ON_PX5,
+};
+
+static const unsigned button_vol_up_px6_pins[] = {
+	TEGRA_PIN_BUTTON_VOL_UP_PX6,
+};
+
+static const unsigned button_vol_down_px7_pins[] = {
+	TEGRA_PIN_BUTTON_VOL_DOWN_PX7,
+};
+
+static const unsigned button_slide_sw_py0_pins[] = {
+	TEGRA_PIN_BUTTON_SLIDE_SW_PY0,
+};
+
+static const unsigned button_home_py1_pins[] = {
+	TEGRA_PIN_BUTTON_HOME_PY1,
+};
+
+static const unsigned lcd_te_py2_pins[] = {
+	TEGRA_PIN_LCD_TE_PY2,
+};
+
+static const unsigned pwr_i2c_scl_py3_pins[] = {
+	TEGRA_PIN_PWR_I2C_SCL_PY3,
+};
+
+static const unsigned pwr_i2c_sda_py4_pins[] = {
+	TEGRA_PIN_PWR_I2C_SDA_PY4,
+};
+
+static const unsigned clk_32k_out_py5_pins[] = {
+	TEGRA_PIN_CLK_32K_OUT_PY5,
+};
+
+static const unsigned pz0_pins[] = {
+	TEGRA_PIN_PZ0,
+};
+
+static const unsigned pz1_pins[] = {
+	TEGRA_PIN_PZ1,
+};
+
+static const unsigned pz2_pins[] = {
+	TEGRA_PIN_PZ2,
+};
+
+static const unsigned pz3_pins[] = {
+	TEGRA_PIN_PZ3,
+};
+
+static const unsigned pz4_pins[] = {
+	TEGRA_PIN_PZ4,
+};
+
+static const unsigned pz5_pins[] = {
+	TEGRA_PIN_PZ5,
+};
+
+static const unsigned dap2_fs_paa0_pins[] = {
+	TEGRA_PIN_DAP2_FS_PAA0,
+};
+
+static const unsigned dap2_sclk_paa1_pins[] = {
+	TEGRA_PIN_DAP2_SCLK_PAA1,
+};
+
+static const unsigned dap2_din_paa2_pins[] = {
+	TEGRA_PIN_DAP2_DIN_PAA2,
+};
+
+static const unsigned dap2_dout_paa3_pins[] = {
+	TEGRA_PIN_DAP2_DOUT_PAA3,
+};
+
+static const unsigned aud_mclk_pbb0_pins[] = {
+	TEGRA_PIN_AUD_MCLK_PBB0,
+};
+
+static const unsigned dvfs_pwm_pbb1_pins[] = {
+	TEGRA_PIN_DVFS_PWM_PBB1,
+};
+
+static const unsigned dvfs_clk_pbb2_pins[] = {
+	TEGRA_PIN_DVFS_CLK_PBB2,
+};
+
+static const unsigned gpio_x1_aud_pbb3_pins[] = {
+	TEGRA_PIN_GPIO_X1_AUD_PBB3,
+};
+
+static const unsigned gpio_x3_aud_pbb4_pins[] = {
+	TEGRA_PIN_GPIO_X3_AUD_PBB4,
+};
+
+static const unsigned hdmi_cec_pcc0_pins[] = {
+	TEGRA_PIN_HDMI_CEC_PCC0,
+};
+
+static const unsigned hdmi_int_dp_hpd_pcc1_pins[] = {
+	TEGRA_PIN_HDMI_INT_DP_HPD_PCC1,
+};
+
+static const unsigned spdif_out_pcc2_pins[] = {
+	TEGRA_PIN_SPDIF_OUT_PCC2,
+};
+
+static const unsigned spdif_in_pcc3_pins[] = {
+	TEGRA_PIN_SPDIF_IN_PCC3,
+};
+
+static const unsigned usb_vbus_en0_pcc4_pins[] = {
+	TEGRA_PIN_USB_VBUS_EN0_PCC4,
+};
+
+static const unsigned usb_vbus_en1_pcc5_pins[] = {
+	TEGRA_PIN_USB_VBUS_EN1_PCC5,
+};
+
+static const unsigned dp_hpd0_pcc6_pins[] = {
+	TEGRA_PIN_DP_HPD0_PCC6,
+};
+
+static const unsigned pcc7_pins[] = {
+	TEGRA_PIN_PCC7,
+};
+
+static const unsigned spi2_cs1_pdd0_pins[] = {
+	TEGRA_PIN_SPI2_CS1_PDD0,
+};
+
+static const unsigned qspi_sck_pee0_pins[] = {
+	TEGRA_PIN_QSPI_SCK_PEE0,
+};
+
+static const unsigned qspi_cs_n_pee1_pins[] = {
+	TEGRA_PIN_QSPI_CS_N_PEE1,
+};
+
+static const unsigned qspi_io0_pee2_pins[] = {
+	TEGRA_PIN_QSPI_IO0_PEE2,
+};
+
+static const unsigned qspi_io1_pee3_pins[] = {
+	TEGRA_PIN_QSPI_IO1_PEE3,
+};
+
+static const unsigned qspi_io2_pee4_pins[] = {
+	TEGRA_PIN_QSPI_IO2_PEE4,
+};
+
+static const unsigned qspi_io3_pee5_pins[] = {
+	TEGRA_PIN_QSPI_IO3_PEE5,
+};
+
+static const unsigned core_pwr_req_pins[] = {
+	TEGRA_PIN_CORE_PWR_REQ,
+};
+
+static const unsigned cpu_pwr_req_pins[] = {
+	TEGRA_PIN_CPU_PWR_REQ,
+};
+
+static const unsigned pwr_int_n_pins[] = {
+	TEGRA_PIN_PWR_INT_N,
+};
+
+static const unsigned clk_32k_in_pins[] = {
+	TEGRA_PIN_CLK_32K_IN,
+};
+
+static const unsigned jtag_rtck_pins[] = {
+	TEGRA_PIN_JTAG_RTCK,
+};
+
+static const unsigned batt_bcl_pins[] = {
+	TEGRA_PIN_BATT_BCL,
+};
+
+static const unsigned clk_req_pins[] = {
+	TEGRA_PIN_CLK_REQ,
+};
+
+static const unsigned shutdown_pins[] = {
+	TEGRA_PIN_SHUTDOWN,
+};
+
+static const unsigned drive_pa6_pins[] = {
+	TEGRA_PIN_PA6,
+};
+
+static const unsigned drive_pcc7_pins[] = {
+	TEGRA_PIN_PCC7,
+};
+
+static const unsigned drive_pe6_pins[] = {
+	TEGRA_PIN_PE6,
+};
+
+static const unsigned drive_pe7_pins[] = {
+	TEGRA_PIN_PE7,
+};
+
+static const unsigned drive_ph6_pins[] = {
+	TEGRA_PIN_PH6,
+};
+
+static const unsigned drive_pk0_pins[] = {
+	TEGRA_PIN_PK0,
+};
+
+static const unsigned drive_pk1_pins[] = {
+	TEGRA_PIN_PK1,
+};
+
+static const unsigned drive_pk2_pins[] = {
+	TEGRA_PIN_PK2,
+};
+
+static const unsigned drive_pk3_pins[] = {
+	TEGRA_PIN_PK3,
+};
+
+static const unsigned drive_pk4_pins[] = {
+	TEGRA_PIN_PK4,
+};
+
+static const unsigned drive_pk5_pins[] = {
+	TEGRA_PIN_PK5,
+};
+
+static const unsigned drive_pk6_pins[] = {
+	TEGRA_PIN_PK6,
+};
+
+static const unsigned drive_pk7_pins[] = {
+	TEGRA_PIN_PK7,
+};
+
+static const unsigned drive_pl0_pins[] = {
+	TEGRA_PIN_PL0,
+};
+
+static const unsigned drive_pl1_pins[] = {
+	TEGRA_PIN_PL1,
+};
+
+static const unsigned drive_pz0_pins[] = {
+	TEGRA_PIN_PZ0,
+};
+
+static const unsigned drive_pz1_pins[] = {
+	TEGRA_PIN_PZ1,
+};
+
+static const unsigned drive_pz2_pins[] = {
+	TEGRA_PIN_PZ2,
+};
+
+static const unsigned drive_pz3_pins[] = {
+	TEGRA_PIN_PZ3,
+};
+
+static const unsigned drive_pz4_pins[] = {
+	TEGRA_PIN_PZ4,
+};
+
+static const unsigned drive_pz5_pins[] = {
+	TEGRA_PIN_PZ5,
+};
+
+static const unsigned drive_sdmmc1_pins[] = {
+	TEGRA_PIN_SDMMC1_CLK_PM0,
+	TEGRA_PIN_SDMMC1_CMD_PM1,
+	TEGRA_PIN_SDMMC1_DAT3_PM2,
+	TEGRA_PIN_SDMMC1_DAT2_PM3,
+	TEGRA_PIN_SDMMC1_DAT1_PM4,
+	TEGRA_PIN_SDMMC1_DAT0_PM5,
+};
+
+static const unsigned drive_sdmmc2_pins[] = {
+};
+
+static const unsigned drive_sdmmc3_pins[] = {
+	TEGRA_PIN_SDMMC3_CLK_PP0,
+	TEGRA_PIN_SDMMC3_CMD_PP1,
+	TEGRA_PIN_SDMMC3_DAT3_PP2,
+	TEGRA_PIN_SDMMC3_DAT2_PP3,
+	TEGRA_PIN_SDMMC3_DAT1_PP4,
+	TEGRA_PIN_SDMMC3_DAT0_PP5,
+};
+
+static const unsigned drive_sdmmc4_pins[] = {
+};
+
+enum tegra_mux {
+	TEGRA_MUX_AUD,
+	TEGRA_MUX_BCL,
+	TEGRA_MUX_BLINK,
+	TEGRA_MUX_CCLA,
+	TEGRA_MUX_CEC,
+	TEGRA_MUX_CLDVFS,
+	TEGRA_MUX_CLK,
+	TEGRA_MUX_CORE,
+	TEGRA_MUX_CPU,
+	TEGRA_MUX_DISPLAYA,
+	TEGRA_MUX_DISPLAYB,
+	TEGRA_MUX_DMIC1,
+	TEGRA_MUX_DMIC2,
+	TEGRA_MUX_DMIC3,
+	TEGRA_MUX_DP,
+	TEGRA_MUX_DTV,
+	TEGRA_MUX_EXTPERIPH3,
+	TEGRA_MUX_I2C1,
+	TEGRA_MUX_I2C2,
+	TEGRA_MUX_I2C3,
+	TEGRA_MUX_I2CPMU,
+	TEGRA_MUX_I2CVI,
+	TEGRA_MUX_I2S1,
+	TEGRA_MUX_I2S2,
+	TEGRA_MUX_I2S3,
+	TEGRA_MUX_I2S4A,
+	TEGRA_MUX_I2S4B,
+	TEGRA_MUX_I2S5A,
+	TEGRA_MUX_I2S5B,
+	TEGRA_MUX_IQC0,
+	TEGRA_MUX_IQC1,
+	TEGRA_MUX_JTAG,
+	TEGRA_MUX_PE,
+	TEGRA_MUX_PE0,
+	TEGRA_MUX_PE1,
+	TEGRA_MUX_PMI,
+	TEGRA_MUX_PWM0,
+	TEGRA_MUX_PWM1,
+	TEGRA_MUX_PWM2,
+	TEGRA_MUX_PWM3,
+	TEGRA_MUX_QSPI,
+	TEGRA_MUX_RSVD0,
+	TEGRA_MUX_RSVD1,
+	TEGRA_MUX_RSVD2,
+	TEGRA_MUX_RSVD3,
+	TEGRA_MUX_SATA,
+	TEGRA_MUX_SDMMC1,
+	TEGRA_MUX_SDMMC3,
+	TEGRA_MUX_SHUTDOWN,
+	TEGRA_MUX_SOC,
+	TEGRA_MUX_SOR0,
+	TEGRA_MUX_SOR1,
+	TEGRA_MUX_SPDIF,
+	TEGRA_MUX_SPI1,
+	TEGRA_MUX_SPI2,
+	TEGRA_MUX_SPI3,
+	TEGRA_MUX_SPI4,
+	TEGRA_MUX_SYS,
+	TEGRA_MUX_TOUCH,
+	TEGRA_MUX_UART,
+	TEGRA_MUX_UARTA,
+	TEGRA_MUX_UARTB,
+	TEGRA_MUX_UARTC,
+	TEGRA_MUX_UARTD,
+	TEGRA_MUX_USB,
+	TEGRA_MUX_VGP1,
+	TEGRA_MUX_VGP2,
+	TEGRA_MUX_VGP3,
+	TEGRA_MUX_VGP4,
+	TEGRA_MUX_VGP5,
+	TEGRA_MUX_VGP6,
+	TEGRA_MUX_VIMCLK,
+	TEGRA_MUX_VIMCLK2,
+};
+
+#define FUNCTION(fname)					\
+	{						\
+		.name = #fname,				\
+	}
+
+static struct tegra_function tegra210_functions[] = {
+	FUNCTION(aud),
+	FUNCTION(bcl),
+	FUNCTION(blink),
+	FUNCTION(ccla),
+	FUNCTION(cec),
+	FUNCTION(cldvfs),
+	FUNCTION(clk),
+	FUNCTION(core),
+	FUNCTION(cpu),
+	FUNCTION(displaya),
+	FUNCTION(displayb),
+	FUNCTION(dmic1),
+	FUNCTION(dmic2),
+	FUNCTION(dmic3),
+	FUNCTION(dp),
+	FUNCTION(dtv),
+	FUNCTION(extperiph3),
+	FUNCTION(i2c1),
+	FUNCTION(i2c2),
+	FUNCTION(i2c3),
+	FUNCTION(i2cpmu),
+	FUNCTION(i2cvi),
+	FUNCTION(i2s1),
+	FUNCTION(i2s2),
+	FUNCTION(i2s3),
+	FUNCTION(i2s4a),
+	FUNCTION(i2s4b),
+	FUNCTION(i2s5a),
+	FUNCTION(i2s5b),
+	FUNCTION(iqc0),
+	FUNCTION(iqc1),
+	FUNCTION(jtag),
+	FUNCTION(pe),
+	FUNCTION(pe0),
+	FUNCTION(pe1),
+	FUNCTION(pmi),
+	FUNCTION(pwm0),
+	FUNCTION(pwm1),
+	FUNCTION(pwm2),
+	FUNCTION(pwm3),
+	FUNCTION(qspi),
+	FUNCTION(rsvd0),
+	FUNCTION(rsvd1),
+	FUNCTION(rsvd2),
+	FUNCTION(rsvd3),
+	FUNCTION(sata),
+	FUNCTION(sdmmc1),
+	FUNCTION(sdmmc3),
+	FUNCTION(shutdown),
+	FUNCTION(soc),
+	FUNCTION(sor0),
+	FUNCTION(sor1),
+	FUNCTION(spdif),
+	FUNCTION(spi1),
+	FUNCTION(spi2),
+	FUNCTION(spi3),
+	FUNCTION(spi4),
+	FUNCTION(sys),
+	FUNCTION(touch),
+	FUNCTION(uart),
+	FUNCTION(uarta),
+	FUNCTION(uartb),
+	FUNCTION(uartc),
+	FUNCTION(uartd),
+	FUNCTION(usb),
+	FUNCTION(vgp1),
+	FUNCTION(vgp2),
+	FUNCTION(vgp3),
+	FUNCTION(vgp4),
+	FUNCTION(vgp5),
+	FUNCTION(vgp6),
+	FUNCTION(vimclk),
+	FUNCTION(vimclk2),
+};
+
+#define DRV_PINGROUP_REG_A		0x8d4	/* bank 0 */
+#define PINGROUP_REG_A			0x3000	/* bank 1 */
+
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
+#define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
+
+#define PINGROUP_BIT_Y(b)		(b)
+#define PINGROUP_BIT_N(b)		(-1)
+
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, hsm, drvtype, e_io_hv,	\
+		 rdrv, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b,	\
+		 slwr_w, slwf_b, slwf_w)				\
+	{								\
+		.name = #pg_name,					\
+		.pins = pg_name##_pins,					\
+		.npins = ARRAY_SIZE(pg_name##_pins),			\
+		.funcs = {						\
+			TEGRA_MUX_##f0,					\
+			TEGRA_MUX_##f1,					\
+			TEGRA_MUX_##f2,					\
+			TEGRA_MUX_##f3,					\
+		},							\
+		.mux_reg = PINGROUP_REG(r),				\
+		.mux_bank = 1,						\
+		.mux_bit = 0,						\
+		.pupd_reg = PINGROUP_REG(r),				\
+		.pupd_bank = 1,						\
+		.pupd_bit = 2,						\
+		.tri_reg = PINGROUP_REG(r),				\
+		.tri_bank = 1,						\
+		.tri_bit = 4,						\
+		.einput_bit = 6,					\
+		.odrain_bit = 11,					\
+		.lock_bit = 7,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = PINGROUP_BIT_##e_io_hv(10),		\
+		.hsm_bit = PINGROUP_BIT_##hsm(9),			\
+		.schmitt_bit = 12,					\
+		.drvtype_bit = PINGROUP_BIT_##drvtype(13),		\
+		.drv_reg = DRV_PINGROUP_REG(rdrv),			\
+		.drv_bank = 0,						\
+		.lpmd_bit = -1,						\
+		.drvdn_bit = drvdn_b,					\
+		.drvdn_width = drvdn_w,					\
+		.drvup_bit = drvup_b,					\
+		.drvup_width = drvup_w,					\
+		.slwr_bit = slwr_b,					\
+		.slwr_width = slwr_w,					\
+		.slwf_bit = slwf_b,					\
+		.slwf_width = slwf_w,					\
+	}
+
+#define DRV_PINGROUP(pg_name, r, drvdn_b, drvdn_w, drvup_b, drvup_w,	\
+		     slwr_b, slwr_w, slwf_b, slwf_w)			\
+	{								\
+		.name = "drive_" #pg_name,				\
+		.pins = drive_##pg_name##_pins,				\
+		.npins = ARRAY_SIZE(drive_##pg_name##_pins),		\
+		.mux_reg = -1,						\
+		.pupd_reg = -1,						\
+		.tri_reg = -1,						\
+		.einput_bit = -1,					\
+		.odrain_bit = -1,					\
+		.lock_bit = -1,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = DRV_PINGROUP_REG(r),				\
+		.drv_bank = 0,						\
+		.hsm_bit = -1,						\
+		.schmitt_bit = -1,					\
+		.lpmd_bit = -1,						\
+		.drvdn_bit = drvdn_b,					\
+		.drvdn_width = drvdn_w,					\
+		.drvup_bit = drvup_b,					\
+		.drvup_width = drvup_w,					\
+		.slwr_bit = slwr_b,					\
+		.slwr_width = slwr_w,					\
+		.slwf_bit = slwf_b,					\
+		.slwf_width = slwf_w,					\
+		.drvtype_bit = -1,					\
+	}
+
+static const struct tegra_pingroup tegra210_groups[] = {
+	/*       pg_name,              f0,         f1,     f2,    f3,    r,      hsm, drvtype, e_io_hv, rdrv,  drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w */
+	PINGROUP(sdmmc1_clk_pm0,       SDMMC1,     RSVD1,  RSVD2, RSVD3, 0x3000, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc1_cmd_pm1,       SDMMC1,     SPI3,   RSVD2, RSVD3, 0x3004, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc1_dat3_pm2,      SDMMC1,     SPI3,   RSVD2, RSVD3, 0x3008, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc1_dat2_pm3,      SDMMC1,     SPI3,   RSVD2, RSVD3, 0x300c, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc1_dat1_pm4,      SDMMC1,     SPI3,   RSVD2, RSVD3, 0x3010, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc1_dat0_pm5,      SDMMC1,     RSVD1,  RSVD2, RSVD3, 0x3014, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc3_clk_pp0,       SDMMC3,     RSVD1,  RSVD2, RSVD3, 0x301c, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc3_cmd_pp1,       SDMMC3,     RSVD1,  RSVD2, RSVD3, 0x3020, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc3_dat0_pp5,      SDMMC3,     RSVD1,  RSVD2, RSVD3, 0x3024, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc3_dat1_pp4,      SDMMC3,     RSVD1,  RSVD2, RSVD3, 0x3028, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc3_dat2_pp3,      SDMMC3,     RSVD1,  RSVD2, RSVD3, 0x302c, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(sdmmc3_dat3_pp2,      SDMMC3,     RSVD1,  RSVD2, RSVD3, 0x3030, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pex_l0_rst_n_pa0,     PE0,        RSVD1,  RSVD2, RSVD3, 0x3038, N,   N,       Y,       0xa5c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pex_l0_clkreq_n_pa1,  PE0,        RSVD1,  RSVD2, RSVD3, 0x303c, N,   N,       Y,       0xa58, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pex_wake_n_pa2,       PE,         RSVD1,  RSVD2, RSVD3, 0x3040, N,   N,       Y,       0xa68, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pex_l1_rst_n_pa3,     PE1,        RSVD1,  RSVD2, RSVD3, 0x3044, N,   N,       Y,       0xa64, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pex_l1_clkreq_n_pa4,  PE1,        RSVD1,  RSVD2, RSVD3, 0x3048, N,   N,       Y,       0xa60, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(sata_led_active_pa5,  SATA,       RSVD1,  RSVD2, RSVD3, 0x304c, N,   N,       N,       0xa94, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(spi1_mosi_pc0,        SPI1,       RSVD1,  RSVD2, RSVD3, 0x3050, Y,   Y,       N,       0xae0, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi1_miso_pc1,        SPI1,       RSVD1,  RSVD2, RSVD3, 0x3054, Y,   Y,       N,       0xadc, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi1_sck_pc2,         SPI1,       RSVD1,  RSVD2, RSVD3, 0x3058, Y,   Y,       N,       0xae4, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi1_cs0_pc3,         SPI1,       RSVD1,  RSVD2, RSVD3, 0x305c, Y,   Y,       N,       0xad4, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi1_cs1_pc4,         SPI1,       RSVD1,  RSVD2, RSVD3, 0x3060, Y,   Y,       N,       0xad8, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi2_mosi_pb4,        SPI2,       DTV,    RSVD2, RSVD3, 0x3064, Y,   Y,       N,       0xaf4, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi2_miso_pb5,        SPI2,       DTV,    RSVD2, RSVD3, 0x3068, Y,   Y,       N,       0xaf0, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi2_sck_pb6,         SPI2,       DTV,    RSVD2, RSVD3, 0x306c, Y,   Y,       N,       0xaf8, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi2_cs0_pb7,         SPI2,       DTV,    RSVD2, RSVD3, 0x3070, Y,   Y,       N,       0xae8, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi2_cs1_pdd0,        SPI2,       RSVD1,  RSVD2, RSVD3, 0x3074, Y,   Y,       N,       0xaec, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi4_mosi_pc7,        SPI4,       RSVD1,  RSVD2, RSVD3, 0x3078, Y,   Y,       N,       0xb04, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi4_miso_pd0,        SPI4,       RSVD1,  RSVD2, RSVD3, 0x307c, Y,   Y,       N,       0xb00, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi4_sck_pc5,         SPI4,       RSVD1,  RSVD2, RSVD3, 0x3080, Y,   Y,       N,       0xb08, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(spi4_cs0_pc6,         SPI4,       RSVD1,  RSVD2, RSVD3, 0x3084, Y,   Y,       N,       0xafc, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(qspi_sck_pee0,        QSPI,       RSVD1,  RSVD2, RSVD3, 0x3088, Y,   Y,       N,       0xa90, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(qspi_cs_n_pee1,       QSPI,       RSVD1,  RSVD2, RSVD3, 0x308c, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(qspi_io0_pee2,        QSPI,       RSVD1,  RSVD2, RSVD3, 0x3090, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(qspi_io1_pee3,        QSPI,       RSVD1,  RSVD2, RSVD3, 0x3094, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(qspi_io2_pee4,        QSPI,       RSVD1,  RSVD2, RSVD3, 0x3098, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(qspi_io3_pee5,        QSPI,       RSVD1,  RSVD2, RSVD3, 0x309c, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(dmic1_clk_pe0,        DMIC1,      I2S3,   RSVD2, RSVD3, 0x30a4, N,   N,       N,       0x984, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dmic1_dat_pe1,        DMIC1,      I2S3,   RSVD2, RSVD3, 0x30a8, N,   N,       N,       0x988, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dmic2_clk_pe2,        DMIC2,      I2S3,   RSVD2, RSVD3, 0x30ac, N,   N,       N,       0x98c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dmic2_dat_pe3,        DMIC2,      I2S3,   RSVD2, RSVD3, 0x30b0, N,   N,       N,       0x990, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dmic3_clk_pe4,        DMIC3,      I2S5A,  RSVD2, RSVD3, 0x30b4, N,   N,       N,       0x994, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dmic3_dat_pe5,        DMIC3,      I2S5A,  RSVD2, RSVD3, 0x30b8, N,   N,       N,       0x998, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gen1_i2c_scl_pj1,     I2C1,       RSVD1,  RSVD2, RSVD3, 0x30bc, N,   N,       Y,       0x9a8, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gen1_i2c_sda_pj0,     I2C1,       RSVD1,  RSVD2, RSVD3, 0x30c0, N,   N,       Y,       0x9ac, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gen2_i2c_scl_pj2,     I2C2,       RSVD1,  RSVD2, RSVD3, 0x30c4, N,   N,       Y,       0x9b0, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gen2_i2c_sda_pj3,     I2C2,       RSVD1,  RSVD2, RSVD3, 0x30c8, N,   N,       Y,       0x9b4, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gen3_i2c_scl_pf0,     I2C3,       RSVD1,  RSVD2, RSVD3, 0x30cc, N,   N,       Y,       0x9b8, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gen3_i2c_sda_pf1,     I2C3,       RSVD1,  RSVD2, RSVD3, 0x30d0, N,   N,       Y,       0x9bc, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam_i2c_scl_ps2,      I2C3,       I2CVI,  RSVD2, RSVD3, 0x30d4, N,   N,       Y,       0x934, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam_i2c_sda_ps3,      I2C3,       I2CVI,  RSVD2, RSVD3, 0x30d8, N,   N,       Y,       0x938, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pwr_i2c_scl_py3,      I2CPMU,     RSVD1,  RSVD2, RSVD3, 0x30dc, N,   N,       Y,       0xa6c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pwr_i2c_sda_py4,      I2CPMU,     RSVD1,  RSVD2, RSVD3, 0x30e0, N,   N,       Y,       0xa70, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart1_tx_pu0,         UARTA,      RSVD1,  RSVD2, RSVD3, 0x30e4, N,   N,       N,       0xb28, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart1_rx_pu1,         UARTA,      RSVD1,  RSVD2, RSVD3, 0x30e8, N,   N,       N,       0xb24, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart1_rts_pu2,        UARTA,      RSVD1,  RSVD2, RSVD3, 0x30ec, N,   N,       N,       0xb20, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart1_cts_pu3,        UARTA,      RSVD1,  RSVD2, RSVD3, 0x30f0, N,   N,       N,       0xb1c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart2_tx_pg0,         UARTB,      I2S4A,  SPDIF, UART,  0x30f4, N,   N,       N,       0xb38, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart2_rx_pg1,         UARTB,      I2S4A,  SPDIF, UART,  0x30f8, N,   N,       N,       0xb34, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart2_rts_pg2,        UARTB,      I2S4A,  RSVD2, UART,  0x30fc, N,   N,       N,       0xb30, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart2_cts_pg3,        UARTB,      I2S4A,  RSVD2, UART,  0x3100, N,   N,       N,       0xb2c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart3_tx_pd1,         UARTC,      SPI4,   RSVD2, RSVD3, 0x3104, N,   N,       N,       0xb48, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart3_rx_pd2,         UARTC,      SPI4,   RSVD2, RSVD3, 0x3108, N,   N,       N,       0xb44, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart3_rts_pd3,        UARTC,      SPI4,   RSVD2, RSVD3, 0x310c, N,   N,       N,       0xb40, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart3_cts_pd4,        UARTC,      SPI4,   RSVD2, RSVD3, 0x3110, N,   N,       N,       0xb3c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart4_tx_pi4,         UARTD,      UART,   RSVD2, RSVD3, 0x3114, N,   N,       N,       0xb58, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart4_rx_pi5,         UARTD,      UART,   RSVD2, RSVD3, 0x3118, N,   N,       N,       0xb54, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart4_rts_pi6,        UARTD,      UART,   RSVD2, RSVD3, 0x311c, N,   N,       N,       0xb50, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(uart4_cts_pi7,        UARTD,      UART,   RSVD2, RSVD3, 0x3120, N,   N,       N,       0xb4c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dap1_fs_pb0,          I2S1,       RSVD1,  RSVD2, RSVD3, 0x3124, Y,   Y,       N,       0x95c, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap1_din_pb1,         I2S1,       RSVD1,  RSVD2, RSVD3, 0x3128, Y,   Y,       N,       0x954, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap1_dout_pb2,        I2S1,       RSVD1,  RSVD2, RSVD3, 0x312c, Y,   Y,       N,       0x958, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap1_sclk_pb3,        I2S1,       RSVD1,  RSVD2, RSVD3, 0x3130, Y,   Y,       N,       0x960, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap2_fs_paa0,         I2S2,       RSVD1,  RSVD2, RSVD3, 0x3134, Y,   Y,       N,       0x96c, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap2_din_paa2,        I2S2,       RSVD1,  RSVD2, RSVD3, 0x3138, Y,   Y,       N,       0x964, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap2_dout_paa3,       I2S2,       RSVD1,  RSVD2, RSVD3, 0x313c, Y,   Y,       N,       0x968, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap2_sclk_paa1,       I2S2,       RSVD1,  RSVD2, RSVD3, 0x3140, Y,   Y,       N,       0x970, -1,      -1,      -1,      -1,      28,     2,      30,     2),
+	PINGROUP(dap4_fs_pj4,          I2S4B,      RSVD1,  RSVD2, RSVD3, 0x3144, N,   N,       N,       0x97c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dap4_din_pj5,         I2S4B,      RSVD1,  RSVD2, RSVD3, 0x3148, N,   N,       N,       0x974, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dap4_dout_pj6,        I2S4B,      RSVD1,  RSVD2, RSVD3, 0x314c, N,   N,       N,       0x978, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dap4_sclk_pj7,        I2S4B,      RSVD1,  RSVD2, RSVD3, 0x3150, N,   N,       N,       0x980, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam1_mclk_ps0,        EXTPERIPH3, RSVD1,  RSVD2, RSVD3, 0x3154, N,   N,       N,       0x918, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam2_mclk_ps1,        EXTPERIPH3, RSVD1,  RSVD2, RSVD3, 0x3158, N,   N,       N,       0x924, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(jtag_rtck,            JTAG,       RSVD1,  RSVD2, RSVD3, 0x315c, N,   N,       N,       0xa2c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(clk_32k_in,           CLK,        RSVD1,  RSVD2, RSVD3, 0x3160, N,   N,       N,       0x940, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(clk_32k_out_py5,      SOC,        BLINK,  RSVD2, RSVD3, 0x3164, N,   N,       N,       0x944, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(batt_bcl,             BCL,        RSVD1,  RSVD2, RSVD3, 0x3168, N,   N,       Y,       0x8f8, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(clk_req,              SYS,        RSVD1,  RSVD2, RSVD3, 0x316c, N,   N,       N,       0x948, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cpu_pwr_req,          CPU,        RSVD1,  RSVD2, RSVD3, 0x3170, N,   N,       N,       0x950, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pwr_int_n,            PMI,        RSVD1,  RSVD2, RSVD3, 0x3174, N,   N,       N,       0xa74, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(shutdown,             SHUTDOWN,   RSVD1,  RSVD2, RSVD3, 0x3178, N,   N,       N,       0xac8, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(core_pwr_req,         CORE,       RSVD1,  RSVD2, RSVD3, 0x317c, N,   N,       N,       0x94c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(aud_mclk_pbb0,        AUD,        RSVD1,  RSVD2, RSVD3, 0x3180, N,   N,       N,       0x8f4, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dvfs_pwm_pbb1,        RSVD0,      CLDVFS, SPI3,  RSVD3, 0x3184, N,   N,       N,       0x9a4, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dvfs_clk_pbb2,        RSVD0,      CLDVFS, SPI3,  RSVD3, 0x3188, N,   N,       N,       0x9a0, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gpio_x1_aud_pbb3,     RSVD0,      RSVD1,  SPI3,  RSVD3, 0x318c, N,   N,       N,       0xa14, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gpio_x3_aud_pbb4,     RSVD0,      RSVD1,  SPI3,  RSVD3, 0x3190, N,   N,       N,       0xa18, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pcc7,                 RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3194, N,   N,       Y,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(hdmi_cec_pcc0,        CEC,        RSVD1,  RSVD2, RSVD3, 0x3198, N,   N,       Y,       0xa24, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(hdmi_int_dp_hpd_pcc1, DP,         RSVD1,  RSVD2, RSVD3, 0x319c, N,   N,       Y,       0xa28, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(spdif_out_pcc2,       SPDIF,      RSVD1,  RSVD2, RSVD3, 0x31a0, N,   N,       N,       0xad0, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(spdif_in_pcc3,        SPDIF,      RSVD1,  RSVD2, RSVD3, 0x31a4, N,   N,       N,       0xacc, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(usb_vbus_en0_pcc4,    USB,        RSVD1,  RSVD2, RSVD3, 0x31a8, N,   N,       Y,       0xb5c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(usb_vbus_en1_pcc5,    USB,        RSVD1,  RSVD2, RSVD3, 0x31ac, N,   N,       Y,       0xb60, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(dp_hpd0_pcc6,         DP,         RSVD1,  RSVD2, RSVD3, 0x31b0, N,   N,       N,       0x99c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(wifi_en_ph0,          RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31b4, N,   N,       N,       0xb64, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(wifi_rst_ph1,         RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31b8, N,   N,       N,       0xb68, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(wifi_wake_ap_ph2,     RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31bc, N,   N,       N,       0xb6c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(ap_wake_bt_ph3,       RSVD0,      UARTB,  SPDIF, RSVD3, 0x31c0, N,   N,       N,       0x8ec, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(bt_rst_ph4,           RSVD0,      UARTB,  SPDIF, RSVD3, 0x31c4, N,   N,       N,       0x8fc, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(bt_wake_ap_ph5,       RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31c8, N,   N,       N,       0x900, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(ap_wake_nfc_ph7,      RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31cc, N,   N,       N,       0x8f0, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(nfc_en_pi0,           RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31d0, N,   N,       N,       0xa50, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(nfc_int_pi1,          RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31d4, N,   N,       N,       0xa54, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gps_en_pi2,           RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31d8, N,   N,       N,       0xa1c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(gps_rst_pi3,          RSVD0,      RSVD1,  RSVD2, RSVD3, 0x31dc, N,   N,       N,       0xa20, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam_rst_ps4,          VGP1,       RSVD1,  RSVD2, RSVD3, 0x31e0, N,   N,       N,       0x93c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam_af_en_ps5,        VIMCLK,     VGP2,   RSVD2, RSVD3, 0x31e4, N,   N,       N,       0x92c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam_flash_en_ps6,     VIMCLK,     VGP3,   RSVD2, RSVD3, 0x31e8, N,   N,       N,       0x930, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam1_pwdn_ps7,        VGP4,       RSVD1,  RSVD2, RSVD3, 0x31ec, N,   N,       N,       0x91c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam2_pwdn_pt0,        VGP5,       RSVD1,  RSVD2, RSVD3, 0x31f0, N,   N,       N,       0x928, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(cam1_strobe_pt1,      VGP6,       RSVD1,  RSVD2, RSVD3, 0x31f4, N,   N,       N,       0x920, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(lcd_te_py2,           DISPLAYA,   RSVD1,  RSVD2, RSVD3, 0x31f8, N,   N,       N,       0xa44, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(lcd_bl_pwm_pv0,       DISPLAYA,   PWM0,   SOR0,  RSVD3, 0x31fc, N,   N,       N,       0xa34, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(lcd_bl_en_pv1,        RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3200, N,   N,       N,       0xa30, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(lcd_rst_pv2,          RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3204, N,   N,       N,       0xa40, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(lcd_gpio1_pv3,        DISPLAYB,   RSVD1,  RSVD2, RSVD3, 0x3208, N,   N,       N,       0xa38, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(lcd_gpio2_pv4,        DISPLAYB,   PWM1,   RSVD2, SOR1,  0x320c, N,   N,       N,       0xa3c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(ap_ready_pv5,         RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3210, N,   N,       N,       0x8e8, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(touch_rst_pv6,        RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3214, N,   N,       N,       0xb18, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(touch_clk_pv7,        TOUCH,      RSVD1,  RSVD2, RSVD3, 0x3218, N,   N,       N,       0xb10, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(modem_wake_ap_px0,    RSVD0,      RSVD1,  RSVD2, RSVD3, 0x321c, N,   N,       N,       0xa48, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(touch_int_px1,        RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3220, N,   N,       N,       0xb14, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(motion_int_px2,       RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3224, N,   N,       N,       0xa4c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(als_prox_int_px3,     RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3228, N,   N,       N,       0x8e4, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(temp_alert_px4,       RSVD0,      RSVD1,  RSVD2, RSVD3, 0x322c, N,   N,       N,       0xb0c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(button_power_on_px5,  RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3230, N,   N,       N,       0x908, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(button_vol_up_px6,    RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3234, N,   N,       N,       0x914, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(button_vol_down_px7,  RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3238, N,   N,       N,       0x910, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(button_slide_sw_py0,  RSVD0,      RSVD1,  RSVD2, RSVD3, 0x323c, N,   N,       N,       0x90c, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(button_home_py1,      RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3240, N,   N,       N,       0x904, 12,      5,       20,      5,       -1,     -1,     -1,     -1),
+	PINGROUP(pa6,                  SATA,       RSVD1,  RSVD2, RSVD3, 0x3244, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pe6,                  RSVD0,      I2S5A,  PWM2,  RSVD3, 0x3248, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pe7,                  RSVD0,      I2S5A,  PWM3,  RSVD3, 0x324c, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(ph6,                  RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3250, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk0,                  IQC0,       I2S5B,  RSVD2, RSVD3, 0x3254, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk1,                  IQC0,       I2S5B,  RSVD2, RSVD3, 0x3258, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk2,                  IQC0,       I2S5B,  RSVD2, RSVD3, 0x325c, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk3,                  IQC0,       I2S5B,  RSVD2, RSVD3, 0x3260, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk4,                  IQC1,       RSVD1,  RSVD2, RSVD3, 0x3264, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk5,                  IQC1,       RSVD1,  RSVD2, RSVD3, 0x3268, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk6,                  IQC1,       RSVD1,  RSVD2, RSVD3, 0x326c, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pk7,                  IQC1,       RSVD1,  RSVD2, RSVD3, 0x3270, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pl0,                  RSVD0,      RSVD1,  RSVD2, RSVD3, 0x3274, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pl1,                  SOC,        RSVD1,  RSVD2, RSVD3, 0x3278, Y,   Y,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pz0,                  VIMCLK2,    RSVD1,  RSVD2, RSVD3, 0x327c, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pz1,                  VIMCLK2,    SDMMC1, RSVD2, RSVD3, 0x3280, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pz2,                  SDMMC3,     CCLA,   RSVD2, RSVD3, 0x3284, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pz3,                  SDMMC3,     RSVD1,  RSVD2, RSVD3, 0x3288, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pz4,                  SDMMC1,     RSVD1,  RSVD2, RSVD3, 0x328c, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+	PINGROUP(pz5,                  SOC,        RSVD1,  RSVD2, RSVD3, 0x3290, N,   N,       N,       -1,    -1,      -1,      -1,      -1,      -1,     -1,     -1,     -1),
+
+	/* pg_name, r, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w */
+	DRV_PINGROUP(pa6,    0x9c0, 12, 5,  20, 5,  -1, -1, -1, -1),
+	DRV_PINGROUP(pcc7,   0x9c4, 12, 5,  20, 5,  -1, -1, -1, -1),
+	DRV_PINGROUP(pe6,    0x9c8, 12, 5,  20, 5,  -1, -1, -1, -1),
+	DRV_PINGROUP(pe7,    0x9cc, 12, 5,  20, 5,  -1, -1, -1, -1),
+	DRV_PINGROUP(ph6,    0x9d0, 12, 5,  20, 5,  -1, -1, -1, -1),
+	DRV_PINGROUP(pk0,    0x9d4, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pk1,    0x9d8, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pk2,    0x9dc, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pk3,    0x9e0, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pk4,    0x9e4, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pk5,    0x9e8, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pk6,    0x9ec, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pk7,    0x9f0, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pl0,    0x9f4, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pl1,    0x9f8, -1, -1, -1, -1, 28, 2,  30, 2),
+	DRV_PINGROUP(pz0,    0x9fc, 12, 7,  20, 7,  -1, -1, -1, -1),
+	DRV_PINGROUP(pz1,    0xa00, 12, 7,  20, 7,  -1, -1, -1, -1),
+	DRV_PINGROUP(pz2,    0xa04, 12, 7,  20, 7,  -1, -1, -1, -1),
+	DRV_PINGROUP(pz3,    0xa08, 12, 7,  20, 7,  -1, -1, -1, -1),
+	DRV_PINGROUP(pz4,    0xa0c, 12, 7,  20, 7,  -1, -1, -1, -1),
+	DRV_PINGROUP(pz5,    0xa10, 12, 7,  20, 7,  -1, -1, -1, -1),
+	DRV_PINGROUP(sdmmc1, 0xa98, 12, 7,  20, 7,  28, 2,  30, 2),
+	DRV_PINGROUP(sdmmc2, 0xa9c, 2,  6,  8,  6,  28, 2,  30, 2),
+	DRV_PINGROUP(sdmmc3, 0xab0, 12, 7,  20, 7,  28, 2,  30, 2),
+	DRV_PINGROUP(sdmmc4, 0xab4, 2,  6,  8,  6,  28, 2,  30, 2),
+};
+
+static const struct tegra_pinctrl_soc_data tegra210_pinctrl = {
+	.ngpios = NUM_GPIOS,
+	.pins = tegra210_pins,
+	.npins = ARRAY_SIZE(tegra210_pins),
+	.functions = tegra210_functions,
+	.nfunctions = ARRAY_SIZE(tegra210_functions),
+	.groups = tegra210_groups,
+	.ngroups = ARRAY_SIZE(tegra210_groups),
+	.hsm_in_mux = true,
+	.schmitt_in_mux = true,
+	.drvtype_in_mux = true,
+};
+
+static int tegra210_pinctrl_probe(struct platform_device *pdev)
+{
+	return tegra_pinctrl_probe(pdev, &tegra210_pinctrl);
+}
+
+static const struct of_device_id tegra210_pinctrl_of_match[] = {
+	{ .compatible = "nvidia,tegra210-pinmux", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tegra210_pinctrl_of_match);
+
+static struct platform_driver tegra210_pinctrl_driver = {
+	.driver = {
+		.name = "tegra210-pinctrl",
+		.of_match_table = tegra210_pinctrl_of_match,
+	},
+	.probe = tegra210_pinctrl_probe,
+	.remove = tegra_pinctrl_remove,
+};
+module_platform_driver(tegra210_pinctrl_driver);
+
+MODULE_AUTHOR("NVIDIA");
+MODULE_DESCRIPTION("NVIDIA Tegra210 pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-tegra30.c b/drivers/pinctrl/pinctrl-tegra30.c
index f6edc2ff5494..47b2fd8bb2e9 100644
--- a/drivers/pinctrl/pinctrl-tegra30.c
+++ b/drivers/pinctrl/pinctrl-tegra30.c
@@ -2108,70 +2108,69 @@ static struct tegra_function tegra30_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 #define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
 #define PINGROUP_BIT_Y(b)		(b)
 #define PINGROUP_BIT_N(b)		(-1)
 
-#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior)		\
-	{							\
-		.name = #pg_name,				\
-		.pins = pg_name##_pins,				\
-		.npins = ARRAY_SIZE(pg_name##_pins),		\
-		.funcs = {					\
-			TEGRA_MUX_##f0,				\
-			TEGRA_MUX_##f1,				\
-			TEGRA_MUX_##f2,				\
-			TEGRA_MUX_##f3,				\
-		},						\
-		.mux_reg = PINGROUP_REG(r),			\
-		.mux_bank = 1,					\
-		.mux_bit = 0,					\
-		.pupd_reg = PINGROUP_REG(r),			\
-		.pupd_bank = 1,					\
-		.pupd_bit = 2,					\
-		.tri_reg = PINGROUP_REG(r),			\
-		.tri_bank = 1,					\
-		.tri_bit = 4,					\
-		.einput_bit = PINGROUP_BIT_Y(5),		\
-		.odrain_bit = PINGROUP_BIT_##od(6),		\
-		.lock_bit = PINGROUP_BIT_Y(7),			\
-		.ioreset_bit = PINGROUP_BIT_##ior(8),		\
-		.rcv_sel_bit = -1,				\
-		.drv_reg = -1,					\
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior)			\
+	{								\
+		.name = #pg_name,					\
+		.pins = pg_name##_pins,					\
+		.npins = ARRAY_SIZE(pg_name##_pins),			\
+		.funcs = {						\
+			TEGRA_MUX_##f0,					\
+			TEGRA_MUX_##f1,					\
+			TEGRA_MUX_##f2,					\
+			TEGRA_MUX_##f3,					\
+		},							\
+		.mux_reg = PINGROUP_REG(r),				\
+		.mux_bank = 1,						\
+		.mux_bit = 0,						\
+		.pupd_reg = PINGROUP_REG(r),				\
+		.pupd_bank = 1,						\
+		.pupd_bit = 2,						\
+		.tri_reg = PINGROUP_REG(r),				\
+		.tri_bank = 1,						\
+		.tri_bit = 4,						\
+		.einput_bit = 5,					\
+		.odrain_bit = PINGROUP_BIT_##od(6),			\
+		.lock_bit = 7,						\
+		.ioreset_bit = PINGROUP_BIT_##ior(8),			\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = -1,						\
 	}
 
-#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
-
-#define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,	\
-		     drvdn_b, drvdn_w, drvup_b, drvup_w,	\
-		     slwr_b, slwr_w, slwf_b, slwf_w)		\
-	{							\
-		.name = "drive_" #pg_name,			\
-		.pins = drive_##pg_name##_pins,			\
-		.npins = ARRAY_SIZE(drive_##pg_name##_pins),	\
-		.mux_reg = -1,					\
-		.pupd_reg = -1,					\
-		.tri_reg = -1,					\
-		.einput_bit = -1,				\
-		.odrain_bit = -1,				\
-		.lock_bit = -1,					\
-		.ioreset_bit = -1,				\
-		.rcv_sel_bit = -1,				\
-		.drv_reg = DRV_PINGROUP_REG(r),			\
-		.drv_bank = 0,					\
-		.hsm_bit = hsm_b,				\
-		.schmitt_bit = schmitt_b,			\
-		.lpmd_bit = lpmd_b,				\
-		.drvdn_bit = drvdn_b,				\
-		.drvdn_width = drvdn_w,				\
-		.drvup_bit = drvup_b,				\
-		.drvup_width = drvup_w,				\
-		.slwr_bit = slwr_b,				\
-		.slwr_width = slwr_w,				\
-		.slwf_bit = slwf_b,				\
-		.slwf_width = slwf_w,				\
-		.drvtype_bit = -1,				\
+#define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b,	\
+		     drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w,		\
+		     slwf_b, slwf_w)					\
+	{								\
+		.name = "drive_" #pg_name,				\
+		.pins = drive_##pg_name##_pins,				\
+		.npins = ARRAY_SIZE(drive_##pg_name##_pins),		\
+		.mux_reg = -1,						\
+		.pupd_reg = -1,						\
+		.tri_reg = -1,						\
+		.einput_bit = -1,					\
+		.odrain_bit = -1,					\
+		.lock_bit = -1,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = DRV_PINGROUP_REG(r),				\
+		.drv_bank = 0,						\
+		.hsm_bit = hsm_b,					\
+		.schmitt_bit = schmitt_b,				\
+		.lpmd_bit = lpmd_b,					\
+		.drvdn_bit = drvdn_b,					\
+		.drvdn_width = drvdn_w,					\
+		.drvup_bit = drvup_b,					\
+		.drvup_width = drvup_w,					\
+		.slwr_bit = slwr_b,					\
+		.slwr_width = slwr_w,					\
+		.slwf_bit = slwf_b,					\
+		.slwf_width = slwf_w,					\
+		.drvtype_bit = -1,					\
 	}
 
 static const struct tegra_pingroup tegra30_groups[] = {
@@ -2477,6 +2476,9 @@ static const struct tegra_pinctrl_soc_data tegra30_pinctrl = {
 	.nfunctions = ARRAY_SIZE(tegra30_functions),
 	.groups = tegra30_groups,
 	.ngroups = ARRAY_SIZE(tegra30_groups),
+	.hsm_in_mux = false,
+	.schmitt_in_mux = false,
+	.drvtype_in_mux = false,
 };
 
 static int tegra30_pinctrl_probe(struct platform_device *pdev)
diff --git a/drivers/pinctrl/pinctrl-tz1090-pdc.c b/drivers/pinctrl/pinctrl-tz1090-pdc.c
index fab6aafa6a9f..8a8911bb883a 100644
--- a/drivers/pinctrl/pinctrl-tz1090-pdc.c
+++ b/drivers/pinctrl/pinctrl-tz1090-pdc.c
@@ -969,7 +969,7 @@ static int tz1090_pdc_pinctrl_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id tz1090_pdc_pinctrl_of_match[] = {
+static const struct of_device_id tz1090_pdc_pinctrl_of_match[] = {
 	{ .compatible = "img,tz1090-pdc-pinctrl", },
 	{ },
 };
diff --git a/drivers/pinctrl/pinctrl-tz1090.c b/drivers/pinctrl/pinctrl-tz1090.c
index 8bd73075f9dd..fc5594a530c2 100644
--- a/drivers/pinctrl/pinctrl-tz1090.c
+++ b/drivers/pinctrl/pinctrl-tz1090.c
@@ -1984,7 +1984,7 @@ static int tz1090_pinctrl_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id tz1090_pinctrl_of_match[] = {
+static const struct of_device_id tz1090_pinctrl_of_match[] = {
 	{ .compatible = "img,tz1090-pinctrl", },
 	{ },
 };
diff --git a/drivers/pinctrl/qcom/pinctrl-msm.c b/drivers/pinctrl/qcom/pinctrl-msm.c
index a535f9c23678..f3d800f796c2 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm.c
@@ -193,11 +193,11 @@ static int msm_config_reg(struct msm_pinctrl *pctrl,
 		*mask = 7;
 		break;
 	case PIN_CONFIG_OUTPUT:
+	case PIN_CONFIG_INPUT_ENABLE:
 		*bit = g->oe_bit;
 		*mask = 1;
 		break;
 	default:
-		dev_err(pctrl->dev, "Invalid config param %04x\n", param);
 		return -ENOTSUPP;
 	}
 
@@ -261,10 +261,14 @@ static int msm_config_group_get(struct pinctrl_dev *pctldev,
 		val = readl(pctrl->regs + g->io_reg);
 		arg = !!(val & BIT(g->in_bit));
 		break;
+	case PIN_CONFIG_INPUT_ENABLE:
+		/* Pin is output */
+		if (arg)
+			return -EINVAL;
+		arg = 1;
+		break;
 	default:
-		dev_err(pctrl->dev, "Unsupported config parameter: %x\n",
-			param);
-		return -EINVAL;
+		return -ENOTSUPP;
 	}
 
 	*config = pinconf_to_config_packed(param, arg);
@@ -333,6 +337,10 @@ static int msm_config_group_set(struct pinctrl_dev *pctldev,
 			/* enable output */
 			arg = 1;
 			break;
+		case PIN_CONFIG_INPUT_ENABLE:
+			/* disable output */
+			arg = 0;
+			break;
 		default:
 			dev_err(pctrl->dev, "Unsupported config parameter: %x\n",
 				param);
@@ -357,6 +365,7 @@ static int msm_config_group_set(struct pinctrl_dev *pctldev,
 }
 
 static const struct pinconf_ops msm_pinconf_ops = {
+	.is_generic		= true,
 	.pin_config_group_get	= msm_config_group_get,
 	.pin_config_group_set	= msm_config_group_set,
 };
diff --git a/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c b/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
index 0f11a26d932b..b2d22218a258 100644
--- a/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
+++ b/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
@@ -810,6 +810,7 @@ static int pmic_gpio_remove(struct platform_device *pdev)
 }
 
 static const struct of_device_id pmic_gpio_of_match[] = {
+	{ .compatible = "qcom,pm8916-gpio" },	/* 4 GPIO's */
 	{ .compatible = "qcom,pm8941-gpio" },	/* 36 GPIO's */
 	{ .compatible = "qcom,pma8084-gpio" },	/* 22 GPIO's */
 	{ },
diff --git a/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c b/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
index a8924dba335e..8f36c5f91949 100644
--- a/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
+++ b/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
@@ -925,6 +925,7 @@ static int pmic_mpp_remove(struct platform_device *pdev)
 
 static const struct of_device_id pmic_mpp_of_match[] = {
 	{ .compatible = "qcom,pm8841-mpp" },	/* 4 MPP's */
+	{ .compatible = "qcom,pm8916-mpp" },	/* 4 MPP's */
 	{ .compatible = "qcom,pm8941-mpp" },	/* 8 MPP's */
 	{ .compatible = "qcom,pma8084-mpp" },	/* 8 MPP's */
 	{ },
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos.c b/drivers/pinctrl/samsung/pinctrl-exynos.c
index c8f83f96546c..0b7afa50121a 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos.c
@@ -1240,6 +1240,159 @@ const struct samsung_pin_ctrl exynos5420_pin_ctrl[] __initconst = {
 	},
 };
 
+/* pin banks of exynos5433 pin-controller - ALIVE */
+static const struct samsung_pin_bank_data exynos5433_pin_banks0[] = {
+	EXYNOS_PIN_BANK_EINTW(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0x060, "gpa3", 0x0c),
+};
+
+/* pin banks of exynos5433 pin-controller - AUD */
+static const struct samsung_pin_bank_data exynos5433_pin_banks1[] = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(4, 0x020, "gpz1", 0x04),
+};
+
+/* pin banks of exynos5433 pin-controller - CPIF */
+static const struct samsung_pin_bank_data exynos5433_pin_banks2[] = {
+	EXYNOS_PIN_BANK_EINTG(2, 0x000, "gpv6", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - eSE */
+static const struct samsung_pin_bank_data exynos5433_pin_banks3[] = {
+	EXYNOS_PIN_BANK_EINTG(3, 0x000, "gpj2", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - FINGER */
+static const struct samsung_pin_bank_data exynos5433_pin_banks4[] = {
+	EXYNOS_PIN_BANK_EINTG(4, 0x000, "gpd5", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - FSYS */
+static const struct samsung_pin_bank_data exynos5433_pin_banks5[] = {
+	EXYNOS_PIN_BANK_EINTG(6, 0x000, "gph1", 0x00),
+	EXYNOS_PIN_BANK_EINTG(7, 0x020, "gpr4", 0x04),
+	EXYNOS_PIN_BANK_EINTG(5, 0x040, "gpr0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpr1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpr2", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0a0, "gpr3", 0x14),
+};
+
+/* pin banks of exynos5433 pin-controller - IMEM */
+static const struct samsung_pin_bank_data exynos5433_pin_banks6[] = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpf0", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - NFC */
+static const struct samsung_pin_bank_data exynos5433_pin_banks7[] = {
+	EXYNOS_PIN_BANK_EINTG(3, 0x000, "gpj0", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - PERIC */
+static const struct samsung_pin_bank_data exynos5433_pin_banks8[] = {
+	EXYNOS_PIN_BANK_EINTG(6, 0x000, "gpv7", 0x00),
+	EXYNOS_PIN_BANK_EINTG(5, 0x020, "gpb0", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpc0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(2, 0x060, "gpc1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(6, 0x080, "gpc2", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0a0, "gpc3", 0x14),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0c0, "gpg0", 0x18),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0e0, "gpd0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(6, 0x100, "gpd1", 0x20),
+	EXYNOS_PIN_BANK_EINTG(8, 0x120, "gpd2", 0x24),
+	EXYNOS_PIN_BANK_EINTG(5, 0x140, "gpd4", 0x28),
+	EXYNOS_PIN_BANK_EINTG(2, 0x160, "gpd8", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x180, "gpd6", 0x30),
+	EXYNOS_PIN_BANK_EINTG(3, 0x1a0, "gpd7", 0x34),
+	EXYNOS_PIN_BANK_EINTG(5, 0x1c0, "gpg1", 0x38),
+	EXYNOS_PIN_BANK_EINTG(2, 0x1e0, "gpg2", 0x3c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x200, "gpg3", 0x40),
+};
+
+/* pin banks of exynos5433 pin-controller - TOUCH */
+static const struct samsung_pin_bank_data exynos5433_pin_banks9[] = {
+	EXYNOS_PIN_BANK_EINTG(3, 0x000, "gpj1", 0x00),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5433 SoC. Exynos5433 SoC includes
+ * ten gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos5433_pin_ctrl[] = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5433_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks0),
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5433_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5433_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos5433_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 4 data */
+		.pin_banks	= exynos5433_pin_banks4,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks4),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 5 data */
+		.pin_banks	= exynos5433_pin_banks5,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks5),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 6 data */
+		.pin_banks	= exynos5433_pin_banks6,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks6),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 7 data */
+		.pin_banks	= exynos5433_pin_banks7,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks7),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 8 data */
+		.pin_banks	= exynos5433_pin_banks8,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks8),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 9 data */
+		.pin_banks	= exynos5433_pin_banks9,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks9),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	},
+};
+
 /* pin banks of exynos7 pin-controller - ALIVE */
 static const struct samsung_pin_bank_data exynos7_pin_banks0[] __initconst = {
 	EXYNOS_PIN_BANK_EINTW(8, 0x000, "gpa0", 0x00),
@@ -1324,7 +1477,6 @@ const struct samsung_pin_ctrl exynos7_pin_ctrl[] __initconst = {
 		/* pin-controller instance 0 Alive data */
 		.pin_banks	= exynos7_pin_banks0,
 		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
 		.eint_wkup_init = exynos_eint_wkup_init,
 	}, {
 		/* pin-controller instance 1 BUS0 data */
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.c b/drivers/pinctrl/samsung/pinctrl-samsung.c
index ec580af35856..ed165ba2eb2f 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.c
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.c
@@ -1239,6 +1239,8 @@ static const struct of_device_id samsung_pinctrl_dt_match[] = {
 		.data = (void *)exynos5260_pin_ctrl },
 	{ .compatible = "samsung,exynos5420-pinctrl",
 		.data = (void *)exynos5420_pin_ctrl },
+	{ .compatible = "samsung,exynos5433-pinctrl",
+		.data = (void *)exynos5433_pin_ctrl },
 	{ .compatible = "samsung,s5pv210-pinctrl",
 		.data = (void *)s5pv210_pin_ctrl },
 	{ .compatible = "samsung,exynos7-pinctrl",
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.h b/drivers/pinctrl/samsung/pinctrl-samsung.h
index 1b8c0139d604..c1239ff6157d 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.h
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.h
@@ -271,6 +271,7 @@ extern const struct samsung_pin_ctrl exynos4415_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos5250_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos5260_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos5420_pin_ctrl[];
+extern const struct samsung_pin_ctrl exynos5433_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos7_pin_ctrl[];
 extern const struct samsung_pin_ctrl s3c64xx_pin_ctrl[];
 extern const struct samsung_pin_ctrl s3c2412_pin_ctrl[];
diff --git a/drivers/pinctrl/sh-pfc/core.c b/drivers/pinctrl/sh-pfc/core.c
index a56280814a3f..7b2c9495c383 100644
--- a/drivers/pinctrl/sh-pfc/core.c
+++ b/drivers/pinctrl/sh-pfc/core.c
@@ -92,10 +92,10 @@ static int sh_pfc_map_resources(struct sh_pfc *pfc,
 	return 0;
 }
 
-static void __iomem *sh_pfc_phys_to_virt(struct sh_pfc *pfc,
-					 unsigned long address)
+static void __iomem *sh_pfc_phys_to_virt(struct sh_pfc *pfc, u32 reg)
 {
 	struct sh_pfc_window *window;
+	phys_addr_t address = reg;
 	unsigned int i;
 
 	/* scan through physical windows and convert address */
@@ -144,8 +144,7 @@ static int sh_pfc_enum_in_range(u16 enum_id, const struct pinmux_range *r)
 	return 1;
 }
 
-unsigned long sh_pfc_read_raw_reg(void __iomem *mapped_reg,
-				  unsigned long reg_width)
+u32 sh_pfc_read_raw_reg(void __iomem *mapped_reg, unsigned int reg_width)
 {
 	switch (reg_width) {
 	case 8:
@@ -160,8 +159,8 @@ unsigned long sh_pfc_read_raw_reg(void __iomem *mapped_reg,
 	return 0;
 }
 
-void sh_pfc_write_raw_reg(void __iomem *mapped_reg, unsigned long reg_width,
-			  unsigned long data)
+void sh_pfc_write_raw_reg(void __iomem *mapped_reg, unsigned int reg_width,
+			  u32 data)
 {
 	switch (reg_width) {
 	case 8:
@@ -180,10 +179,9 @@ void sh_pfc_write_raw_reg(void __iomem *mapped_reg, unsigned long reg_width,
 
 static void sh_pfc_config_reg_helper(struct sh_pfc *pfc,
 				     const struct pinmux_cfg_reg *crp,
-				     unsigned long in_pos,
-				     void __iomem **mapped_regp,
-				     unsigned long *maskp,
-				     unsigned long *posp)
+				     unsigned int in_pos,
+				     void __iomem **mapped_regp, u32 *maskp,
+				     unsigned int *posp)
 {
 	unsigned int k;
 
@@ -202,15 +200,16 @@ static void sh_pfc_config_reg_helper(struct sh_pfc *pfc,
 
 static void sh_pfc_write_config_reg(struct sh_pfc *pfc,
 				    const struct pinmux_cfg_reg *crp,
-				    unsigned long field, unsigned long value)
+				    unsigned int field, u32 value)
 {
 	void __iomem *mapped_reg;
-	unsigned long mask, pos, data;
+	unsigned int pos;
+	u32 mask, data;
 
 	sh_pfc_config_reg_helper(pfc, crp, field, &mapped_reg, &mask, &pos);
 
-	dev_dbg(pfc->dev, "write_reg addr = %lx, value = %ld, field = %ld, "
-		"r_width = %ld, f_width = %ld\n",
+	dev_dbg(pfc->dev, "write_reg addr = %x, value = 0x%x, field = %u, "
+		"r_width = %u, f_width = %u\n",
 		crp->reg, value, field, crp->reg_width, crp->field_width);
 
 	mask = ~(mask << pos);
@@ -229,26 +228,28 @@ static void sh_pfc_write_config_reg(struct sh_pfc *pfc,
 }
 
 static int sh_pfc_get_config_reg(struct sh_pfc *pfc, u16 enum_id,
-				 const struct pinmux_cfg_reg **crp, int *fieldp,
-				 int *valuep)
+				 const struct pinmux_cfg_reg **crp,
+				 unsigned int *fieldp, u32 *valuep)
 {
-	const struct pinmux_cfg_reg *config_reg;
-	unsigned long r_width, f_width, curr_width, ncomb;
-	unsigned int k, m, n, pos, bit_pos;
+	unsigned int k = 0;
 
-	k = 0;
 	while (1) {
-		config_reg = pfc->info->cfg_regs + k;
-
-		r_width = config_reg->reg_width;
-		f_width = config_reg->field_width;
+		const struct pinmux_cfg_reg *config_reg =
+			pfc->info->cfg_regs + k;
+		unsigned int r_width = config_reg->reg_width;
+		unsigned int f_width = config_reg->field_width;
+		unsigned int curr_width;
+		unsigned int bit_pos;
+		unsigned int pos = 0;
+		unsigned int m = 0;
 
 		if (!r_width)
 			break;
 
-		pos = 0;
-		m = 0;
 		for (bit_pos = 0; bit_pos < r_width; bit_pos += curr_width) {
+			u32 ncomb;
+			u32 n;
+
 			if (f_width)
 				curr_width = f_width;
 			else
@@ -297,11 +298,8 @@ static int sh_pfc_mark_to_enum(struct sh_pfc *pfc, u16 mark, int pos,
 
 int sh_pfc_config_mux(struct sh_pfc *pfc, unsigned mark, int pinmux_type)
 {
-	const struct pinmux_cfg_reg *cr = NULL;
-	u16 enum_id;
 	const struct pinmux_range *range;
-	int in_range, pos, field, value;
-	int ret;
+	int pos = 0;
 
 	switch (pinmux_type) {
 	case PINMUX_TYPE_GPIO:
@@ -321,13 +319,15 @@ int sh_pfc_config_mux(struct sh_pfc *pfc, unsigned mark, int pinmux_type)
 		return -EINVAL;
 	}
 
-	pos = 0;
-	enum_id = 0;
-	field = 0;
-	value = 0;
-
 	/* Iterate over all the configuration fields we need to update. */
 	while (1) {
+		const struct pinmux_cfg_reg *cr;
+		unsigned int field;
+		u16 enum_id;
+		u32 value;
+		int in_range;
+		int ret;
+
 		pos = sh_pfc_mark_to_enum(pfc, mark, pos, &enum_id);
 		if (pos < 0)
 			return pos;
@@ -579,9 +579,6 @@ static int sh_pfc_remove(struct platform_device *pdev)
 }
 
 static const struct platform_device_id sh_pfc_id_table[] = {
-#ifdef CONFIG_PINCTRL_PFC_EMEV2
-	{ "pfc-emev2", (kernel_ulong_t)&emev2_pinmux_info },
-#endif
 #ifdef CONFIG_PINCTRL_PFC_R8A73A4
 	{ "pfc-r8a73a4", (kernel_ulong_t)&r8a73a4_pinmux_info },
 #endif
@@ -594,12 +591,6 @@ static const struct platform_device_id sh_pfc_id_table[] = {
 #ifdef CONFIG_PINCTRL_PFC_R8A7779
 	{ "pfc-r8a7779", (kernel_ulong_t)&r8a7779_pinmux_info },
 #endif
-#ifdef CONFIG_PINCTRL_PFC_R8A7790
-	{ "pfc-r8a7790", (kernel_ulong_t)&r8a7790_pinmux_info },
-#endif
-#ifdef CONFIG_PINCTRL_PFC_R8A7791
-	{ "pfc-r8a7791", (kernel_ulong_t)&r8a7791_pinmux_info },
-#endif
 #ifdef CONFIG_PINCTRL_PFC_SH7203
 	{ "pfc-sh7203", (kernel_ulong_t)&sh7203_pinmux_info },
 #endif
diff --git a/drivers/pinctrl/sh-pfc/core.h b/drivers/pinctrl/sh-pfc/core.h
index 6b59d63b9c01..6dc8a6fc2746 100644
--- a/drivers/pinctrl/sh-pfc/core.h
+++ b/drivers/pinctrl/sh-pfc/core.h
@@ -57,10 +57,9 @@ int sh_pfc_unregister_gpiochip(struct sh_pfc *pfc);
 int sh_pfc_register_pinctrl(struct sh_pfc *pfc);
 int sh_pfc_unregister_pinctrl(struct sh_pfc *pfc);
 
-unsigned long sh_pfc_read_raw_reg(void __iomem *mapped_reg,
-				  unsigned long reg_width);
-void sh_pfc_write_raw_reg(void __iomem *mapped_reg, unsigned long reg_width,
-			  unsigned long data);
+u32 sh_pfc_read_raw_reg(void __iomem *mapped_reg, unsigned int reg_width);
+void sh_pfc_write_raw_reg(void __iomem *mapped_reg, unsigned int reg_width,
+			  u32 data);
 
 int sh_pfc_get_pin_index(struct sh_pfc *pfc, unsigned int pin);
 int sh_pfc_config_mux(struct sh_pfc *pfc, unsigned mark, int pinmux_type);
diff --git a/drivers/pinctrl/sh-pfc/gpio.c b/drivers/pinctrl/sh-pfc/gpio.c
index 80f641ee4dea..ba353735ecf2 100644
--- a/drivers/pinctrl/sh-pfc/gpio.c
+++ b/drivers/pinctrl/sh-pfc/gpio.c
@@ -21,7 +21,7 @@
 
 struct sh_pfc_gpio_data_reg {
 	const struct pinmux_data_reg *info;
-	unsigned long shadow;
+	u32 shadow;
 };
 
 struct sh_pfc_gpio_pin {
@@ -59,19 +59,20 @@ static void gpio_get_data_reg(struct sh_pfc_chip *chip, unsigned int offset,
 	*bit = gpio_pin->dbit;
 }
 
-static unsigned long gpio_read_data_reg(struct sh_pfc_chip *chip,
-					const struct pinmux_data_reg *dreg)
+static u32 gpio_read_data_reg(struct sh_pfc_chip *chip,
+			      const struct pinmux_data_reg *dreg)
 {
-	void __iomem *mem = dreg->reg - chip->mem->phys + chip->mem->virt;
+	phys_addr_t address = dreg->reg;
+	void __iomem *mem = address - chip->mem->phys + chip->mem->virt;
 
 	return sh_pfc_read_raw_reg(mem, dreg->reg_width);
 }
 
 static void gpio_write_data_reg(struct sh_pfc_chip *chip,
-				const struct pinmux_data_reg *dreg,
-				unsigned long value)
+				const struct pinmux_data_reg *dreg, u32 value)
 {
-	void __iomem *mem = dreg->reg - chip->mem->phys + chip->mem->virt;
+	phys_addr_t address = dreg->reg;
+	void __iomem *mem = address - chip->mem->phys + chip->mem->virt;
 
 	sh_pfc_write_raw_reg(mem, dreg->reg_width, value);
 }
@@ -85,7 +86,7 @@ static void gpio_setup_data_reg(struct sh_pfc_chip *chip, unsigned idx)
 	unsigned int bit;
 	unsigned int i;
 
-	for (i = 0, dreg = pfc->info->data_regs; dreg->reg; ++i, ++dreg) {
+	for (i = 0, dreg = pfc->info->data_regs; dreg->reg_width; ++i, ++dreg) {
 		for (bit = 0; bit < dreg->reg_width; bit++) {
 			if (dreg->enum_ids[bit] == pin->enum_id) {
 				gpio_pin->dreg = i;
@@ -154,17 +155,17 @@ static void gpio_pin_set_value(struct sh_pfc_chip *chip, unsigned offset,
 			       int value)
 {
 	struct sh_pfc_gpio_data_reg *reg;
-	unsigned long pos;
 	unsigned int bit;
+	unsigned int pos;
 
 	gpio_get_data_reg(chip, offset, &reg, &bit);
 
 	pos = reg->info->reg_width - (bit + 1);
 
 	if (value)
-		set_bit(pos, &reg->shadow);
+		reg->shadow |= BIT(pos);
 	else
-		clear_bit(pos, &reg->shadow);
+		reg->shadow &= ~BIT(pos);
 
 	gpio_write_data_reg(chip, reg->info, reg->shadow);
 }
@@ -186,8 +187,8 @@ static int gpio_pin_get(struct gpio_chip *gc, unsigned offset)
 {
 	struct sh_pfc_chip *chip = gpio_to_pfc_chip(gc);
 	struct sh_pfc_gpio_data_reg *reg;
-	unsigned long pos;
 	unsigned int bit;
+	unsigned int pos;
 
 	gpio_get_data_reg(chip, offset, &reg, &bit);
 
@@ -341,6 +342,7 @@ sh_pfc_add_gpiochip(struct sh_pfc *pfc, int(*setup)(struct sh_pfc_chip *),
 int sh_pfc_register_gpiochip(struct sh_pfc *pfc)
 {
 	struct sh_pfc_chip *chip;
+	phys_addr_t address;
 	unsigned int i;
 	int ret;
 
@@ -352,11 +354,12 @@ int sh_pfc_register_gpiochip(struct sh_pfc *pfc)
 	 * that covers the data registers. In that case don't try to handle
 	 * GPIOs.
 	 */
+	address = pfc->info->data_regs[0].reg;
 	for (i = 0; i < pfc->num_windows; ++i) {
 		struct sh_pfc_window *window = &pfc->windows[i];
 
-		if (pfc->info->data_regs[0].reg >= window->phys &&
-		    pfc->info->data_regs[0].reg < window->phys + window->size)
+		if (address >= window->phys &&
+		    address < window->phys + window->size)
 			break;
 	}
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
index 80c1843bb6ad..22a5470889f5 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
@@ -1799,6 +1799,81 @@ static const unsigned int audio_clkout_d_pins[] = {
 static const unsigned int audio_clkout_d_mux[] = {
 	AUDIO_CLKOUT_D_MARK,
 };
+/* - AVB -------------------------------------------------------------------- */
+static const unsigned int avb_link_pins[] = {
+	RCAR_GP_PIN(3, 11),
+};
+static const unsigned int avb_link_mux[] = {
+	AVB_LINK_MARK,
+};
+static const unsigned int avb_magic_pins[] = {
+	RCAR_GP_PIN(2, 14),
+};
+static const unsigned int avb_magic_mux[] = {
+	AVB_MAGIC_MARK,
+};
+static const unsigned int avb_phy_int_pins[] = {
+	RCAR_GP_PIN(2, 15),
+};
+static const unsigned int avb_phy_int_mux[] = {
+	AVB_PHY_INT_MARK,
+};
+static const unsigned int avb_mdio_pins[] = {
+	RCAR_GP_PIN(2, 11), RCAR_GP_PIN(2, 12),
+};
+static const unsigned int avb_mdio_mux[] = {
+	AVB_MDC_MARK, AVB_MDIO_MARK,
+};
+static const unsigned int avb_mii_pins[] = {
+	RCAR_GP_PIN(0, 8), RCAR_GP_PIN(0, 9), RCAR_GP_PIN(0, 10),
+	RCAR_GP_PIN(0, 11),
+
+	RCAR_GP_PIN(3, 13), RCAR_GP_PIN(2, 0), RCAR_GP_PIN(2, 1),
+	RCAR_GP_PIN(2, 2),
+
+	RCAR_GP_PIN(2, 7), RCAR_GP_PIN(2, 8), RCAR_GP_PIN(2, 9),
+	RCAR_GP_PIN(2, 10), RCAR_GP_PIN(3, 8), RCAR_GP_PIN(3, 10),
+	RCAR_GP_PIN(3, 12),
+};
+static const unsigned int avb_mii_mux[] = {
+	AVB_TXD0_MARK, AVB_TXD1_MARK, AVB_TXD2_MARK,
+	AVB_TXD3_MARK,
+
+	AVB_RXD0_MARK, AVB_RXD1_MARK, AVB_RXD2_MARK,
+	AVB_RXD3_MARK,
+
+	AVB_RX_ER_MARK, AVB_RX_CLK_MARK, AVB_RX_DV_MARK,
+	AVB_CRS_MARK, AVB_TX_EN_MARK, AVB_TX_CLK_MARK,
+	AVB_COL_MARK,
+};
+static const unsigned int avb_gmii_pins[] = {
+	RCAR_GP_PIN(0, 8), RCAR_GP_PIN(0, 9), RCAR_GP_PIN(0, 10),
+	RCAR_GP_PIN(0, 11), RCAR_GP_PIN(0, 12), RCAR_GP_PIN(0, 13),
+	RCAR_GP_PIN(0, 14), RCAR_GP_PIN(0, 15),
+
+	RCAR_GP_PIN(3, 13), RCAR_GP_PIN(2, 0), RCAR_GP_PIN(2, 1),
+	RCAR_GP_PIN(2, 2), RCAR_GP_PIN(2, 3), RCAR_GP_PIN(2, 4),
+	RCAR_GP_PIN(2, 5), RCAR_GP_PIN(2, 6),
+
+	RCAR_GP_PIN(2, 7), RCAR_GP_PIN(2, 8), RCAR_GP_PIN(2, 9),
+	RCAR_GP_PIN(2, 10), RCAR_GP_PIN(2, 13), RCAR_GP_PIN(2, 16),
+	RCAR_GP_PIN(3, 8), RCAR_GP_PIN(3, 9), RCAR_GP_PIN(3, 10),
+	RCAR_GP_PIN(3, 12),
+};
+static const unsigned int avb_gmii_mux[] = {
+	AVB_TXD0_MARK, AVB_TXD1_MARK, AVB_TXD2_MARK,
+	AVB_TXD3_MARK, AVB_TXD4_MARK, AVB_TXD5_MARK,
+	AVB_TXD6_MARK, AVB_TXD7_MARK,
+
+	AVB_RXD0_MARK, AVB_RXD1_MARK, AVB_RXD2_MARK,
+	AVB_RXD3_MARK, AVB_RXD4_MARK, AVB_RXD5_MARK,
+	AVB_RXD6_MARK, AVB_RXD7_MARK,
+
+	AVB_RX_ER_MARK, AVB_RX_CLK_MARK, AVB_RX_DV_MARK,
+	AVB_CRS_MARK, AVB_GTX_CLK_MARK, AVB_GTXREFCLK_MARK,
+	AVB_TX_EN_MARK, AVB_TX_ER_MARK, AVB_TX_CLK_MARK,
+	AVB_COL_MARK,
+};
 /* - DU RGB ----------------------------------------------------------------- */
 static const unsigned int du_rgb666_pins[] = {
 	/* R[7:2], G[7:2], B[7:2] */
@@ -3823,6 +3898,12 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(audio_clkout_b),
 	SH_PFC_PIN_GROUP(audio_clkout_c),
 	SH_PFC_PIN_GROUP(audio_clkout_d),
+	SH_PFC_PIN_GROUP(avb_link),
+	SH_PFC_PIN_GROUP(avb_magic),
+	SH_PFC_PIN_GROUP(avb_phy_int),
+	SH_PFC_PIN_GROUP(avb_mdio),
+	SH_PFC_PIN_GROUP(avb_mii),
+	SH_PFC_PIN_GROUP(avb_gmii),
 	SH_PFC_PIN_GROUP(du_rgb666),
 	SH_PFC_PIN_GROUP(du_rgb888),
 	SH_PFC_PIN_GROUP(du_clk_out_0),
@@ -4101,6 +4182,15 @@ static const char * const audio_clk_groups[] = {
 	"audio_clkout_d",
 };
 
+static const char * const avb_groups[] = {
+	"avb_link",
+	"avb_magic",
+	"avb_phy_int",
+	"avb_mdio",
+	"avb_mii",
+	"avb_gmii",
+};
+
 static const char * const du_groups[] = {
 	"du_rgb666",
 	"du_rgb888",
@@ -4507,6 +4597,7 @@ static const char * const vin3_groups[] = {
 
 static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(audio_clk),
+	SH_PFC_FUNCTION(avb),
 	SH_PFC_FUNCTION(du),
 	SH_PFC_FUNCTION(du0),
 	SH_PFC_FUNCTION(du1),
diff --git a/drivers/pinctrl/sh-pfc/sh_pfc.h b/drivers/pinctrl/sh-pfc/sh_pfc.h
index c83728626906..c7508d5f6886 100644
--- a/drivers/pinctrl/sh-pfc/sh_pfc.h
+++ b/drivers/pinctrl/sh-pfc/sh_pfc.h
@@ -69,9 +69,10 @@ struct pinmux_func {
 };
 
 struct pinmux_cfg_reg {
-	unsigned long reg, reg_width, field_width;
+	u32 reg;
+	u8 reg_width, field_width;
 	const u16 *enum_ids;
-	const unsigned long *var_field_width;
+	const u8 *var_field_width;
 };
 
 #define PINMUX_CFG_REG(name, r, r_width, f_width) \
@@ -80,12 +81,13 @@ struct pinmux_cfg_reg {
 
 #define PINMUX_CFG_REG_VAR(name, r, r_width, var_fw0, var_fwn...) \
 	.reg = r, .reg_width = r_width,	\
-	.var_field_width = (const unsigned long [r_width]) \
+	.var_field_width = (const u8 [r_width]) \
 		{ var_fw0, var_fwn, 0 }, \
 	.enum_ids = (const u16 [])
 
 struct pinmux_data_reg {
-	unsigned long reg, reg_width;
+	u32 reg;
+	u8 reg_width;
 	const u16 *enum_ids;
 };
 
@@ -148,7 +150,7 @@ struct sh_pfc_soc_info {
 	const struct pinmux_irq *gpio_irq;
 	unsigned int gpio_irq_size;
 
-	unsigned long unlock_reg;
+	u32 unlock_reg;
 };
 
 /* -----------------------------------------------------------------------------
@@ -302,20 +304,21 @@ struct sh_pfc_soc_info {
 /*
  * PORTnCR macro
  */
-#define _PCRH(in, in_pd, in_pu, out)	\
-	0, (out), (in), 0,		\
-	0, 0, 0, 0,			\
-	0, 0, (in_pd), 0,		\
-	0, 0, (in_pu), 0
-
 #define PORTCR(nr, reg)							\
 	{								\
-		PINMUX_CFG_REG("PORT" nr "CR", reg, 8, 4) {		\
-			_PCRH(PORT##nr##_IN, 0, 0, PORT##nr##_OUT),	\
-				PORT##nr##_FN0, PORT##nr##_FN1,		\
-				PORT##nr##_FN2, PORT##nr##_FN3,		\
-				PORT##nr##_FN4, PORT##nr##_FN5,		\
-				PORT##nr##_FN6, PORT##nr##_FN7 }	\
+		PINMUX_CFG_REG_VAR("PORT" nr "CR", reg, 8, 2, 2, 1, 3) {\
+			/* PULMD[1:0], handled by .set_bias() */	\
+			0, 0, 0, 0,					\
+			/* IE and OE */					\
+			0, PORT##nr##_OUT, PORT##nr##_IN, 0,		\
+			/* SEC, not supported */			\
+			0, 0,						\
+			/* PTMD[2:0] */					\
+			PORT##nr##_FN0, PORT##nr##_FN1,			\
+			PORT##nr##_FN2, PORT##nr##_FN3,			\
+			PORT##nr##_FN4, PORT##nr##_FN5,			\
+			PORT##nr##_FN6, PORT##nr##_FN7			\
+		}							\
 	}
 
 #endif /* __SH_PFC_H */
diff --git a/drivers/pinctrl/sirf/pinctrl-sirf.c b/drivers/pinctrl/sirf/pinctrl-sirf.c
index 2a1f07249b2f..e2efbbae4061 100644
--- a/drivers/pinctrl/sirf/pinctrl-sirf.c
+++ b/drivers/pinctrl/sirf/pinctrl-sirf.c
@@ -39,10 +39,9 @@ struct sirfsoc_gpio_bank {
 struct sirfsoc_gpio_chip {
 	struct of_mm_gpio_chip chip;
 	struct sirfsoc_gpio_bank sgpio_bank[SIRFSOC_GPIO_NO_OF_BANKS];
+	spinlock_t lock;
 };
 
-static DEFINE_SPINLOCK(sgpio_lock);
-
 static struct sirfsoc_pin_group *sirfsoc_pin_groups;
 static int sirfsoc_pingrp_cnt;
 
@@ -427,13 +426,13 @@ static void sirfsoc_gpio_irq_ack(struct irq_data *d)
 
 	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
 
-	spin_lock_irqsave(&sgpio_lock, flags);
+	spin_lock_irqsave(&sgpio->lock, flags);
 
 	val = readl(sgpio->chip.regs + offset);
 
 	writel(val, sgpio->chip.regs + offset);
 
-	spin_unlock_irqrestore(&sgpio_lock, flags);
+	spin_unlock_irqrestore(&sgpio->lock, flags);
 }
 
 static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_chip *sgpio,
@@ -445,14 +444,14 @@ static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_chip *sgpio,
 
 	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
 
-	spin_lock_irqsave(&sgpio_lock, flags);
+	spin_lock_irqsave(&sgpio->lock, flags);
 
 	val = readl(sgpio->chip.regs + offset);
 	val &= ~SIRFSOC_GPIO_CTL_INTR_EN_MASK;
 	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
 	writel(val, sgpio->chip.regs + offset);
 
-	spin_unlock_irqrestore(&sgpio_lock, flags);
+	spin_unlock_irqrestore(&sgpio->lock, flags);
 }
 
 static void sirfsoc_gpio_irq_mask(struct irq_data *d)
@@ -475,14 +474,14 @@ static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
 
 	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
 
-	spin_lock_irqsave(&sgpio_lock, flags);
+	spin_lock_irqsave(&sgpio->lock, flags);
 
 	val = readl(sgpio->chip.regs + offset);
 	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
 	val |= SIRFSOC_GPIO_CTL_INTR_EN_MASK;
 	writel(val, sgpio->chip.regs + offset);
 
-	spin_unlock_irqrestore(&sgpio_lock, flags);
+	spin_unlock_irqrestore(&sgpio->lock, flags);
 }
 
 static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
@@ -496,7 +495,7 @@ static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 
 	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
 
-	spin_lock_irqsave(&sgpio_lock, flags);
+	spin_lock_irqsave(&sgpio->lock, flags);
 
 	val = readl(sgpio->chip.regs + offset);
 	val &= ~(SIRFSOC_GPIO_CTL_INTR_STS_MASK | SIRFSOC_GPIO_CTL_OUT_EN_MASK);
@@ -533,7 +532,7 @@ static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 
 	writel(val, sgpio->chip.regs + offset);
 
-	spin_unlock_irqrestore(&sgpio_lock, flags);
+	spin_unlock_irqrestore(&sgpio->lock, flags);
 
 	return 0;
 }
@@ -568,7 +567,7 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 	status = readl(sgpio->chip.regs + SIRFSOC_GPIO_INT_STATUS(bank->id));
 	if (!status) {
 		printk(KERN_WARNING
-			"%s: gpio id %d status %#x no interrupt is flaged\n",
+			"%s: gpio id %d status %#x no interrupt is flagged\n",
 			__func__, bank->id, status);
 		handle_bad_irq(irq, desc);
 		return;
@@ -697,11 +696,11 @@ static int sirfsoc_gpio_direction_output(struct gpio_chip *chip,
 
 	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
 
-	spin_lock_irqsave(&sgpio_lock, flags);
+	spin_lock_irqsave(&sgpio->lock, flags);
 
 	sirfsoc_gpio_set_output(sgpio, bank, offset, value);
 
-	spin_unlock_irqrestore(&sgpio_lock, flags);
+	spin_unlock_irqrestore(&sgpio->lock, flags);
 
 	return 0;
 }
@@ -793,6 +792,7 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 	sgpio = devm_kzalloc(&pdev->dev, sizeof(*sgpio), GFP_KERNEL);
 	if (!sgpio)
 		return -ENOMEM;
+	spin_lock_init(&sgpio->lock);
 
 	regs = of_iomap(np, 0);
 	if (!regs)
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
index 24c5d88f943f..7376a97b5e65 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)
@@ -1019,7 +1020,7 @@ static int sun4i_a10_pinctrl_probe(struct platform_device *pdev)
 				  &sun4i_a10_pinctrl_data);
 }
 
-static struct of_device_id sun4i_a10_pinctrl_match[] = {
+static const struct of_device_id sun4i_a10_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun4i-a10-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
index 45a351affa59..63676617bc59 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
@@ -670,7 +670,7 @@ static int sun5i_a10s_pinctrl_probe(struct platform_device *pdev)
 				  &sun5i_a10s_pinctrl_data);
 }
 
-static struct of_device_id sun5i_a10s_pinctrl_match[] = {
+static const struct of_device_id sun5i_a10s_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun5i-a10s-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
index 4bd23471412c..2bb07b38834f 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
@@ -388,7 +388,7 @@ static int sun5i_a13_pinctrl_probe(struct platform_device *pdev)
 				  &sun5i_a13_pinctrl_data);
 }
 
-static struct of_device_id sun5i_a13_pinctrl_match[] = {
+static const struct of_device_id sun5i_a13_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun5i-a13-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
index 02174fa57997..9596b0a3df6b 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
@@ -120,7 +120,7 @@ static int sun6i_a31_r_pinctrl_probe(struct platform_device *pdev)
 	return ret;
 }
 
-static struct of_device_id sun6i_a31_r_pinctrl_match[] = {
+static const struct of_device_id sun6i_a31_r_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun6i-a31-r-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
index 18038f0d6b52..022863ab0c58 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
@@ -922,7 +922,7 @@ static int sun6i_a31_pinctrl_probe(struct platform_device *pdev)
 				  &sun6i_a31_pinctrl_data);
 }
 
-static struct of_device_id sun6i_a31_pinctrl_match[] = {
+static const struct of_device_id sun6i_a31_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun6i-a31-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c
index 9b5a91f610c7..d3725dcd6979 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c
@@ -794,7 +794,7 @@ static int sun6i_a31s_pinctrl_probe(struct platform_device *pdev)
 				  &sun6i_a31s_pinctrl_data);
 }
 
-static struct of_device_id sun6i_a31s_pinctrl_match[] = {
+static const struct of_device_id sun6i_a31s_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun6i-a31s-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
index 6af6cc8547b0..cf1ce0c02600 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
@@ -1045,7 +1045,7 @@ static int sun7i_a20_pinctrl_probe(struct platform_device *pdev)
 				  &sun7i_a20_pinctrl_data);
 }
 
-static struct of_device_id sun7i_a20_pinctrl_match[] = {
+static const struct of_device_id sun7i_a20_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun7i-a20-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c
index 327e03ff7c4d..056287635873 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23-r.c
@@ -119,7 +119,7 @@ static int sun8i_a23_r_pinctrl_probe(struct platform_device *pdev)
 	return ret;
 }
 
-static struct of_device_id sun8i_a23_r_pinctrl_match[] = {
+static const struct of_device_id sun8i_a23_r_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun8i-a23-r-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c
index 62695c9a92c2..55083d278bb1 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-a23.c
@@ -571,7 +571,7 @@ static int sun8i_a23_pinctrl_probe(struct platform_device *pdev)
 				  &sun8i_a23_pinctrl_data);
 }
 
-static struct of_device_id sun8i_a23_pinctrl_match[] = {
+static const struct of_device_id sun8i_a23_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun8i-a23-pinctrl", },
 	{}
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun9i-a80.c b/drivers/pinctrl/sunxi/pinctrl-sun9i-a80.c
index adb29422efc9..1b580ba76453 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun9i-a80.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun9i-a80.c
@@ -729,7 +729,7 @@ static int sun9i_a80_pinctrl_probe(struct platform_device *pdev)
 				  &sun9i_a80_pinctrl_data);
 }
 
-static struct of_device_id sun9i_a80_pinctrl_match[] = {
+static const struct of_device_id sun9i_a80_pinctrl_match[] = {
 	{ .compatible = "allwinner,sun9i-a80-pinctrl", },
 	{}
 };
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/pinctrl/vt8500/pinctrl-vt8500.c b/drivers/pinctrl/vt8500/pinctrl-vt8500.c
index cf8bbc946ff7..ca946b3dbdb4 100644
--- a/drivers/pinctrl/vt8500/pinctrl-vt8500.c
+++ b/drivers/pinctrl/vt8500/pinctrl-vt8500.c
@@ -478,7 +478,7 @@ static int vt8500_pinctrl_remove(struct platform_device *pdev)
 	return wmt_pinctrl_remove(pdev);
 }
 
-static struct of_device_id wmt_pinctrl_of_match[] = {
+static const struct of_device_id wmt_pinctrl_of_match[] = {
 	{ .compatible = "via,vt8500-pinctrl" },
 	{ /* sentinel */ },
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8505.c b/drivers/pinctrl/vt8500/pinctrl-wm8505.c
index 3f9c32dcb3d0..626fc7ec0174 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8505.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8505.c
@@ -509,7 +509,7 @@ static int wm8505_pinctrl_remove(struct platform_device *pdev)
 	return wmt_pinctrl_remove(pdev);
 }
 
-static struct of_device_id wmt_pinctrl_of_match[] = {
+static const struct of_device_id wmt_pinctrl_of_match[] = {
 	{ .compatible = "wm,wm8505-pinctrl" },
 	{ /* sentinel */ },
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8650.c b/drivers/pinctrl/vt8500/pinctrl-wm8650.c
index 4e80f98c2ba7..8953aba8bfc2 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8650.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8650.c
@@ -347,7 +347,7 @@ static int wm8650_pinctrl_remove(struct platform_device *pdev)
 	return wmt_pinctrl_remove(pdev);
 }
 
-static struct of_device_id wmt_pinctrl_of_match[] = {
+static const struct of_device_id wmt_pinctrl_of_match[] = {
 	{ .compatible = "wm,wm8650-pinctrl" },
 	{ /* sentinel */ },
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8750.c b/drivers/pinctrl/vt8500/pinctrl-wm8750.c
index 47b52a7cacac..c79053d430db 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8750.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8750.c
@@ -386,7 +386,7 @@ static int wm8750_pinctrl_remove(struct platform_device *pdev)
 	return wmt_pinctrl_remove(pdev);
 }
 
-static struct of_device_id wmt_pinctrl_of_match[] = {
+static const struct of_device_id wmt_pinctrl_of_match[] = {
 	{ .compatible = "wm,wm8750-pinctrl" },
 	{ /* sentinel */ },
 };
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8850.c b/drivers/pinctrl/vt8500/pinctrl-wm8850.c
index 8bbb38c931f6..f232b163c735 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wm8850.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8850.c
@@ -365,7 +365,7 @@ static int wm8850_pinctrl_remove(struct platform_device *pdev)
 	return wmt_pinctrl_remove(pdev);
 }
 
-static struct of_device_id wmt_pinctrl_of_match[] = {
+static const struct of_device_id wmt_pinctrl_of_match[] = {
 	{ .compatible = "wm,wm8850-pinctrl" },
 	{ /* sentinel */ },
 };
diff --git a/drivers/platform/x86/apple-gmux.c b/drivers/platform/x86/apple-gmux.c
index b9429fbf1cd8..66d6d22c239c 100644
--- a/drivers/platform/x86/apple-gmux.c
+++ b/drivers/platform/x86/apple-gmux.c
@@ -624,19 +624,7 @@ static struct pnp_driver gmux_pnp_driver = {
 	},
 };
 
-static int __init apple_gmux_init(void)
-{
-	return pnp_register_driver(&gmux_pnp_driver);
-}
-
-static void __exit apple_gmux_exit(void)
-{
-	pnp_unregister_driver(&gmux_pnp_driver);
-}
-
-module_init(apple_gmux_init);
-module_exit(apple_gmux_exit);
-
+module_pnp_driver(gmux_pnp_driver);
 MODULE_AUTHOR("Seth Forshee <seth.forshee@canonical.com>");
 MODULE_DESCRIPTION("Apple Gmux Driver");
 MODULE_LICENSE("GPL");
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/platform/x86/fujitsu-tablet.c b/drivers/platform/x86/fujitsu-tablet.c
index 53bdbb01bd3f..baea077a02cc 100644
--- a/drivers/platform/x86/fujitsu-tablet.c
+++ b/drivers/platform/x86/fujitsu-tablet.c
@@ -59,7 +59,7 @@ static unsigned short keymap_Lifebook_Tseries[KEYMAP_LEN] __initdata = {
 	KEY_RESERVED,
 	KEY_SCROLLDOWN,
 	KEY_SCROLLUP,
-	KEY_DIRECTION,
+	KEY_ROTATE_DISPLAY,
 	KEY_LEFTCTRL,
 	KEY_BRIGHTNESSUP,
 	KEY_BRIGHTNESSDOWN,
@@ -116,7 +116,7 @@ static unsigned short keymap_Lifebook_U810[KEYMAP_LEN] __initdata = {
 	KEY_RESERVED,
 	KEY_PROG1,
 	KEY_PROG2,
-	KEY_DIRECTION,
+	KEY_ROTATE_DISPLAY,
 	KEY_RESERVED,
 	KEY_RESERVED,
 	KEY_RESERVED,
@@ -153,7 +153,7 @@ static unsigned short keymap_Stylistic_ST5xxx[KEYMAP_LEN] __initdata = {
 	KEY_RESERVED,
 	KEY_RESERVED,
 	KEY_MAIL,
-	KEY_DIRECTION,
+	KEY_ROTATE_DISPLAY,
 	KEY_ESC,
 	KEY_ENTER,
 	KEY_BRIGHTNESSUP,
diff --git a/drivers/platform/x86/hp-wmi.c b/drivers/platform/x86/hp-wmi.c
index 0ab2b377a778..06697315a088 100644
--- a/drivers/platform/x86/hp-wmi.c
+++ b/drivers/platform/x86/hp-wmi.c
@@ -144,7 +144,7 @@ static const struct key_entry hp_wmi_keymap[] = {
 	{ KE_KEY, 0x20e8, { KEY_MEDIA } },
 	{ KE_KEY, 0x2142, { KEY_MEDIA } },
 	{ KE_KEY, 0x213b, { KEY_INFO } },
-	{ KE_KEY, 0x2169, { KEY_DIRECTION } },
+	{ KE_KEY, 0x2169, { KEY_ROTATE_DISPLAY } },
 	{ KE_KEY, 0x216a, { KEY_SETUP } },
 	{ KE_KEY, 0x231b, { KEY_HELP } },
 	{ KE_END, 0 }
diff --git a/drivers/pnp/base.h b/drivers/pnp/base.h
index c8873b0ca551..3151fd164614 100644
--- a/drivers/pnp/base.h
+++ b/drivers/pnp/base.h
@@ -3,7 +3,7 @@
  *	Bjorn Helgaas <bjorn.helgaas@hp.com>
  */
 
-extern spinlock_t pnp_lock;
+extern struct mutex pnp_lock;
 extern const struct attribute_group *pnp_dev_groups[];
 void *pnp_alloc(long size);
 
diff --git a/drivers/pnp/card.c b/drivers/pnp/card.c
index 874c236ac1a7..31ad9fc3f701 100644
--- a/drivers/pnp/card.c
+++ b/drivers/pnp/card.c
@@ -5,6 +5,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/ctype.h>
 #include <linux/slab.h>
 #include <linux/pnp.h>
@@ -244,10 +245,10 @@ int pnp_add_card(struct pnp_card *card)
 	}
 
 	pnp_interface_attach_card(card);
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	list_add_tail(&card->global_list, &pnp_cards);
 	list_add_tail(&card->protocol_list, &card->protocol->cards);
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 
 	/* we wait until now to add devices in order to ensure the drivers
 	 * will be able to use all of the related devices on the card
@@ -276,10 +277,10 @@ void pnp_remove_card(struct pnp_card *card)
 	struct list_head *pos, *temp;
 
 	device_unregister(&card->dev);
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	list_del(&card->global_list);
 	list_del(&card->protocol_list);
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 	list_for_each_safe(pos, temp, &card->devices) {
 		struct pnp_dev *dev = card_to_pnp_dev(pos);
 		pnp_remove_card_device(dev);
@@ -297,10 +298,10 @@ int pnp_add_card_device(struct pnp_card *card, struct pnp_dev *dev)
 	dev->card_link = NULL;
 	dev_set_name(&dev->dev, "%02x:%02x.%02x",
 		     dev->protocol->number, card->number, dev->number);
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	dev->card = card;
 	list_add_tail(&dev->card_list, &card->devices);
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 	return 0;
 }
 
@@ -310,10 +311,10 @@ int pnp_add_card_device(struct pnp_card *card, struct pnp_dev *dev)
  */
 void pnp_remove_card_device(struct pnp_dev *dev)
 {
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	dev->card = NULL;
 	list_del(&dev->card_list);
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 	__pnp_remove_device(dev);
 }
 
@@ -426,9 +427,9 @@ int pnp_register_card_driver(struct pnp_card_driver *drv)
 	if (error < 0)
 		return error;
 
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	list_add_tail(&drv->global_list, &pnp_card_drivers);
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 
 	list_for_each_safe(pos, temp, &pnp_cards) {
 		struct pnp_card *card =
@@ -444,9 +445,9 @@ int pnp_register_card_driver(struct pnp_card_driver *drv)
  */
 void pnp_unregister_card_driver(struct pnp_card_driver *drv)
 {
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	list_del(&drv->global_list);
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 	pnp_unregister_driver(&drv->link);
 }
 
diff --git a/drivers/pnp/core.c b/drivers/pnp/core.c
index cb6ce42f8e77..b54620e53830 100644
--- a/drivers/pnp/core.c
+++ b/drivers/pnp/core.c
@@ -9,6 +9,7 @@
 #include <linux/list.h>
 #include <linux/device.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/init.h>
 #include <linux/string.h>
 #include <linux/slab.h>
@@ -19,7 +20,7 @@
 
 static LIST_HEAD(pnp_protocols);
 LIST_HEAD(pnp_global);
-DEFINE_SPINLOCK(pnp_lock);
+DEFINE_MUTEX(pnp_lock);
 
 /*
  * ACPI or PNPBIOS should tell us about all platform devices, so we can
@@ -41,6 +42,13 @@ void *pnp_alloc(long size)
 	return result;
 }
 
+static void pnp_remove_protocol(struct pnp_protocol *protocol)
+{
+	mutex_lock(&pnp_lock);
+	list_del(&protocol->protocol_list);
+	mutex_unlock(&pnp_lock);
+}
+
 /**
  * pnp_protocol_register - adds a pnp protocol to the pnp layer
  * @protocol: pointer to the corresponding pnp_protocol structure
@@ -49,13 +57,14 @@ void *pnp_alloc(long size)
  */
 int pnp_register_protocol(struct pnp_protocol *protocol)
 {
-	int nodenum;
 	struct list_head *pos;
+	int nodenum, ret;
 
 	INIT_LIST_HEAD(&protocol->devices);
 	INIT_LIST_HEAD(&protocol->cards);
 	nodenum = 0;
-	spin_lock(&pnp_lock);
+
+	mutex_lock(&pnp_lock);
 
 	/* assign the lowest unused number */
 	list_for_each(pos, &pnp_protocols) {
@@ -66,12 +75,18 @@ int pnp_register_protocol(struct pnp_protocol *protocol)
 		}
 	}
 
-	list_add_tail(&protocol->protocol_list, &pnp_protocols);
-	spin_unlock(&pnp_lock);
-
 	protocol->number = nodenum;
 	dev_set_name(&protocol->dev, "pnp%d", nodenum);
-	return device_register(&protocol->dev);
+
+	list_add_tail(&protocol->protocol_list, &pnp_protocols);
+
+	mutex_unlock(&pnp_lock);
+
+	ret = device_register(&protocol->dev);
+	if (ret)
+		pnp_remove_protocol(protocol);
+
+	return ret;
 }
 
 /**
@@ -80,9 +95,7 @@ int pnp_register_protocol(struct pnp_protocol *protocol)
  */
 void pnp_unregister_protocol(struct pnp_protocol *protocol)
 {
-	spin_lock(&pnp_lock);
-	list_del(&protocol->protocol_list);
-	spin_unlock(&pnp_lock);
+	pnp_remove_protocol(protocol);
 	device_unregister(&protocol->dev);
 }
 
@@ -157,18 +170,36 @@ struct pnp_dev *pnp_alloc_dev(struct pnp_protocol *protocol, int id,
 	return dev;
 }
 
+static void pnp_delist_device(struct pnp_dev *dev)
+{
+	mutex_lock(&pnp_lock);
+	list_del(&dev->global_list);
+	list_del(&dev->protocol_list);
+	mutex_unlock(&pnp_lock);
+}
+
 int __pnp_add_device(struct pnp_dev *dev)
 {
+	int ret;
+
 	pnp_fixup_device(dev);
 	dev->status = PNP_READY;
-	spin_lock(&pnp_lock);
+
+	mutex_lock(&pnp_lock);
+
 	list_add_tail(&dev->global_list, &pnp_global);
 	list_add_tail(&dev->protocol_list, &dev->protocol->devices);
-	spin_unlock(&pnp_lock);
-	if (dev->protocol->can_wakeup)
+
+	mutex_unlock(&pnp_lock);
+
+	ret = device_register(&dev->dev);
+	if (ret)
+		pnp_delist_device(dev);
+	else if (dev->protocol->can_wakeup)
 		device_set_wakeup_capable(&dev->dev,
 				dev->protocol->can_wakeup(dev));
-	return device_register(&dev->dev);
+
+	return ret;
 }
 
 /*
@@ -203,10 +234,7 @@ int pnp_add_device(struct pnp_dev *dev)
 
 void __pnp_remove_device(struct pnp_dev *dev)
 {
-	spin_lock(&pnp_lock);
-	list_del(&dev->global_list);
-	list_del(&dev->protocol_list);
-	spin_unlock(&pnp_lock);
+	pnp_delist_device(dev);
 	device_unregister(&dev->dev);
 }
 
diff --git a/drivers/pnp/driver.c b/drivers/pnp/driver.c
index 4e57d3370368..153a493b5413 100644
--- a/drivers/pnp/driver.c
+++ b/drivers/pnp/driver.c
@@ -58,22 +58,22 @@ static const struct pnp_device_id *match_device(struct pnp_driver *drv,
 
 int pnp_device_attach(struct pnp_dev *pnp_dev)
 {
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	if (pnp_dev->status != PNP_READY) {
-		spin_unlock(&pnp_lock);
+		mutex_unlock(&pnp_lock);
 		return -EBUSY;
 	}
 	pnp_dev->status = PNP_ATTACHED;
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 	return 0;
 }
 
 void pnp_device_detach(struct pnp_dev *pnp_dev)
 {
-	spin_lock(&pnp_lock);
+	mutex_lock(&pnp_lock);
 	if (pnp_dev->status == PNP_ATTACHED)
 		pnp_dev->status = PNP_READY;
-	spin_unlock(&pnp_lock);
+	mutex_unlock(&pnp_lock);
 	pnp_disable_dev(pnp_dev);
 }
 
diff --git a/drivers/pnp/pnpacpi/core.c b/drivers/pnp/pnpacpi/core.c
index d2b780aade89..5153d1d69aee 100644
--- a/drivers/pnp/pnpacpi/core.c
+++ b/drivers/pnp/pnpacpi/core.c
@@ -248,6 +248,7 @@ static int __init pnpacpi_add_device(struct acpi_device *device)
 	if (!dev)
 		return -ENOMEM;
 
+	ACPI_COMPANION_SET(&dev->dev, device);
 	dev->data = device;
 	/* .enabled means the device can decode the resources */
 	dev->active = device->status.enabled;
@@ -290,11 +291,9 @@ static int __init pnpacpi_add_device(struct acpi_device *device)
 		return error;
 	}
 
-	error = acpi_bind_one(&dev->dev, device);
-
 	num++;
 
-	return error;
+	return 0;
 }
 
 static acpi_status __init pnpacpi_add_device_handler(acpi_handle handle,
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..e03877c4b195 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,
@@ -1030,14 +1054,15 @@ static const struct rapl_defaults rapl_defaults_atom = {
 		.driver_data = (kernel_ulong_t)&_ops,	\
 		}
 
-static const struct x86_cpu_id rapl_ids[] = {
+static const struct x86_cpu_id rapl_ids[] __initconst = {
 	RAPL_CPU(0x2a, rapl_defaults_core),/* Sandy Bridge */
 	RAPL_CPU(0x2d, rapl_defaults_core),/* Sandy Bridge EP */
 	RAPL_CPU(0x37, rapl_defaults_atom),/* Valleyview */
 	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(0x4f, rapl_defaults_hsw_server),/* Broadwell 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/ptp/ptp_chardev.c b/drivers/ptp/ptp_chardev.c
index f8a76090cbca..da7bae991552 100644
--- a/drivers/ptp/ptp_chardev.c
+++ b/drivers/ptp/ptp_chardev.c
@@ -124,7 +124,7 @@ long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
 	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
 	struct ptp_clock_info *ops = ptp->info;
 	struct ptp_clock_time *pct;
-	struct timespec ts;
+	struct timespec64 ts;
 	int enable, err = 0;
 	unsigned int i, pin_index;
 
@@ -197,16 +197,16 @@ long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
 		}
 		pct = &sysoff->ts[0];
 		for (i = 0; i < sysoff->n_samples; i++) {
-			getnstimeofday(&ts);
+			getnstimeofday64(&ts);
 			pct->sec = ts.tv_sec;
 			pct->nsec = ts.tv_nsec;
 			pct++;
-			ptp->info->gettime(ptp->info, &ts);
+			ptp->info->gettime64(ptp->info, &ts);
 			pct->sec = ts.tv_sec;
 			pct->nsec = ts.tv_nsec;
 			pct++;
 		}
-		getnstimeofday(&ts);
+		getnstimeofday64(&ts);
 		pct->sec = ts.tv_sec;
 		pct->nsec = ts.tv_nsec;
 		if (copy_to_user((void __user *)arg, sysoff, sizeof(*sysoff)))
diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c
index 296b0ec8744d..2e481b9e8ea5 100644
--- a/drivers/ptp/ptp_clock.c
+++ b/drivers/ptp/ptp_clock.c
@@ -107,13 +107,21 @@ static int ptp_clock_getres(struct posix_clock *pc, struct timespec *tp)
 static int ptp_clock_settime(struct posix_clock *pc, const struct timespec *tp)
 {
 	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
-	return ptp->info->settime(ptp->info, tp);
+	struct timespec64 ts = timespec_to_timespec64(*tp);
+
+	return  ptp->info->settime64(ptp->info, &ts);
 }
 
 static int ptp_clock_gettime(struct posix_clock *pc, struct timespec *tp)
 {
 	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
-	return ptp->info->gettime(ptp->info, tp);
+	struct timespec64 ts;
+	int err;
+
+	err = ptp->info->gettime64(ptp->info, &ts);
+	if (!err)
+		*tp = timespec64_to_timespec(ts);
+	return err;
 }
 
 static int ptp_clock_adjtime(struct posix_clock *pc, struct timex *tx)
diff --git a/drivers/ptp/ptp_ixp46x.c b/drivers/ptp/ptp_ixp46x.c
index 604d340f2095..934c139916c6 100644
--- a/drivers/ptp/ptp_ixp46x.c
+++ b/drivers/ptp/ptp_ixp46x.c
@@ -175,7 +175,7 @@ static int ptp_ixp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int ptp_ixp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int ptp_ixp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	u64 ns;
 	u32 remainder;
@@ -195,7 +195,7 @@ static int ptp_ixp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 }
 
 static int ptp_ixp_settime(struct ptp_clock_info *ptp,
-			   const struct timespec *ts)
+			   const struct timespec64 *ts)
 {
 	u64 ns;
 	unsigned long flags;
@@ -248,8 +248,8 @@ static struct ptp_clock_info ptp_ixp_caps = {
 	.pps		= 0,
 	.adjfreq	= ptp_ixp_adjfreq,
 	.adjtime	= ptp_ixp_adjtime,
-	.gettime	= ptp_ixp_gettime,
-	.settime	= ptp_ixp_settime,
+	.gettime64	= ptp_ixp_gettime,
+	.settime64	= ptp_ixp_settime,
 	.enable		= ptp_ixp_enable,
 };
 
diff --git a/drivers/ptp/ptp_pch.c b/drivers/ptp/ptp_pch.c
index 255487272859..3aa22ae4d94c 100644
--- a/drivers/ptp/ptp_pch.c
+++ b/drivers/ptp/ptp_pch.c
@@ -449,7 +449,7 @@ static int ptp_pch_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	return 0;
 }
 
-static int ptp_pch_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+static int ptp_pch_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	u64 ns;
 	u32 remainder;
@@ -467,7 +467,7 @@ static int ptp_pch_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 }
 
 static int ptp_pch_settime(struct ptp_clock_info *ptp,
-			   const struct timespec *ts)
+			   const struct timespec64 *ts)
 {
 	u64 ns;
 	unsigned long flags;
@@ -518,8 +518,8 @@ static struct ptp_clock_info ptp_pch_caps = {
 	.pps		= 0,
 	.adjfreq	= ptp_pch_adjfreq,
 	.adjtime	= ptp_pch_adjtime,
-	.gettime	= ptp_pch_gettime,
-	.settime	= ptp_pch_settime,
+	.gettime64	= ptp_pch_gettime,
+	.settime64	= ptp_pch_settime,
 	.enable		= ptp_pch_enable,
 };
 
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..fcb98dbda837 100644
--- a/drivers/regulator/arizona-micsupp.c
+++ b/drivers/regulator/arizona-micsupp.c
@@ -246,6 +246,7 @@ static int arizona_micsupp_probe(struct platform_device *pdev)
 	 */
 	switch (arizona->type) {
 	case WM5110:
+	case WM8280:
 		desc = &arizona_micsupp_ext;
 		micsupp->init_data = arizona_micsupp_ext_default;
 		break;
@@ -284,6 +285,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 +295,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 1245dca79009..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
@@ -3495,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;
@@ -3529,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);
@@ -3626,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 */
@@ -3643,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++) {
@@ -3704,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:
@@ -3800,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;
@@ -3936,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;
@@ -3949,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/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/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 b4f7744f6751..b283a1a573b3 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -324,7 +324,7 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *dev_id)
 
 		ret = IRQ_HANDLED;
 	}
-	spin_lock(&suspended_lock);
+	spin_unlock(&suspended_lock);
 
 	return ret;
 }
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index 5b2e76159b41..87647f459198 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -459,23 +459,25 @@ static int cmos_procfs(struct device *dev, struct seq_file *seq)
 	/* NOTE:  at least ICH6 reports battery status using a different
 	 * (non-RTC) bit; and SQWE is ignored on many current systems.
 	 */
-	return seq_printf(seq,
-			"periodic_IRQ\t: %s\n"
-			"update_IRQ\t: %s\n"
-			"HPET_emulated\t: %s\n"
-			// "square_wave\t: %s\n"
-			"BCD\t\t: %s\n"
-			"DST_enable\t: %s\n"
-			"periodic_freq\t: %d\n"
-			"batt_status\t: %s\n",
-			(rtc_control & RTC_PIE) ? "yes" : "no",
-			(rtc_control & RTC_UIE) ? "yes" : "no",
-			is_hpet_enabled() ? "yes" : "no",
-			// (rtc_control & RTC_SQWE) ? "yes" : "no",
-			(rtc_control & RTC_DM_BINARY) ? "no" : "yes",
-			(rtc_control & RTC_DST_EN) ? "yes" : "no",
-			cmos->rtc->irq_freq,
-			(valid & RTC_VRT) ? "okay" : "dead");
+	seq_printf(seq,
+		   "periodic_IRQ\t: %s\n"
+		   "update_IRQ\t: %s\n"
+		   "HPET_emulated\t: %s\n"
+		   // "square_wave\t: %s\n"
+		   "BCD\t\t: %s\n"
+		   "DST_enable\t: %s\n"
+		   "periodic_freq\t: %d\n"
+		   "batt_status\t: %s\n",
+		   (rtc_control & RTC_PIE) ? "yes" : "no",
+		   (rtc_control & RTC_UIE) ? "yes" : "no",
+		   is_hpet_enabled() ? "yes" : "no",
+		   // (rtc_control & RTC_SQWE) ? "yes" : "no",
+		   (rtc_control & RTC_DM_BINARY) ? "no" : "yes",
+		   (rtc_control & RTC_DST_EN) ? "yes" : "no",
+		   cmos->rtc->irq_freq,
+		   (valid & RTC_VRT) ? "okay" : "dead");
+
+	return 0;
 }
 
 #else
diff --git a/drivers/rtc/rtc-ds1305.c b/drivers/rtc/rtc-ds1305.c
index 129add77065d..12b07158a366 100644
--- a/drivers/rtc/rtc-ds1305.c
+++ b/drivers/rtc/rtc-ds1305.c
@@ -434,9 +434,9 @@ static int ds1305_proc(struct device *dev, struct seq_file *seq)
 	}
 
 done:
-	return seq_printf(seq,
-			"trickle_charge\t: %s%s\n",
-			diodes, resistors);
+	seq_printf(seq, "trickle_charge\t: %s%s\n", diodes, resistors);
+
+	return 0;
 }
 
 #else
diff --git a/drivers/rtc/rtc-hid-sensor-time.c b/drivers/rtc/rtc-hid-sensor-time.c
index ae7c2ba440cf..af4f85a66b39 100644
--- a/drivers/rtc/rtc-hid-sensor-time.c
+++ b/drivers/rtc/rtc-hid-sensor-time.c
@@ -213,7 +213,7 @@ static int hid_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	/* get a report with all values through requesting one value */
 	sensor_hub_input_attr_get_raw_value(time_state->common_attributes.hsdev,
 			HID_USAGE_SENSOR_TIME, hid_time_addresses[0],
-			time_state->info[0].report_id);
+			time_state->info[0].report_id, SENSOR_HUB_SYNC);
 	/* wait for all values (event) */
 	ret = wait_for_completion_killable_timeout(
 			&time_state->comp_last_time, HZ*6);
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..548ea6f6f384 100644
--- a/drivers/rtc/rtc-mrst.c
+++ b/drivers/rtc/rtc-mrst.c
@@ -277,13 +277,15 @@ static int mrst_procfs(struct device *dev, struct seq_file *seq)
 	valid = vrtc_cmos_read(RTC_VALID);
 	spin_unlock_irq(&rtc_lock);
 
-	return seq_printf(seq,
-			"periodic_IRQ\t: %s\n"
-			"alarm\t\t: %s\n"
-			"BCD\t\t: no\n"
-			"periodic_freq\t: daily (not adjustable)\n",
-			(rtc_control & RTC_PIE) ? "on" : "off",
-			(rtc_control & RTC_AIE) ? "on" : "off");
+	seq_printf(seq,
+		   "periodic_IRQ\t: %s\n"
+		   "alarm\t\t: %s\n"
+		   "BCD\t\t: no\n"
+		   "periodic_freq\t: daily (not adjustable)\n",
+		   (rtc_control & RTC_PIE) ? "on" : "off",
+		   (rtc_control & RTC_AIE) ? "on" : "off");
+
+	return 0;
 }
 
 #else
@@ -413,8 +415,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 +455,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 +492,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 +533,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-s5m.c b/drivers/rtc/rtc-s5m.c
index 89ac1d5083c6..4008b84246ca 100644
--- a/drivers/rtc/rtc-s5m.c
+++ b/drivers/rtc/rtc-s5m.c
@@ -48,8 +48,6 @@ struct s5m_rtc_reg_config {
 	unsigned int alarm0;
 	/* First register for alarm 1, seconds */
 	unsigned int alarm1;
-	/* SMPL/WTSR register */
-	unsigned int smpl_wtsr;
 	/*
 	 * Register for update flag (UDR). Typically setting UDR field to 1
 	 * will enable update of time or alarm register. Then it will be
@@ -67,7 +65,6 @@ static const struct s5m_rtc_reg_config s5m_rtc_regs = {
 	.ctrl			= S5M_ALARM1_CONF,
 	.alarm0			= S5M_ALARM0_SEC,
 	.alarm1			= S5M_ALARM1_SEC,
-	.smpl_wtsr		= S5M_WTSR_SMPL_CNTL,
 	.rtc_udr_update		= S5M_RTC_UDR_CON,
 	.rtc_udr_mask		= S5M_RTC_UDR_MASK,
 };
@@ -82,7 +79,6 @@ static const struct s5m_rtc_reg_config s2mps_rtc_regs = {
 	.ctrl			= S2MPS_RTC_CTRL,
 	.alarm0			= S2MPS_ALARM0_SEC,
 	.alarm1			= S2MPS_ALARM1_SEC,
-	.smpl_wtsr		= S2MPS_WTSR_SMPL_CNTL,
 	.rtc_udr_update		= S2MPS_RTC_UDR_CON,
 	.rtc_udr_mask		= S2MPS_RTC_WUDR_MASK,
 };
@@ -96,7 +92,6 @@ struct s5m_rtc_info {
 	int irq;
 	int device_type;
 	int rtc_24hr_mode;
-	bool wtsr_smpl;
 	const struct s5m_rtc_reg_config	*regs;
 };
 
@@ -597,28 +592,6 @@ static const struct rtc_class_ops s5m_rtc_ops = {
 	.alarm_irq_enable = s5m_rtc_alarm_irq_enable,
 };
 
-static void s5m_rtc_enable_wtsr(struct s5m_rtc_info *info, bool enable)
-{
-	int ret;
-	ret = regmap_update_bits(info->regmap, info->regs->smpl_wtsr,
-				 WTSR_ENABLE_MASK,
-				 enable ? WTSR_ENABLE_MASK : 0);
-	if (ret < 0)
-		dev_err(info->dev, "%s: fail to update WTSR reg(%d)\n",
-			__func__, ret);
-}
-
-static void s5m_rtc_enable_smpl(struct s5m_rtc_info *info, bool enable)
-{
-	int ret;
-	ret = regmap_update_bits(info->regmap, info->regs->smpl_wtsr,
-				 SMPL_ENABLE_MASK,
-				 enable ? SMPL_ENABLE_MASK : 0);
-	if (ret < 0)
-		dev_err(info->dev, "%s: fail to update SMPL reg(%d)\n",
-			__func__, ret);
-}
-
 static int s5m8767_rtc_init_reg(struct s5m_rtc_info *info)
 {
 	u8 data[2];
@@ -715,7 +688,6 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 	info->dev = &pdev->dev;
 	info->s5m87xx = s5m87xx;
 	info->device_type = s5m87xx->device_type;
-	info->wtsr_smpl = s5m87xx->wtsr_smpl;
 
 	if (s5m87xx->irq_data) {
 		info->irq = regmap_irq_get_virq(s5m87xx->irq_data, alarm_irq);
@@ -731,11 +703,6 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 
 	ret = s5m8767_rtc_init_reg(info);
 
-	if (info->wtsr_smpl) {
-		s5m_rtc_enable_wtsr(info, true);
-		s5m_rtc_enable_smpl(info, true);
-	}
-
 	device_init_wakeup(&pdev->dev, 1);
 
 	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "s5m-rtc",
@@ -768,36 +735,10 @@ err:
 	return ret;
 }
 
-static void s5m_rtc_shutdown(struct platform_device *pdev)
-{
-	struct s5m_rtc_info *info = platform_get_drvdata(pdev);
-	int i;
-	unsigned int val = 0;
-	if (info->wtsr_smpl) {
-		for (i = 0; i < 3; i++) {
-			s5m_rtc_enable_wtsr(info, false);
-			regmap_read(info->regmap, info->regs->smpl_wtsr, &val);
-			pr_debug("%s: WTSR_SMPL reg(0x%02x)\n", __func__, val);
-			if (val & WTSR_ENABLE_MASK)
-				pr_emerg("%s: fail to disable WTSR\n",
-					 __func__);
-			else {
-				pr_info("%s: success to disable WTSR\n",
-					__func__);
-				break;
-			}
-		}
-	}
-	/* Disable SMPL when power off */
-	s5m_rtc_enable_smpl(info, false);
-}
-
 static int s5m_rtc_remove(struct platform_device *pdev)
 {
 	struct s5m_rtc_info *info = platform_get_drvdata(pdev);
 
-	/* Perform also all shutdown steps when removing */
-	s5m_rtc_shutdown(pdev);
 	i2c_unregister_device(info->i2c);
 
 	return 0;
@@ -842,7 +783,6 @@ static struct platform_driver s5m_rtc_driver = {
 	},
 	.probe		= s5m_rtc_probe,
 	.remove		= s5m_rtc_remove,
-	.shutdown	= s5m_rtc_shutdown,
 	.id_table	= s5m_rtc_id,
 };
 
diff --git a/drivers/rtc/rtc-tegra.c b/drivers/rtc/rtc-tegra.c
index d948277057d8..60232bd366ef 100644
--- a/drivers/rtc/rtc-tegra.c
+++ b/drivers/rtc/rtc-tegra.c
@@ -261,7 +261,9 @@ static int tegra_rtc_proc(struct device *dev, struct seq_file *seq)
 	if (!dev || !dev->driver)
 		return 0;
 
-	return seq_printf(seq, "name\t\t: %s\n", dev_name(dev));
+	seq_printf(seq, "name\t\t: %s\n", dev_name(dev));
+
+	return 0;
 }
 
 static irqreturn_t tegra_rtc_irq_handler(int irq, void *data)
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/dasd.c b/drivers/s390/block/dasd.c
index be34ef41b7c7..26a51dc4278d 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -1237,7 +1237,6 @@ EXPORT_SYMBOL(dasd_smalloc_request);
  */
 void dasd_kfree_request(struct dasd_ccw_req *cqr, struct dasd_device *device)
 {
-#ifdef CONFIG_64BIT
 	struct ccw1 *ccw;
 
 	/* Clear any idals used for the request. */
@@ -1245,7 +1244,6 @@ void dasd_kfree_request(struct dasd_ccw_req *cqr, struct dasd_device *device)
 	do {
 		clear_normalized_cda(ccw);
 	} while (ccw++->flags & (CCW_FLAG_CC | CCW_FLAG_DC));
-#endif
 	kfree(cqr->cpaddr);
 	kfree(cqr->data);
 	kfree(cqr);
@@ -2967,8 +2965,6 @@ enum blk_eh_timer_return dasd_times_out(struct request *req)
  */
 static int dasd_alloc_queue(struct dasd_block *block)
 {
-	int rc;
-
 	block->request_queue = blk_init_queue(do_dasd_request,
 					       &block->request_queue_lock);
 	if (block->request_queue == NULL)
@@ -2976,14 +2972,7 @@ static int dasd_alloc_queue(struct dasd_block *block)
 
 	block->request_queue->queuedata = block;
 
-	elevator_exit(block->request_queue->elevator);
-	block->request_queue->elevator = NULL;
-	mutex_lock(&block->request_queue->sysfs_lock);
-	rc = elevator_init(block->request_queue, "deadline");
-	if (rc)
-		blk_cleanup_queue(block->request_queue);
-	mutex_unlock(&block->request_queue->sysfs_lock);
-	return rc;
+	return 0;
 }
 
 /*
diff --git a/drivers/s390/block/dasd_diag.h b/drivers/s390/block/dasd_diag.h
index a803cc731586..e84a5468d810 100644
--- a/drivers/s390/block/dasd_diag.h
+++ b/drivers/s390/block/dasd_diag.h
@@ -38,8 +38,6 @@ struct dasd_diag_characteristics {
 	u8 rdev_features;
 } __attribute__ ((packed, aligned(4)));
 
-
-#ifdef CONFIG_64BIT
 #define DASD_DIAG_FLAGA_DEFAULT		DASD_DIAG_FLAGA_FORMAT_64BIT
 
 typedef u64 blocknum_t;
@@ -80,43 +78,3 @@ struct dasd_diag_rw_io {
 	struct dasd_diag_bio *bio_list;
 	u8  spare4[8];
 } __attribute__ ((packed, aligned(8)));
-#else /* CONFIG_64BIT */
-#define DASD_DIAG_FLAGA_DEFAULT		0x0
-
-typedef u32 blocknum_t;
-typedef s32 sblocknum_t;
-
-struct dasd_diag_bio {
-	u8 type;
-	u8 status;
-	u16 spare1;
-	blocknum_t block_number;
-	u32 alet;
-	void *buffer;
-} __attribute__ ((packed, aligned(8)));
-
-struct dasd_diag_init_io {
-	u16 dev_nr;
-	u8 flaga;
-	u8 spare1[21];
-	u32 block_size;
-	blocknum_t offset;
-	sblocknum_t start_block;
-	blocknum_t end_block;
-	u8 spare2[24];
-} __attribute__ ((packed, aligned(8)));
-
-struct dasd_diag_rw_io {
-	u16 dev_nr;
-	u8 flaga;
-	u8 spare1[21];
-	u8 key;
-	u8 flags;
-	u8 spare2[2];
-	u32 block_count;
-	u32 alet;
-	struct dasd_diag_bio *bio_list;
-	u32 interrupt_params;
-	u8 spare3[20];
-} __attribute__ ((packed, aligned(8)));
-#endif /* CONFIG_64BIT */
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index d47f5b99623a..49b48a887c66 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -1633,7 +1633,6 @@ static void dasd_eckd_kick_validate_server(struct dasd_device *device)
 
 static u32 get_fcx_max_data(struct dasd_device *device)
 {
-#if defined(CONFIG_64BIT)
 	int tpm, mdc;
 	int fcx_in_css, fcx_in_gneq, fcx_in_features;
 	struct dasd_eckd_private *private;
@@ -1657,9 +1656,6 @@ static u32 get_fcx_max_data(struct dasd_device *device)
 		return 0;
 	} else
 		return mdc * FCX_MAX_DATA_FACTOR;
-#else
-	return 0;
-#endif
 }
 
 /*
@@ -2615,10 +2611,8 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_cmd_single(
 			/* Eckd can only do full blocks. */
 			return ERR_PTR(-EINVAL);
 		count += bv.bv_len >> (block->s2b_shift + 9);
-#if defined(CONFIG_64BIT)
 		if (idal_is_needed (page_address(bv.bv_page), bv.bv_len))
 			cidaw += bv.bv_len >> (block->s2b_shift + 9);
-#endif
 	}
 	/* Paranoia. */
 	if (count != last_rec - first_rec + 1)
diff --git a/drivers/s390/block/dasd_fba.c b/drivers/s390/block/dasd_fba.c
index 2c8e68bf9a1c..c9262e78938b 100644
--- a/drivers/s390/block/dasd_fba.c
+++ b/drivers/s390/block/dasd_fba.c
@@ -287,10 +287,8 @@ static struct dasd_ccw_req *dasd_fba_build_cp(struct dasd_device * memdev,
 			/* Fba can only do full blocks. */
 			return ERR_PTR(-EINVAL);
 		count += bv.bv_len >> (block->s2b_shift + 9);
-#if defined(CONFIG_64BIT)
 		if (idal_is_needed (page_address(bv.bv_page), bv.bv_len))
 			cidaw += bv.bv_len / blksize;
-#endif
 	}
 	/* Paranoia. */
 	if (count != last_rec - first_rec + 1)
diff --git a/drivers/s390/char/Kconfig b/drivers/s390/char/Kconfig
index a5c6f7e157aa..eaca3e006301 100644
--- a/drivers/s390/char/Kconfig
+++ b/drivers/s390/char/Kconfig
@@ -115,7 +115,7 @@ config SCLP_ASYNC_ID
 config HMC_DRV
 	def_tristate m
 	prompt "Support for file transfers from HMC drive CD/DVD-ROM"
-	depends on S390 && 64BIT
+	depends on S390
 	select CRC16
 	help
 	  This option enables support for file transfers from a Hardware
diff --git a/drivers/s390/char/sclp_sdias.c b/drivers/s390/char/sclp_sdias.c
index 561a0414b352..eb7cb076c001 100644
--- a/drivers/s390/char/sclp_sdias.c
+++ b/drivers/s390/char/sclp_sdias.c
@@ -178,11 +178,7 @@ int sclp_sdias_copy(void *dest, int start_blk, int nr_blks)
 	sccb.evbuf.event_qual = SDIAS_EQ_STORE_DATA;
 	sccb.evbuf.data_id = SDIAS_DI_FCP_DUMP;
 	sccb.evbuf.event_id = 4712;
-#ifdef CONFIG_64BIT
 	sccb.evbuf.asa_size = SDIAS_ASA_SIZE_64;
-#else
-	sccb.evbuf.asa_size = SDIAS_ASA_SIZE_32;
-#endif
 	sccb.evbuf.event_status = 0;
 	sccb.evbuf.blk_cnt = nr_blks;
 	sccb.evbuf.asa = (unsigned long)dest;
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index efcf48481c5f..a68fcfd1d48c 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -212,11 +212,7 @@ static struct zcore_header zcore_header = {
 	.dump_level	= 0,
 	.page_size	= PAGE_SIZE,
 	.mem_start	= 0,
-#ifdef CONFIG_64BIT
 	.build_arch	= DUMP_ARCH_S390X,
-#else
-	.build_arch	= DUMP_ARCH_S390,
-#endif
 };
 
 /*
@@ -516,23 +512,6 @@ static const struct file_operations zcore_hsa_fops = {
 	.llseek		= no_llseek,
 };
 
-#ifdef CONFIG_32BIT
-
-static void __init set_lc_mask(struct save_area *map)
-{
-	memset(&map->ext_save, 0xff, sizeof(map->ext_save));
-	memset(&map->timer, 0xff, sizeof(map->timer));
-	memset(&map->clk_cmp, 0xff, sizeof(map->clk_cmp));
-	memset(&map->psw, 0xff, sizeof(map->psw));
-	memset(&map->pref_reg, 0xff, sizeof(map->pref_reg));
-	memset(&map->acc_regs, 0xff, sizeof(map->acc_regs));
-	memset(&map->fp_regs, 0xff, sizeof(map->fp_regs));
-	memset(&map->gp_regs, 0xff, sizeof(map->gp_regs));
-	memset(&map->ctrl_regs, 0xff, sizeof(map->ctrl_regs));
-}
-
-#else /* CONFIG_32BIT */
-
 static void __init set_lc_mask(struct save_area *map)
 {
 	memset(&map->fp_regs, 0xff, sizeof(map->fp_regs));
@@ -547,8 +526,6 @@ static void __init set_lc_mask(struct save_area *map)
 	memset(&map->ctrl_regs, 0xff, sizeof(map->ctrl_regs));
 }
 
-#endif /* CONFIG_32BIT */
-
 /*
  * Initialize dump globals for a given architecture
  */
@@ -688,21 +665,12 @@ static int __init zcore_init(void)
 	if (rc)
 		goto fail;
 
-#ifdef CONFIG_64BIT
 	if (arch == ARCH_S390) {
 		pr_alert("The 64-bit dump tool cannot be used for a "
 			 "32-bit system\n");
 		rc = -EINVAL;
 		goto fail;
 	}
-#else /* CONFIG_64BIT */
-	if (arch == ARCH_S390X) {
-		pr_alert("The 32-bit dump tool cannot be used for a "
-			 "64-bit system\n");
-		rc = -EINVAL;
-		goto fail;
-	}
-#endif /* CONFIG_64BIT */
 
 	rc = get_mem_info(&mem_size, &mem_end);
 	if (rc)
diff --git a/drivers/s390/cio/blacklist.c b/drivers/s390/cio/blacklist.c
index b3f791b2c1f8..20314aad7ab7 100644
--- a/drivers/s390/cio/blacklist.c
+++ b/drivers/s390/cio/blacklist.c
@@ -330,18 +330,20 @@ cio_ignore_proc_seq_show(struct seq_file *s, void *it)
 	if (!iter->in_range) {
 		/* First device in range. */
 		if ((iter->devno == __MAX_SUBCHANNEL) ||
-		    !is_blacklisted(iter->ssid, iter->devno + 1))
+		    !is_blacklisted(iter->ssid, iter->devno + 1)) {
 			/* Singular device. */
-			return seq_printf(s, "0.%x.%04x\n",
-					  iter->ssid, iter->devno);
+			seq_printf(s, "0.%x.%04x\n", iter->ssid, iter->devno);
+			return 0;
+		}
 		iter->in_range = 1;
-		return seq_printf(s, "0.%x.%04x-", iter->ssid, iter->devno);
+		seq_printf(s, "0.%x.%04x-", iter->ssid, iter->devno);
+		return 0;
 	}
 	if ((iter->devno == __MAX_SUBCHANNEL) ||
 	    !is_blacklisted(iter->ssid, iter->devno + 1)) {
 		/* Last device in range. */
 		iter->in_range = 0;
-		return seq_printf(s, "0.%x.%04x\n", iter->ssid, iter->devno);
+		seq_printf(s, "0.%x.%04x\n", iter->ssid, iter->devno);
 	}
 	return 0;
 }
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 3578105989a0..07fc5d9e7f10 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -143,13 +143,11 @@ cio_start_key (struct subchannel *sch,	/* subchannel structure */
 	orb->cmd.spnd = priv->options.suspend;
 	orb->cmd.ssic = priv->options.suspend && priv->options.inter;
 	orb->cmd.lpm = (lpm != 0) ? lpm : sch->lpm;
-#ifdef CONFIG_64BIT
 	/*
 	 * for 64 bit we always support 64 bit IDAWs with 4k page size only
 	 */
 	orb->cmd.c64 = 1;
 	orb->cmd.i2k = 0;
-#endif
 	orb->cmd.key = key >> 4;
 	/* issue "Start Subchannel" */
 	orb->cmd.cpa = (__u32) __pa(cpa);
diff --git a/drivers/s390/cio/qdio.h b/drivers/s390/cio/qdio.h
index a563e4c00590..7e70f9298cc1 100644
--- a/drivers/s390/cio/qdio.h
+++ b/drivers/s390/cio/qdio.h
@@ -84,7 +84,6 @@ enum qdio_irq_states {
 #define QDIO_SIGA_WRITEQ	0x04
 #define QDIO_SIGA_QEBSM_FLAG	0x80
 
-#ifdef CONFIG_64BIT
 static inline int do_sqbs(u64 token, unsigned char state, int queue,
 			  int *start, int *count)
 {
@@ -122,12 +121,6 @@ static inline int do_eqbs(u64 token, unsigned char *state, int queue,
 
 	return (_ccq >> 32) & 0xff;
 }
-#else
-static inline int do_sqbs(u64 token, unsigned char state, int queue,
-			  int *start, int *count) { return 0; }
-static inline int do_eqbs(u64 token, unsigned char *state, int queue,
-			  int *start, int *count, int ack) { return 0; }
-#endif /* CONFIG_64BIT */
 
 struct qdio_irq;
 
diff --git a/drivers/s390/cio/qdio_setup.c b/drivers/s390/cio/qdio_setup.c
index f76bff68d1de..48b3866a9ded 100644
--- a/drivers/s390/cio/qdio_setup.c
+++ b/drivers/s390/cio/qdio_setup.c
@@ -91,10 +91,7 @@ EXPORT_SYMBOL_GPL(qdio_reset_buffers);
  */
 static inline int qebsm_possible(void)
 {
-#ifdef CONFIG_64BIT
 	return css_general_characteristics.qebsm;
-#endif
-	return 0;
 }
 
 /*
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 3d7f19fb9a4e..f0b9871a4bbd 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -165,7 +165,7 @@ static inline int ap_instructions_available(void)
  */
 static int ap_interrupts_available(void)
 {
-	return test_facility(2) && test_facility(65);
+	return test_facility(65);
 }
 
 /**
@@ -174,12 +174,10 @@ static int ap_interrupts_available(void)
  *
  * Returns 1 if AP configuration information is available.
  */
-#ifdef CONFIG_64BIT
 static int ap_configuration_available(void)
 {
-	return test_facility(2) && test_facility(12);
+	return test_facility(12);
 }
-#endif
 
 /**
  * ap_test_queue(): Test adjunct processor queue.
@@ -239,7 +237,6 @@ static inline struct ap_queue_status ap_reset_queue(ap_qid_t qid)
 	return reg1;
 }
 
-#ifdef CONFIG_64BIT
 /**
  * ap_queue_interruption_control(): Enable interruption for a specific AP.
  * @qid: The AP queue number
@@ -261,9 +258,7 @@ ap_queue_interruption_control(ap_qid_t qid, void *ind)
 		: "cc" );
 	return reg1_out;
 }
-#endif
 
-#ifdef CONFIG_64BIT
 static inline struct ap_queue_status
 __ap_query_functions(ap_qid_t qid, unsigned int *functions)
 {
@@ -282,9 +277,7 @@ __ap_query_functions(ap_qid_t qid, unsigned int *functions)
 	*functions = (unsigned int)(reg2 >> 32);
 	return reg1;
 }
-#endif
 
-#ifdef CONFIG_64BIT
 static inline int __ap_query_configuration(struct ap_config_info *config)
 {
 	register unsigned long reg0 asm ("0") = 0x04000000UL;
@@ -302,7 +295,6 @@ static inline int __ap_query_configuration(struct ap_config_info *config)
 
 	return reg1;
 }
-#endif
 
 /**
  * ap_query_functions(): Query supported functions.
@@ -317,7 +309,6 @@ static inline int __ap_query_configuration(struct ap_config_info *config)
  */
 static int ap_query_functions(ap_qid_t qid, unsigned int *functions)
 {
-#ifdef CONFIG_64BIT
 	struct ap_queue_status status;
 	int i;
 	status = __ap_query_functions(qid, functions);
@@ -348,9 +339,6 @@ static int ap_query_functions(ap_qid_t qid, unsigned int *functions)
 		}
 	}
 	return -EBUSY;
-#else
-	return -EINVAL;
-#endif
 }
 
 /**
@@ -364,7 +352,6 @@ static int ap_query_functions(ap_qid_t qid, unsigned int *functions)
  */
 static int ap_queue_enable_interruption(ap_qid_t qid, void *ind)
 {
-#ifdef CONFIG_64BIT
 	struct ap_queue_status status;
 	int t_depth, t_device_type, rc, i;
 
@@ -404,9 +391,6 @@ static int ap_queue_enable_interruption(ap_qid_t qid, void *ind)
 		}
 	}
 	return rc;
-#else
-	return -EINVAL;
-#endif
 }
 
 /**
@@ -1238,7 +1222,6 @@ static struct bus_attribute *const ap_bus_attrs[] = {
  */
 static void ap_query_configuration(void)
 {
-#ifdef CONFIG_64BIT
 	if (ap_configuration_available()) {
 		if (!ap_configuration)
 			ap_configuration =
@@ -1248,9 +1231,6 @@ static void ap_query_configuration(void)
 			__ap_query_configuration(ap_configuration);
 	} else
 		ap_configuration = NULL;
-#else
-	ap_configuration = NULL;
-#endif
 }
 
 /**
diff --git a/drivers/s390/net/Kconfig b/drivers/s390/net/Kconfig
index f1b5111bbaba..b2837b1c70b7 100644
--- a/drivers/s390/net/Kconfig
+++ b/drivers/s390/net/Kconfig
@@ -57,17 +57,6 @@ config SMSGIUCV_EVENT
 
 	  To compile as a module, choose M. The module name is "smsgiucv_app".
 
-config CLAW
-	def_tristate m
-	prompt "CLAW device support"
-	depends on CCW && NETDEVICES
-	help
-	  This driver supports channel attached CLAW devices.
-	  CLAW is Common Link Access for Workstation.  Common devices
-          that use CLAW are RS/6000s, Cisco Routers (CIP) and 3172 devices.
-	  To compile as a module, choose M. The module name is claw.
-	  To compile into the kernel, choose Y.
-
 config QETH
 	def_tristate y
 	prompt "Gigabit Ethernet device support"
@@ -106,6 +95,6 @@ config QETH_IPV6
 
 config CCWGROUP
 	tristate
-	default (LCS || CTCM || QETH || CLAW)
+	default (LCS || CTCM || QETH)
 
 endmenu
diff --git a/drivers/s390/net/Makefile b/drivers/s390/net/Makefile
index d28f05d0c75a..c351b07603e0 100644
--- a/drivers/s390/net/Makefile
+++ b/drivers/s390/net/Makefile
@@ -8,7 +8,6 @@ obj-$(CONFIG_NETIUCV) += netiucv.o fsm.o
 obj-$(CONFIG_SMSGIUCV) += smsgiucv.o
 obj-$(CONFIG_SMSGIUCV_EVENT) += smsgiucv_app.o
 obj-$(CONFIG_LCS) += lcs.o
-obj-$(CONFIG_CLAW) += claw.o
 qeth-y += qeth_core_sys.o qeth_core_main.o qeth_core_mpc.o
 obj-$(CONFIG_QETH) += qeth.o
 qeth_l2-y += qeth_l2_main.o qeth_l2_sys.o
diff --git a/drivers/s390/net/claw.c b/drivers/s390/net/claw.c
deleted file mode 100644
index d609ca09aa94..000000000000
--- a/drivers/s390/net/claw.c
+++ /dev/null
@@ -1,3377 +0,0 @@
-/*
- *    ESCON CLAW network driver
- *
- *  Linux for zSeries version
- *    Copyright IBM Corp. 2002, 2009
- *  Author(s) Original code written by:
- *		Kazuo Iimura <iimura@jp.ibm.com>
- *   	      Rewritten by
- *		Andy Richter <richtera@us.ibm.com>
- *		Marc Price <mwprice@us.ibm.com>
- *
- *    sysfs parms:
- *   group x.x.rrrr,x.x.wwww
- *   read_buffer nnnnnnn
- *   write_buffer nnnnnn
- *   host_name  aaaaaaaa
- *   adapter_name aaaaaaaa
- *   api_type    aaaaaaaa
- *
- *  eg.
- *   group  0.0.0200 0.0.0201
- *   read_buffer 25
- *   write_buffer 20
- *   host_name LINUX390
- *   adapter_name RS6K
- *   api_type     TCPIP
- *
- *  where
- *
- *   The device id is decided by the order entries
- *   are added to the group the first is claw0 the second claw1
- *   up to CLAW_MAX_DEV
- *
- *   rrrr     -	the first of 2 consecutive device addresses used for the
- *		CLAW protocol.
- *		The specified address is always used as the input (Read)
- *		channel and the next address is used as the output channel.
- *
- *   wwww     -	the second of 2 consecutive device addresses used for
- *		the CLAW protocol.
- *              The specified address is always used as the output
- *		channel and the previous address is used as the input channel.
- *
- *   read_buffer	-       specifies number of input buffers to allocate.
- *   write_buffer       -       specifies number of output buffers to allocate.
- *   host_name          -       host name
- *   adaptor_name       -       adaptor name
- *   api_type           -       API type TCPIP or API will be sent and expected
- *				as ws_name
- *
- *   Note the following requirements:
- *   1)  host_name must match the configured adapter_name on the remote side
- *   2)  adaptor_name must match the configured host name on the remote side
- *
- *  Change History
- *    1.00  Initial release shipped
- *    1.10  Changes for Buffer allocation
- *    1.15  Changed for 2.6 Kernel  No longer compiles on 2.4 or lower
- *    1.25  Added Packing support
- *    1.5
- */
-
-#define KMSG_COMPONENT "claw"
-
-#include <asm/ccwdev.h>
-#include <asm/ccwgroup.h>
-#include <asm/debug.h>
-#include <asm/idals.h>
-#include <asm/io.h>
-#include <linux/bitops.h>
-#include <linux/ctype.h>
-#include <linux/delay.h>
-#include <linux/errno.h>
-#include <linux/if_arp.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/ip.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/proc_fs.h>
-#include <linux/sched.h>
-#include <linux/signal.h>
-#include <linux/skbuff.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/tcp.h>
-#include <linux/timer.h>
-#include <linux/types.h>
-
-#include "claw.h"
-
-/*
-   CLAW uses the s390dbf file system  see claw_trace and claw_setup
-*/
-
-static char version[] __initdata = "CLAW driver";
-static char debug_buffer[255];
-/**
- * Debug Facility Stuff
- */
-static debug_info_t *claw_dbf_setup;
-static debug_info_t *claw_dbf_trace;
-
-/**
- *  CLAW Debug Facility functions
- */
-static void
-claw_unregister_debug_facility(void)
-{
-	debug_unregister(claw_dbf_setup);
-	debug_unregister(claw_dbf_trace);
-}
-
-static int
-claw_register_debug_facility(void)
-{
-	claw_dbf_setup = debug_register("claw_setup", 2, 1, 8);
-	claw_dbf_trace = debug_register("claw_trace", 2, 2, 8);
-	if (claw_dbf_setup == NULL || claw_dbf_trace == NULL) {
-		claw_unregister_debug_facility();
-		return -ENOMEM;
-	}
-	debug_register_view(claw_dbf_setup, &debug_hex_ascii_view);
-	debug_set_level(claw_dbf_setup, 2);
-	debug_register_view(claw_dbf_trace, &debug_hex_ascii_view);
-	debug_set_level(claw_dbf_trace, 2);
-	return 0;
-}
-
-static inline void
-claw_set_busy(struct net_device *dev)
-{
- ((struct claw_privbk *)dev->ml_priv)->tbusy = 1;
-}
-
-static inline void
-claw_clear_busy(struct net_device *dev)
-{
-	clear_bit(0, &(((struct claw_privbk *) dev->ml_priv)->tbusy));
-	netif_wake_queue(dev);
-}
-
-static inline int
-claw_check_busy(struct net_device *dev)
-{
-	return ((struct claw_privbk *) dev->ml_priv)->tbusy;
-}
-
-static inline void
-claw_setbit_busy(int nr,struct net_device *dev)
-{
-	netif_stop_queue(dev);
-	set_bit(nr, (void *)&(((struct claw_privbk *)dev->ml_priv)->tbusy));
-}
-
-static inline void
-claw_clearbit_busy(int nr,struct net_device *dev)
-{
-	clear_bit(nr, (void *)&(((struct claw_privbk *)dev->ml_priv)->tbusy));
-	netif_wake_queue(dev);
-}
-
-static inline int
-claw_test_and_setbit_busy(int nr,struct net_device *dev)
-{
-	netif_stop_queue(dev);
-	return test_and_set_bit(nr,
-		(void *)&(((struct claw_privbk *) dev->ml_priv)->tbusy));
-}
-
-
-/* Functions for the DEV methods */
-
-static int claw_probe(struct ccwgroup_device *cgdev);
-static void claw_remove_device(struct ccwgroup_device *cgdev);
-static void claw_purge_skb_queue(struct sk_buff_head *q);
-static int claw_new_device(struct ccwgroup_device *cgdev);
-static int claw_shutdown_device(struct ccwgroup_device *cgdev);
-static int claw_tx(struct sk_buff *skb, struct net_device *dev);
-static int claw_change_mtu( struct net_device *dev, int new_mtu);
-static int claw_open(struct net_device *dev);
-static void claw_irq_handler(struct ccw_device *cdev,
-	unsigned long intparm, struct irb *irb);
-static void claw_irq_tasklet ( unsigned long data );
-static int claw_release(struct net_device *dev);
-static void claw_write_retry ( struct chbk * p_ch );
-static void claw_write_next ( struct chbk * p_ch );
-static void claw_timer ( struct chbk * p_ch );
-
-/* Functions */
-static int add_claw_reads(struct net_device *dev,
-	struct ccwbk* p_first, struct ccwbk* p_last);
-static void ccw_check_return_code (struct ccw_device *cdev, int return_code);
-static void ccw_check_unit_check (struct chbk * p_ch, unsigned char sense );
-static int find_link(struct net_device *dev, char *host_name, char *ws_name );
-static int claw_hw_tx(struct sk_buff *skb, struct net_device *dev, long linkid);
-static int init_ccw_bk(struct net_device *dev);
-static void probe_error( struct ccwgroup_device *cgdev);
-static struct net_device_stats *claw_stats(struct net_device *dev);
-static int pages_to_order_of_mag(int num_of_pages);
-static struct sk_buff *claw_pack_skb(struct claw_privbk *privptr);
-/* sysfs Functions */
-static ssize_t claw_hname_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
-static ssize_t claw_hname_write(struct device *dev,
-	struct device_attribute *attr,
-	const char *buf, size_t count);
-static ssize_t claw_adname_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
-static ssize_t claw_adname_write(struct device *dev,
-	struct device_attribute *attr,
-	const char *buf, size_t count);
-static ssize_t claw_apname_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
-static ssize_t claw_apname_write(struct device *dev,
-	struct device_attribute *attr,
-	const char *buf, size_t count);
-static ssize_t claw_wbuff_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
-static ssize_t claw_wbuff_write(struct device *dev,
-	struct device_attribute *attr,
-	const char *buf, size_t count);
-static ssize_t claw_rbuff_show(struct device *dev,
-	struct device_attribute *attr, char *buf);
-static ssize_t claw_rbuff_write(struct device *dev,
-	struct device_attribute *attr,
-	const char *buf, size_t count);
-
-/*   Functions for System Validate  */
-static int claw_process_control( struct net_device *dev, struct ccwbk * p_ccw);
-static int claw_send_control(struct net_device *dev, __u8 type, __u8 link,
-       __u8 correlator, __u8 rc , char *local_name, char *remote_name);
-static int claw_snd_conn_req(struct net_device *dev, __u8 link);
-static int claw_snd_disc(struct net_device *dev, struct clawctl * p_ctl);
-static int claw_snd_sys_validate_rsp(struct net_device *dev,
-        struct clawctl * p_ctl, __u32 return_code);
-static int claw_strt_conn_req(struct net_device *dev );
-static void claw_strt_read(struct net_device *dev, int lock);
-static void claw_strt_out_IO(struct net_device *dev);
-static void claw_free_wrt_buf(struct net_device *dev);
-
-/* Functions for unpack reads   */
-static void unpack_read(struct net_device *dev);
-
-static int claw_pm_prepare(struct ccwgroup_device *gdev)
-{
-	return -EPERM;
-}
-
-/* the root device for claw group devices */
-static struct device *claw_root_dev;
-
-/* ccwgroup table  */
-
-static struct ccwgroup_driver claw_group_driver = {
-	.driver = {
-		.owner	= THIS_MODULE,
-		.name	= "claw",
-	},
-	.setup	     = claw_probe,
-	.remove      = claw_remove_device,
-	.set_online  = claw_new_device,
-	.set_offline = claw_shutdown_device,
-	.prepare     = claw_pm_prepare,
-};
-
-static struct ccw_device_id claw_ids[] = {
-	{CCW_DEVICE(0x3088, 0x61), .driver_info = claw_channel_type_claw},
-	{},
-};
-MODULE_DEVICE_TABLE(ccw, claw_ids);
-
-static struct ccw_driver claw_ccw_driver = {
-	.driver = {
-		.owner	= THIS_MODULE,
-		.name	= "claw",
-	},
-	.ids	= claw_ids,
-	.probe	= ccwgroup_probe_ccwdev,
-	.remove	= ccwgroup_remove_ccwdev,
-	.int_class = IRQIO_CLW,
-};
-
-static ssize_t claw_driver_group_store(struct device_driver *ddrv,
-				       const char *buf,	size_t count)
-{
-	int err;
-	err = ccwgroup_create_dev(claw_root_dev, &claw_group_driver, 2, buf);
-	return err ? err : count;
-}
-static DRIVER_ATTR(group, 0200, NULL, claw_driver_group_store);
-
-static struct attribute *claw_drv_attrs[] = {
-	&driver_attr_group.attr,
-	NULL,
-};
-static struct attribute_group claw_drv_attr_group = {
-	.attrs = claw_drv_attrs,
-};
-static const struct attribute_group *claw_drv_attr_groups[] = {
-	&claw_drv_attr_group,
-	NULL,
-};
-
-/*
-*       Key functions
-*/
-
-/*-------------------------------------------------------------------*
- *   claw_tx                                                         *
- *-------------------------------------------------------------------*/
-
-static int
-claw_tx(struct sk_buff *skb, struct net_device *dev)
-{
-        int             rc;
-	struct claw_privbk *privptr = dev->ml_priv;
-	unsigned long saveflags;
-        struct chbk *p_ch;
-
-	CLAW_DBF_TEXT(4, trace, "claw_tx");
-	p_ch = &privptr->channel[WRITE_CHANNEL];
-        spin_lock_irqsave(get_ccwdev_lock(p_ch->cdev), saveflags);
-        rc=claw_hw_tx( skb, dev, 1 );
-        spin_unlock_irqrestore(get_ccwdev_lock(p_ch->cdev), saveflags);
-	CLAW_DBF_TEXT_(4, trace, "clawtx%d", rc);
-	if (rc)
-		rc = NETDEV_TX_BUSY;
-	else
-		rc = NETDEV_TX_OK;
-        return rc;
-}   /*  end of claw_tx */
-
-/*------------------------------------------------------------------*
- *  pack the collect queue into an skb and return it                *
- *   If not packing just return the top skb from the queue          *
- *------------------------------------------------------------------*/
-
-static struct sk_buff *
-claw_pack_skb(struct claw_privbk *privptr)
-{
-	struct sk_buff *new_skb,*held_skb;
-	struct chbk *p_ch = &privptr->channel[WRITE_CHANNEL];
-	struct claw_env  *p_env = privptr->p_env;
-	int	pkt_cnt,pk_ind,so_far;
-
-	new_skb = NULL;		/* assume no dice */
-	pkt_cnt = 0;
-	CLAW_DBF_TEXT(4, trace, "PackSKBe");
-	if (!skb_queue_empty(&p_ch->collect_queue)) {
-	/* some data */
-		held_skb = skb_dequeue(&p_ch->collect_queue);
-		if (held_skb)
-			dev_kfree_skb_any(held_skb);
-		else
-			return NULL;
-		if (p_env->packing != DO_PACKED)
-			return held_skb;
-		/* get a new SKB we will pack at least one */
-		new_skb = dev_alloc_skb(p_env->write_size);
-		if (new_skb == NULL) {
-			atomic_inc(&held_skb->users);
-			skb_queue_head(&p_ch->collect_queue,held_skb);
-			return NULL;
-		}
-		/* we have packed packet and a place to put it  */
-		pk_ind = 1;
-		so_far = 0;
-		new_skb->cb[1] = 'P'; /* every skb on queue has pack header */
-		while ((pk_ind) && (held_skb != NULL)) {
-			if (held_skb->len+so_far <= p_env->write_size-8) {
-				memcpy(skb_put(new_skb,held_skb->len),
-					held_skb->data,held_skb->len);
-				privptr->stats.tx_packets++;
-				so_far += held_skb->len;
-				pkt_cnt++;
-				dev_kfree_skb_any(held_skb);
-				held_skb = skb_dequeue(&p_ch->collect_queue);
-				if (held_skb)
-					atomic_dec(&held_skb->users);
-			} else {
-				pk_ind = 0;
-				atomic_inc(&held_skb->users);
-				skb_queue_head(&p_ch->collect_queue,held_skb);
-			}
-		}
-	}
-	CLAW_DBF_TEXT(4, trace, "PackSKBx");
-	return new_skb;
-}
-
-/*-------------------------------------------------------------------*
- *   claw_change_mtu                                                 *
- *                                                                   *
- *-------------------------------------------------------------------*/
-
-static int
-claw_change_mtu(struct net_device *dev, int new_mtu)
-{
-	struct claw_privbk *privptr = dev->ml_priv;
-	int buff_size;
-	CLAW_DBF_TEXT(4, trace, "setmtu");
-	buff_size = privptr->p_env->write_size;
-        if ((new_mtu < 60) || (new_mtu > buff_size)) {
-                return -EINVAL;
-        }
-        dev->mtu = new_mtu;
-        return 0;
-}  /*   end of claw_change_mtu */
-
-
-/*-------------------------------------------------------------------*
- *   claw_open                                                       *
- *                                                                   *
- *-------------------------------------------------------------------*/
-static int
-claw_open(struct net_device *dev)
-{
-
-        int     rc;
-        int     i;
-        unsigned long       saveflags=0;
-        unsigned long       parm;
-        struct claw_privbk  *privptr;
-	DECLARE_WAITQUEUE(wait, current);
-        struct timer_list  timer;
-        struct ccwbk *p_buf;
-
-	CLAW_DBF_TEXT(4, trace, "open");
-	privptr = (struct claw_privbk *)dev->ml_priv;
-        /*   allocate and initialize CCW blocks */
-	if (privptr->buffs_alloc == 0) {
-	        rc=init_ccw_bk(dev);
-        	if (rc) {
-			CLAW_DBF_TEXT(2, trace, "openmem");
-                	return -ENOMEM;
-        	}
-	}
-        privptr->system_validate_comp=0;
-        privptr->release_pend=0;
-	if(strncmp(privptr->p_env->api_type,WS_APPL_NAME_PACKED,6) == 0) {
-		privptr->p_env->read_size=DEF_PACK_BUFSIZE;
-		privptr->p_env->write_size=DEF_PACK_BUFSIZE;
-		privptr->p_env->packing=PACKING_ASK;
-	} else {
-		privptr->p_env->packing=0;
-		privptr->p_env->read_size=CLAW_FRAME_SIZE;
-		privptr->p_env->write_size=CLAW_FRAME_SIZE;
-	}
-        claw_set_busy(dev);
-	tasklet_init(&privptr->channel[READ_CHANNEL].tasklet, claw_irq_tasklet,
-		(unsigned long) &privptr->channel[READ_CHANNEL]);
-        for ( i = 0; i < 2;  i++) {
-		CLAW_DBF_TEXT_(2, trace, "opn_ch%d", i);
-                init_waitqueue_head(&privptr->channel[i].wait);
-		/* skb_queue_head_init(&p_ch->io_queue); */
-		if (i == WRITE_CHANNEL)
-			skb_queue_head_init(
-				&privptr->channel[WRITE_CHANNEL].collect_queue);
-                privptr->channel[i].flag_a = 0;
-                privptr->channel[i].IO_active = 0;
-                privptr->channel[i].flag  &= ~CLAW_TIMER;
-                init_timer(&timer);
-                timer.function = (void *)claw_timer;
-                timer.data = (unsigned long)(&privptr->channel[i]);
-                timer.expires = jiffies + 15*HZ;
-                add_timer(&timer);
-                spin_lock_irqsave(get_ccwdev_lock(
-			privptr->channel[i].cdev), saveflags);
-                parm = (unsigned long) &privptr->channel[i];
-                privptr->channel[i].claw_state = CLAW_START_HALT_IO;
-		rc = 0;
-		add_wait_queue(&privptr->channel[i].wait, &wait);
-                rc = ccw_device_halt(
-			(struct ccw_device *)privptr->channel[i].cdev,parm);
-                set_current_state(TASK_INTERRUPTIBLE);
-                spin_unlock_irqrestore(
-			get_ccwdev_lock(privptr->channel[i].cdev), saveflags);
-                schedule();
-                remove_wait_queue(&privptr->channel[i].wait, &wait);
-                if(rc != 0)
-                        ccw_check_return_code(privptr->channel[i].cdev, rc);
-                if((privptr->channel[i].flag & CLAW_TIMER) == 0x00)
-                        del_timer(&timer);
-        }
-	if ((((privptr->channel[READ_CHANNEL].last_dstat |
-		privptr->channel[WRITE_CHANNEL].last_dstat) &
-           ~(DEV_STAT_CHN_END | DEV_STAT_DEV_END)) != 0x00) ||
-	   (((privptr->channel[READ_CHANNEL].flag |
-		privptr->channel[WRITE_CHANNEL].flag) & CLAW_TIMER) != 0x00)) {
-		dev_info(&privptr->channel[READ_CHANNEL].cdev->dev,
-			"%s: remote side is not ready\n", dev->name);
-		CLAW_DBF_TEXT(2, trace, "notrdy");
-
-                for ( i = 0; i < 2;  i++) {
-                        spin_lock_irqsave(
-				get_ccwdev_lock(privptr->channel[i].cdev),
-				saveflags);
-                        parm = (unsigned long) &privptr->channel[i];
-                        privptr->channel[i].claw_state = CLAW_STOP;
-                        rc = ccw_device_halt(
-				(struct ccw_device *)&privptr->channel[i].cdev,
-				parm);
-                        spin_unlock_irqrestore(
-				get_ccwdev_lock(privptr->channel[i].cdev),
-				saveflags);
-                        if (rc != 0) {
-                                ccw_check_return_code(
-					privptr->channel[i].cdev, rc);
-                        }
-                }
-                free_pages((unsigned long)privptr->p_buff_ccw,
-			(int)pages_to_order_of_mag(privptr->p_buff_ccw_num));
-                if (privptr->p_env->read_size < PAGE_SIZE) {
-                        free_pages((unsigned long)privptr->p_buff_read,
-			       (int)pages_to_order_of_mag(
-			       		privptr->p_buff_read_num));
-                }
-                else {
-                        p_buf=privptr->p_read_active_first;
-                        while (p_buf!=NULL) {
-                                free_pages((unsigned long)p_buf->p_buffer,
-				      (int)pages_to_order_of_mag(
-				      	privptr->p_buff_pages_perread ));
-                                p_buf=p_buf->next;
-                        }
-                }
-                if (privptr->p_env->write_size < PAGE_SIZE ) {
-                        free_pages((unsigned long)privptr->p_buff_write,
-			     (int)pages_to_order_of_mag(
-			     	privptr->p_buff_write_num));
-                }
-                else {
-                        p_buf=privptr->p_write_active_first;
-                        while (p_buf!=NULL) {
-                                free_pages((unsigned long)p_buf->p_buffer,
-				     (int)pages_to_order_of_mag(
-				     	privptr->p_buff_pages_perwrite ));
-                                p_buf=p_buf->next;
-                        }
-                }
-		privptr->buffs_alloc = 0;
-		privptr->channel[READ_CHANNEL].flag = 0x00;
-		privptr->channel[WRITE_CHANNEL].flag = 0x00;
-                privptr->p_buff_ccw=NULL;
-                privptr->p_buff_read=NULL;
-                privptr->p_buff_write=NULL;
-                claw_clear_busy(dev);
-		CLAW_DBF_TEXT(2, trace, "open EIO");
-                return -EIO;
-        }
-
-        /*   Send SystemValidate command */
-
-        claw_clear_busy(dev);
-	CLAW_DBF_TEXT(4, trace, "openok");
-        return 0;
-}    /*     end of claw_open    */
-
-/*-------------------------------------------------------------------*
-*                                                                    *
-*       claw_irq_handler                                             *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static void
-claw_irq_handler(struct ccw_device *cdev,
-	unsigned long intparm, struct irb *irb)
-{
-        struct chbk *p_ch = NULL;
-        struct claw_privbk *privptr = NULL;
-        struct net_device *dev = NULL;
-        struct claw_env  *p_env;
-        struct chbk *p_ch_r=NULL;
-
-	CLAW_DBF_TEXT(4, trace, "clawirq");
-        /* Bypass all 'unsolicited interrupts' */
-	privptr = dev_get_drvdata(&cdev->dev);
-	if (!privptr) {
-		dev_warn(&cdev->dev, "An uninitialized CLAW device received an"
-			" IRQ, c-%02x d-%02x\n",
-			irb->scsw.cmd.cstat, irb->scsw.cmd.dstat);
-		CLAW_DBF_TEXT(2, trace, "badirq");
-                return;
-        }
-
-	/* Try to extract channel from driver data. */
-	if (privptr->channel[READ_CHANNEL].cdev == cdev)
-		p_ch = &privptr->channel[READ_CHANNEL];
-	else if (privptr->channel[WRITE_CHANNEL].cdev == cdev)
-		p_ch = &privptr->channel[WRITE_CHANNEL];
-	else {
-		dev_warn(&cdev->dev, "The device is not a CLAW device\n");
-		CLAW_DBF_TEXT(2, trace, "badchan");
-		return;
-	}
-	CLAW_DBF_TEXT_(4, trace, "IRQCH=%d", p_ch->flag);
-
-	dev = (struct net_device *) (p_ch->ndev);
-        p_env=privptr->p_env;
-
-	/* Copy interruption response block. */
-	memcpy(p_ch->irb, irb, sizeof(struct irb));
-
-	/* Check for good subchannel return code, otherwise info message */
-	if (irb->scsw.cmd.cstat && !(irb->scsw.cmd.cstat & SCHN_STAT_PCI)) {
-		dev_info(&cdev->dev,
-			"%s: subchannel check for device: %04x -"
-			" Sch Stat %02x  Dev Stat %02x CPA - %04x\n",
-                        dev->name, p_ch->devno,
-			irb->scsw.cmd.cstat, irb->scsw.cmd.dstat,
-			irb->scsw.cmd.cpa);
-		CLAW_DBF_TEXT(2, trace, "chanchk");
-                /* return; */
-        }
-
-        /* Check the reason-code of a unit check */
-	if (irb->scsw.cmd.dstat & DEV_STAT_UNIT_CHECK)
-                ccw_check_unit_check(p_ch, irb->ecw[0]);
-
-        /* State machine to bring the connection up, down and to restart */
-	p_ch->last_dstat = irb->scsw.cmd.dstat;
-
-        switch (p_ch->claw_state) {
-	case CLAW_STOP:/* HALT_IO by claw_release (halt sequence) */
-		if (!((p_ch->irb->scsw.cmd.stctl & SCSW_STCTL_SEC_STATUS) ||
-		(p_ch->irb->scsw.cmd.stctl == SCSW_STCTL_STATUS_PEND) ||
-		(p_ch->irb->scsw.cmd.stctl ==
-		(SCSW_STCTL_ALERT_STATUS | SCSW_STCTL_STATUS_PEND))))
-			return;
-		wake_up(&p_ch->wait);   /* wake up claw_release */
-		CLAW_DBF_TEXT(4, trace, "stop");
-		return;
-	case CLAW_START_HALT_IO: /* HALT_IO issued by claw_open  */
-		if (!((p_ch->irb->scsw.cmd.stctl & SCSW_STCTL_SEC_STATUS) ||
-		(p_ch->irb->scsw.cmd.stctl == SCSW_STCTL_STATUS_PEND) ||
-		(p_ch->irb->scsw.cmd.stctl ==
-		(SCSW_STCTL_ALERT_STATUS | SCSW_STCTL_STATUS_PEND)))) {
-			CLAW_DBF_TEXT(4, trace, "haltio");
-			return;
-		}
-		if (p_ch->flag == CLAW_READ) {
-			p_ch->claw_state = CLAW_START_READ;
-			wake_up(&p_ch->wait); /* wake claw_open (READ)*/
-		} else if (p_ch->flag == CLAW_WRITE) {
-			p_ch->claw_state = CLAW_START_WRITE;
-			/*      send SYSTEM_VALIDATE                    */
-			claw_strt_read(dev, LOCK_NO);
-			claw_send_control(dev,
-				SYSTEM_VALIDATE_REQUEST,
-				0, 0, 0,
-				p_env->host_name,
-				p_env->adapter_name);
-		} else {
-			dev_warn(&cdev->dev, "The CLAW device received"
-				" an unexpected IRQ, "
-				"c-%02x d-%02x\n",
-				irb->scsw.cmd.cstat,
-				irb->scsw.cmd.dstat);
-			return;
-			}
-		CLAW_DBF_TEXT(4, trace, "haltio");
-		return;
-	case CLAW_START_READ:
-		CLAW_DBF_TEXT(4, trace, "ReadIRQ");
-		if (p_ch->irb->scsw.cmd.dstat & DEV_STAT_UNIT_CHECK) {
-			clear_bit(0, (void *)&p_ch->IO_active);
-			if ((p_ch->irb->ecw[0] & 0x41) == 0x41 ||
-			    (p_ch->irb->ecw[0] & 0x40) == 0x40 ||
-			    (p_ch->irb->ecw[0])        == 0) {
-				privptr->stats.rx_errors++;
-				dev_info(&cdev->dev,
-					"%s: Restart is required after remote "
-					"side recovers \n",
-					dev->name);
-			}
-			CLAW_DBF_TEXT(4, trace, "notrdy");
-			return;
-		}
-		if ((p_ch->irb->scsw.cmd.cstat & SCHN_STAT_PCI) &&
-			(p_ch->irb->scsw.cmd.dstat == 0)) {
-			if (test_and_set_bit(CLAW_BH_ACTIVE,
-				(void *)&p_ch->flag_a) == 0)
-				tasklet_schedule(&p_ch->tasklet);
-			else
-				CLAW_DBF_TEXT(4, trace, "PCINoBH");
-			CLAW_DBF_TEXT(4, trace, "PCI_read");
-			return;
-		}
-		if (!((p_ch->irb->scsw.cmd.stctl & SCSW_STCTL_SEC_STATUS) ||
-		 (p_ch->irb->scsw.cmd.stctl == SCSW_STCTL_STATUS_PEND) ||
-		 (p_ch->irb->scsw.cmd.stctl ==
-		 (SCSW_STCTL_ALERT_STATUS | SCSW_STCTL_STATUS_PEND)))) {
-			CLAW_DBF_TEXT(4, trace, "SPend_rd");
-			return;
-		}
-		clear_bit(0, (void *)&p_ch->IO_active);
-		claw_clearbit_busy(TB_RETRY, dev);
-		if (test_and_set_bit(CLAW_BH_ACTIVE,
-			(void *)&p_ch->flag_a) == 0)
-			tasklet_schedule(&p_ch->tasklet);
-		else
-			CLAW_DBF_TEXT(4, trace, "RdBHAct");
-		CLAW_DBF_TEXT(4, trace, "RdIRQXit");
-		return;
-	case CLAW_START_WRITE:
-		if (p_ch->irb->scsw.cmd.dstat & DEV_STAT_UNIT_CHECK) {
-			dev_info(&cdev->dev,
-				"%s: Unit Check Occurred in "
-				"write channel\n", dev->name);
-			clear_bit(0, (void *)&p_ch->IO_active);
-			if (p_ch->irb->ecw[0] & 0x80) {
-				dev_info(&cdev->dev,
-					"%s: Resetting Event "
-					"occurred:\n", dev->name);
-				init_timer(&p_ch->timer);
-				p_ch->timer.function =
-					(void *)claw_write_retry;
-				p_ch->timer.data = (unsigned long)p_ch;
-				p_ch->timer.expires = jiffies + 10*HZ;
-				add_timer(&p_ch->timer);
-				dev_info(&cdev->dev,
-					"%s: write connection "
-					"restarting\n", dev->name);
-			}
-			CLAW_DBF_TEXT(4, trace, "rstrtwrt");
-			return;
-		}
-		if (p_ch->irb->scsw.cmd.dstat & DEV_STAT_UNIT_EXCEP) {
-			clear_bit(0, (void *)&p_ch->IO_active);
-			dev_info(&cdev->dev,
-				"%s: Unit Exception "
-				"occurred in write channel\n",
-				dev->name);
-		}
-		if (!((p_ch->irb->scsw.cmd.stctl & SCSW_STCTL_SEC_STATUS) ||
-		(p_ch->irb->scsw.cmd.stctl == SCSW_STCTL_STATUS_PEND) ||
-		(p_ch->irb->scsw.cmd.stctl ==
-		(SCSW_STCTL_ALERT_STATUS | SCSW_STCTL_STATUS_PEND)))) {
-			CLAW_DBF_TEXT(4, trace, "writeUE");
-			return;
-		}
-		clear_bit(0, (void *)&p_ch->IO_active);
-		if (claw_test_and_setbit_busy(TB_TX, dev) == 0) {
-			claw_write_next(p_ch);
-			claw_clearbit_busy(TB_TX, dev);
-			claw_clear_busy(dev);
-		}
-		p_ch_r = (struct chbk *)&privptr->channel[READ_CHANNEL];
-		if (test_and_set_bit(CLAW_BH_ACTIVE,
-			(void *)&p_ch_r->flag_a) == 0)
-			tasklet_schedule(&p_ch_r->tasklet);
-		CLAW_DBF_TEXT(4, trace, "StWtExit");
-		return;
-	default:
-		dev_warn(&cdev->dev,
-			"The CLAW device for %s received an unexpected IRQ\n",
-			 dev->name);
-		CLAW_DBF_TEXT(2, trace, "badIRQ");
-		return;
-        }
-
-}       /*   end of claw_irq_handler    */
-
-
-/*-------------------------------------------------------------------*
-*       claw_irq_tasklet                                             *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static void
-claw_irq_tasklet ( unsigned long data )
-{
-	struct chbk * p_ch;
-        struct net_device  *dev;
-
-	p_ch = (struct chbk *) data;
-        dev = (struct net_device *)p_ch->ndev;
-	CLAW_DBF_TEXT(4, trace, "IRQtask");
-        unpack_read(dev);
-        clear_bit(CLAW_BH_ACTIVE, (void *)&p_ch->flag_a);
-	CLAW_DBF_TEXT(4, trace, "TskletXt");
-        return;
-}       /*    end of claw_irq_bh    */
-
-/*-------------------------------------------------------------------*
-*       claw_release                                                 *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static int
-claw_release(struct net_device *dev)
-{
-        int                rc;
-        int                i;
-        unsigned long      saveflags;
-        unsigned long      parm;
-        struct claw_privbk *privptr;
-        DECLARE_WAITQUEUE(wait, current);
-        struct ccwbk*             p_this_ccw;
-        struct ccwbk*             p_buf;
-
-	if (!dev)
-                return 0;
-	privptr = (struct claw_privbk *)dev->ml_priv;
-        if (!privptr)
-                return 0;
-	CLAW_DBF_TEXT(4, trace, "release");
-        privptr->release_pend=1;
-        claw_setbit_busy(TB_STOP,dev);
-        for ( i = 1; i >=0 ;  i--) {
-                spin_lock_irqsave(
-			get_ccwdev_lock(privptr->channel[i].cdev), saveflags);
-	     /*   del_timer(&privptr->channel[READ_CHANNEL].timer);  */
- 		privptr->channel[i].claw_state = CLAW_STOP;
-                privptr->channel[i].IO_active = 0;
-                parm = (unsigned long) &privptr->channel[i];
-		if (i == WRITE_CHANNEL)
-			claw_purge_skb_queue(
-				&privptr->channel[WRITE_CHANNEL].collect_queue);
-                rc = ccw_device_halt (privptr->channel[i].cdev, parm);
-	        if (privptr->system_validate_comp==0x00)  /* never opened? */
-                   init_waitqueue_head(&privptr->channel[i].wait);
-                add_wait_queue(&privptr->channel[i].wait, &wait);
-                set_current_state(TASK_INTERRUPTIBLE);
-	        spin_unlock_irqrestore(
-			get_ccwdev_lock(privptr->channel[i].cdev), saveflags);
-	        schedule();
-	        remove_wait_queue(&privptr->channel[i].wait, &wait);
-	        if (rc != 0) {
-                        ccw_check_return_code(privptr->channel[i].cdev, rc);
-                }
-        }
-	if (privptr->pk_skb != NULL) {
-		dev_kfree_skb_any(privptr->pk_skb);
-		privptr->pk_skb = NULL;
-	}
-	if(privptr->buffs_alloc != 1) {
-		CLAW_DBF_TEXT(4, trace, "none2fre");
-		return 0;
-	}
-	CLAW_DBF_TEXT(4, trace, "freebufs");
-	if (privptr->p_buff_ccw != NULL) {
-        	free_pages((unsigned long)privptr->p_buff_ccw,
-	        	(int)pages_to_order_of_mag(privptr->p_buff_ccw_num));
-	}
-	CLAW_DBF_TEXT(4, trace, "freeread");
-        if (privptr->p_env->read_size < PAGE_SIZE) {
-	    if (privptr->p_buff_read != NULL) {
-                free_pages((unsigned long)privptr->p_buff_read,
-		      (int)pages_to_order_of_mag(privptr->p_buff_read_num));
-		}
-        }
-        else {
-                p_buf=privptr->p_read_active_first;
-                while (p_buf!=NULL) {
-                        free_pages((unsigned long)p_buf->p_buffer,
-			     (int)pages_to_order_of_mag(
-			     	privptr->p_buff_pages_perread ));
-                        p_buf=p_buf->next;
-                }
-        }
-	 CLAW_DBF_TEXT(4, trace, "freewrit");
-        if (privptr->p_env->write_size < PAGE_SIZE ) {
-                free_pages((unsigned long)privptr->p_buff_write,
-		      (int)pages_to_order_of_mag(privptr->p_buff_write_num));
-        }
-        else {
-                p_buf=privptr->p_write_active_first;
-                while (p_buf!=NULL) {
-                        free_pages((unsigned long)p_buf->p_buffer,
-			      (int)pages_to_order_of_mag(
-			      privptr->p_buff_pages_perwrite ));
-                        p_buf=p_buf->next;
-                }
-        }
-	 CLAW_DBF_TEXT(4, trace, "clearptr");
-	privptr->buffs_alloc = 0;
-        privptr->p_buff_ccw=NULL;
-        privptr->p_buff_read=NULL;
-        privptr->p_buff_write=NULL;
-        privptr->system_validate_comp=0;
-        privptr->release_pend=0;
-        /*      Remove any writes that were pending and reset all reads   */
-        p_this_ccw=privptr->p_read_active_first;
-        while (p_this_ccw!=NULL) {
-                p_this_ccw->header.length=0xffff;
-                p_this_ccw->header.opcode=0xff;
-                p_this_ccw->header.flag=0x00;
-                p_this_ccw=p_this_ccw->next;
-        }
-
-        while (privptr->p_write_active_first!=NULL) {
-                p_this_ccw=privptr->p_write_active_first;
-                p_this_ccw->header.flag=CLAW_PENDING;
-                privptr->p_write_active_first=p_this_ccw->next;
-                p_this_ccw->next=privptr->p_write_free_chain;
-                privptr->p_write_free_chain=p_this_ccw;
-                ++privptr->write_free_count;
-        }
-        privptr->p_write_active_last=NULL;
-        privptr->mtc_logical_link = -1;
-        privptr->mtc_skipping = 1;
-        privptr->mtc_offset=0;
-
-	if (((privptr->channel[READ_CHANNEL].last_dstat |
-		privptr->channel[WRITE_CHANNEL].last_dstat) &
-		~(DEV_STAT_CHN_END | DEV_STAT_DEV_END)) != 0x00) {
-		dev_warn(&privptr->channel[READ_CHANNEL].cdev->dev,
-			"Deactivating %s completed with incorrect"
-			" subchannel status "
-			"(read %02x, write %02x)\n",
-                dev->name,
-		privptr->channel[READ_CHANNEL].last_dstat,
-		privptr->channel[WRITE_CHANNEL].last_dstat);
-		 CLAW_DBF_TEXT(2, trace, "badclose");
-        }
-	CLAW_DBF_TEXT(4, trace, "rlsexit");
-        return 0;
-}      /* end of claw_release     */
-
-/*-------------------------------------------------------------------*
-*       claw_write_retry                                             *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static void
-claw_write_retry ( struct chbk *p_ch )
-{
-
-        struct net_device  *dev=p_ch->ndev;
-
-	CLAW_DBF_TEXT(4, trace, "w_retry");
-        if (p_ch->claw_state == CLAW_STOP) {
-        	return;
-        }
-	claw_strt_out_IO( dev );
-	CLAW_DBF_TEXT(4, trace, "rtry_xit");
-        return;
-}      /* end of claw_write_retry      */
-
-
-/*-------------------------------------------------------------------*
-*       claw_write_next                                              *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static void
-claw_write_next ( struct chbk * p_ch )
-{
-
-        struct net_device  *dev;
-        struct claw_privbk *privptr=NULL;
-	struct sk_buff *pk_skb;
-
-	CLAW_DBF_TEXT(4, trace, "claw_wrt");
-        if (p_ch->claw_state == CLAW_STOP)
-                return;
-        dev = (struct net_device *) p_ch->ndev;
-	privptr = (struct claw_privbk *) dev->ml_priv;
-        claw_free_wrt_buf( dev );
-	if ((privptr->write_free_count > 0) &&
-	    !skb_queue_empty(&p_ch->collect_queue)) {
-	  	pk_skb = claw_pack_skb(privptr);
-		while (pk_skb != NULL) {
-			claw_hw_tx(pk_skb, dev, 1);
-			if (privptr->write_free_count > 0) {
-	   			pk_skb = claw_pack_skb(privptr);
-			} else
-				pk_skb = NULL;
-		}
-	}
-        if (privptr->p_write_active_first!=NULL) {
-                claw_strt_out_IO(dev);
-        }
-        return;
-}      /* end of claw_write_next      */
-
-/*-------------------------------------------------------------------*
-*                                                                    *
-*       claw_timer                                                   *
-*--------------------------------------------------------------------*/
-
-static void
-claw_timer ( struct chbk * p_ch )
-{
-	CLAW_DBF_TEXT(4, trace, "timer");
-        p_ch->flag |= CLAW_TIMER;
-        wake_up(&p_ch->wait);
-        return;
-}      /* end of claw_timer  */
-
-/*
-*
-*       functions
-*/
-
-
-/*-------------------------------------------------------------------*
-*                                                                    *
-*     pages_to_order_of_mag                                          *
-*                                                                    *
-*    takes a number of pages from 1 to 512 and returns the           *
-*    log(num_pages)/log(2) get_free_pages() needs a base 2 order     *
-*    of magnitude get_free_pages() has an upper order of 9           *
-*--------------------------------------------------------------------*/
-
-static int
-pages_to_order_of_mag(int num_of_pages)
-{
-	int	order_of_mag=1;		/* assume 2 pages */
-	int	nump;
-
-	CLAW_DBF_TEXT_(5, trace, "pages%d", num_of_pages);
-	if (num_of_pages == 1)   {return 0; }  /* magnitude of 0 = 1 page */
-	/* 512 pages = 2Meg on 4k page systems */
-	if (num_of_pages >= 512) {return 9; }
-	/* we have two or more pages order is at least 1 */
-	for (nump=2 ;nump <= 512;nump*=2) {
-	  if (num_of_pages <= nump)
-		  break;
-	  order_of_mag +=1;
-	}
-	if (order_of_mag > 9) { order_of_mag = 9; }  /* I know it's paranoid */
-	CLAW_DBF_TEXT_(5, trace, "mag%d", order_of_mag);
-	return order_of_mag;
-}
-
-/*-------------------------------------------------------------------*
-*                                                                    *
-*     add_claw_reads                                                 *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static int
-add_claw_reads(struct net_device *dev, struct ccwbk* p_first,
-	struct ccwbk* p_last)
-{
-        struct claw_privbk *privptr;
-        struct ccw1  temp_ccw;
-        struct endccw * p_end;
-	CLAW_DBF_TEXT(4, trace, "addreads");
-	privptr = dev->ml_priv;
-        p_end = privptr->p_end_ccw;
-
-        /* first CCW and last CCW contains a new set of read channel programs
-        *       to apend the running channel programs
-        */
-        if ( p_first==NULL) {
-		CLAW_DBF_TEXT(4, trace, "addexit");
-                return 0;
-        }
-
-        /* set up ending CCW sequence for this segment */
-        if (p_end->read1) {
-                p_end->read1=0x00;    /*  second ending CCW is now active */
-                /*      reset ending CCWs and setup TIC CCWs              */
-                p_end->read2_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-                p_end->read2_nop2.flags  = CCW_FLAG_SLI | CCW_FLAG_SKIP;
-                p_last->r_TIC_1.cda =(__u32)__pa(&p_end->read2_nop1);
-                p_last->r_TIC_2.cda =(__u32)__pa(&p_end->read2_nop1);
-                p_end->read2_nop2.cda=0;
-                p_end->read2_nop2.count=1;
-        }
-        else {
-                p_end->read1=0x01;  /* first ending CCW is now active */
-                /*      reset ending CCWs and setup TIC CCWs          */
-                p_end->read1_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-                p_end->read1_nop2.flags  = CCW_FLAG_SLI | CCW_FLAG_SKIP;
-                p_last->r_TIC_1.cda = (__u32)__pa(&p_end->read1_nop1);
-                p_last->r_TIC_2.cda = (__u32)__pa(&p_end->read1_nop1);
-                p_end->read1_nop2.cda=0;
-                p_end->read1_nop2.count=1;
-        }
-
-        if ( privptr-> p_read_active_first ==NULL ) {
-		privptr->p_read_active_first = p_first;  /*  set new first */
-		privptr->p_read_active_last  = p_last;   /*  set new last  */
-        }
-        else {
-
-                /* set up TIC ccw  */
-                temp_ccw.cda= (__u32)__pa(&p_first->read);
-                temp_ccw.count=0;
-                temp_ccw.flags=0;
-                temp_ccw.cmd_code = CCW_CLAW_CMD_TIC;
-
-
-                if (p_end->read1) {
-
-               /* first set of CCW's is chained to the new read              */
-               /* chain, so the second set is chained to the active chain.   */
-               /* Therefore modify the second set to point to the new        */
-               /* read chain set up TIC CCWs                                 */
-               /* make sure we update the CCW so channel doesn't fetch it    */
-               /* when it's only half done                                   */
-                        memcpy( &p_end->read2_nop2, &temp_ccw ,
-				sizeof(struct ccw1));
-                        privptr->p_read_active_last->r_TIC_1.cda=
-				(__u32)__pa(&p_first->read);
-                        privptr->p_read_active_last->r_TIC_2.cda=
-				(__u32)__pa(&p_first->read);
-                }
-                else {
-                        /* make sure we update the CCW so channel doesn't   */
-			/* fetch it when it is only half done               */
-                        memcpy( &p_end->read1_nop2, &temp_ccw ,
-				sizeof(struct ccw1));
-                        privptr->p_read_active_last->r_TIC_1.cda=
-				(__u32)__pa(&p_first->read);
-                        privptr->p_read_active_last->r_TIC_2.cda=
-				(__u32)__pa(&p_first->read);
-                }
-		/*      chain in new set of blocks                         */
-                privptr->p_read_active_last->next = p_first;
-                privptr->p_read_active_last=p_last;
-        } /* end of if ( privptr-> p_read_active_first ==NULL)  */
-	CLAW_DBF_TEXT(4, trace, "addexit");
-        return 0;
-}    /*     end of add_claw_reads   */
-
-/*-------------------------------------------------------------------*
- *   ccw_check_return_code                                           *
- *                                                                   *
- *-------------------------------------------------------------------*/
-
-static void
-ccw_check_return_code(struct ccw_device *cdev, int return_code)
-{
-	CLAW_DBF_TEXT(4, trace, "ccwret");
-        if (return_code != 0) {
-                switch (return_code) {
-		case -EBUSY: /* BUSY is a transient state no action needed */
-			break;
-		case -ENODEV:
-			dev_err(&cdev->dev, "The remote channel adapter is not"
-				" available\n");
-			break;
-		case -EINVAL:
-			dev_err(&cdev->dev,
-				"The status of the remote channel adapter"
-				" is not valid\n");
-			break;
-		default:
-			dev_err(&cdev->dev, "The common device layer"
-				" returned error code %d\n",
-				  return_code);
-		}
-	}
-	CLAW_DBF_TEXT(4, trace, "ccwret");
-}    /*    end of ccw_check_return_code   */
-
-/*-------------------------------------------------------------------*
-*       ccw_check_unit_check                                         *
-*--------------------------------------------------------------------*/
-
-static void
-ccw_check_unit_check(struct chbk * p_ch, unsigned char sense )
-{
-	struct net_device *ndev = p_ch->ndev;
-	struct device *dev = &p_ch->cdev->dev;
-
-	CLAW_DBF_TEXT(4, trace, "unitchek");
-	dev_warn(dev, "The communication peer of %s disconnected\n",
-		ndev->name);
-
-	if (sense & 0x40) {
-		if (sense & 0x01) {
-			dev_warn(dev, "The remote channel adapter for"
-				" %s has been reset\n",
-				ndev->name);
-		}
-	} else if (sense & 0x20) {
-		if (sense & 0x04) {
-			dev_warn(dev, "A data streaming timeout occurred"
-				" for %s\n",
-				ndev->name);
-		} else if (sense & 0x10) {
-			dev_warn(dev, "The remote channel adapter for %s"
-				" is faulty\n",
-				ndev->name);
-		} else {
-			dev_warn(dev, "A data transfer parity error occurred"
-				" for %s\n",
-				ndev->name);
-		}
-	} else if (sense & 0x10) {
-		dev_warn(dev, "A read data parity error occurred"
-			" for %s\n",
-			ndev->name);
-	}
-
-}   /*    end of ccw_check_unit_check    */
-
-/*-------------------------------------------------------------------*
-*               find_link                                            *
-*--------------------------------------------------------------------*/
-static int
-find_link(struct net_device *dev, char *host_name, char *ws_name )
-{
-	struct claw_privbk *privptr;
-	struct claw_env *p_env;
-	int    rc=0;
-
-	CLAW_DBF_TEXT(2, setup, "findlink");
-	privptr = dev->ml_priv;
-        p_env=privptr->p_env;
-	switch (p_env->packing)
-	{
-		case  PACKING_ASK:
-			if ((memcmp(WS_APPL_NAME_PACKED, host_name, 8)!=0) ||
-			    (memcmp(WS_APPL_NAME_PACKED, ws_name, 8)!=0 ))
-        	             rc = EINVAL;
-			break;
-		case  DO_PACKED:
-		case  PACK_SEND:
-			if ((memcmp(WS_APPL_NAME_IP_NAME, host_name, 8)!=0) ||
-			    (memcmp(WS_APPL_NAME_IP_NAME, ws_name, 8)!=0 ))
-        	        	rc = EINVAL;
-			break;
-		default:
-	       		if ((memcmp(HOST_APPL_NAME, host_name, 8)!=0) ||
-		    	    (memcmp(p_env->api_type , ws_name, 8)!=0))
-        	        	rc = EINVAL;
-			break;
-	}
-
-	return rc;
-}    /*    end of find_link    */
-
-/*-------------------------------------------------------------------*
- *   claw_hw_tx                                                      *
- *                                                                   *
- *                                                                   *
- *-------------------------------------------------------------------*/
-
-static int
-claw_hw_tx(struct sk_buff *skb, struct net_device *dev, long linkid)
-{
-        int                             rc=0;
-        struct claw_privbk 		*privptr;
-        struct ccwbk           *p_this_ccw;
-        struct ccwbk           *p_first_ccw;
-        struct ccwbk           *p_last_ccw;
-        __u32                           numBuffers;
-        signed long                     len_of_data;
-        unsigned long                   bytesInThisBuffer;
-        unsigned char                   *pDataAddress;
-        struct endccw                   *pEnd;
-        struct ccw1                     tempCCW;
-	struct claw_env			*p_env;
-	struct clawph			*pk_head;
-	struct chbk			*ch;
-
-	CLAW_DBF_TEXT(4, trace, "hw_tx");
-	privptr = (struct claw_privbk *)(dev->ml_priv);
-	p_env =privptr->p_env;
-	claw_free_wrt_buf(dev);	/* Clean up free chain if posible */
-        /*  scan the write queue to free any completed write packets   */
-        p_first_ccw=NULL;
-        p_last_ccw=NULL;
-	if ((p_env->packing >= PACK_SEND) &&
-       	    (skb->cb[1] != 'P')) {
-		skb_push(skb,sizeof(struct clawph));
-		pk_head=(struct clawph *)skb->data;
-		pk_head->len=skb->len-sizeof(struct clawph);
-		if (pk_head->len%4)  {
-			pk_head->len+= 4-(pk_head->len%4);
-			skb_pad(skb,4-(pk_head->len%4));
-			skb_put(skb,4-(pk_head->len%4));
-		}
-		if (p_env->packing == DO_PACKED)
-			pk_head->link_num = linkid;
-		else
-			pk_head->link_num = 0;
-		pk_head->flag = 0x00;
-		skb_pad(skb,4);
-		skb->cb[1] = 'P';
-	}
-        if (linkid == 0) {
-        	if (claw_check_busy(dev)) {
-                	if (privptr->write_free_count!=0) {
-                                claw_clear_busy(dev);
-                        }
-                        else {
-                                claw_strt_out_IO(dev );
-                                claw_free_wrt_buf( dev );
-                                if (privptr->write_free_count==0) {
-					ch = &privptr->channel[WRITE_CHANNEL];
-					atomic_inc(&skb->users);
-					skb_queue_tail(&ch->collect_queue, skb);
-                                	goto Done;
-                                }
-                                else {
-                                	claw_clear_busy(dev);
-                                }
-                        }
-                }
-                /*  tx lock  */
-                if (claw_test_and_setbit_busy(TB_TX,dev)) { /* set to busy */
-			ch = &privptr->channel[WRITE_CHANNEL];
-			atomic_inc(&skb->users);
-			skb_queue_tail(&ch->collect_queue, skb);
-                        claw_strt_out_IO(dev );
-                        rc=-EBUSY;
-                        goto Done2;
-                }
-        }
-        /*      See how many write buffers are required to hold this data */
-	numBuffers = DIV_ROUND_UP(skb->len, privptr->p_env->write_size);
-
-        /*      If that number of buffers isn't available, give up for now */
-        if (privptr->write_free_count < numBuffers ||
-            privptr->p_write_free_chain == NULL ) {
-
-                claw_setbit_busy(TB_NOBUFFER,dev);
-		ch = &privptr->channel[WRITE_CHANNEL];
-		atomic_inc(&skb->users);
-		skb_queue_tail(&ch->collect_queue, skb);
-		CLAW_DBF_TEXT(2, trace, "clawbusy");
-                goto Done2;
-        }
-        pDataAddress=skb->data;
-        len_of_data=skb->len;
-
-        while (len_of_data > 0) {
-                p_this_ccw=privptr->p_write_free_chain;  /* get a block */
-		if (p_this_ccw == NULL) { /* lost the race */
-			ch = &privptr->channel[WRITE_CHANNEL];
-			atomic_inc(&skb->users);
-			skb_queue_tail(&ch->collect_queue, skb);
-			goto Done2;
-		}
-                privptr->p_write_free_chain=p_this_ccw->next;
-                p_this_ccw->next=NULL;
-                --privptr->write_free_count; /* -1 */
-		if (len_of_data >= privptr->p_env->write_size)
-			bytesInThisBuffer = privptr->p_env->write_size;
-		else
-			bytesInThisBuffer = len_of_data;
-                memcpy( p_this_ccw->p_buffer,pDataAddress, bytesInThisBuffer);
-                len_of_data-=bytesInThisBuffer;
-                pDataAddress+=(unsigned long)bytesInThisBuffer;
-                /*      setup write CCW         */
-                p_this_ccw->write.cmd_code = (linkid * 8) +1;
-                if (len_of_data>0) {
-                        p_this_ccw->write.cmd_code+=MORE_to_COME_FLAG;
-                }
-                p_this_ccw->write.count=bytesInThisBuffer;
-                /*      now add to end of this chain    */
-                if (p_first_ccw==NULL)    {
-                        p_first_ccw=p_this_ccw;
-                }
-                if (p_last_ccw!=NULL) {
-                        p_last_ccw->next=p_this_ccw;
-                        /*      set up TIC ccws         */
-                        p_last_ccw->w_TIC_1.cda=
-				(__u32)__pa(&p_this_ccw->write);
-                }
-                p_last_ccw=p_this_ccw;      /* save new last block */
-        }
-
-        /*      FirstCCW and LastCCW now contain a new set of write channel
-        *       programs to append to the running channel program
-        */
-
-        if (p_first_ccw!=NULL) {
-		/*      setup ending ccw sequence for this segment           */
-                pEnd=privptr->p_end_ccw;
-                if (pEnd->write1) {
-                        pEnd->write1=0x00;   /* second end ccw is now active */
-                        /*      set up Tic CCWs         */
-                        p_last_ccw->w_TIC_1.cda=
-				(__u32)__pa(&pEnd->write2_nop1);
-                        pEnd->write2_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-                        pEnd->write2_nop2.flags    =
-				CCW_FLAG_SLI | CCW_FLAG_SKIP;
-                        pEnd->write2_nop2.cda=0;
-                        pEnd->write2_nop2.count=1;
-                }
-                else {  /*  end of if (pEnd->write1)*/
-                        pEnd->write1=0x01;   /* first end ccw is now active */
-                        /*      set up Tic CCWs         */
-                        p_last_ccw->w_TIC_1.cda=
-				(__u32)__pa(&pEnd->write1_nop1);
-                        pEnd->write1_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-                        pEnd->write1_nop2.flags    =
-				CCW_FLAG_SLI | CCW_FLAG_SKIP;
-                        pEnd->write1_nop2.cda=0;
-                        pEnd->write1_nop2.count=1;
-                }  /* end if if (pEnd->write1) */
-
-                if (privptr->p_write_active_first==NULL ) {
-                        privptr->p_write_active_first=p_first_ccw;
-                        privptr->p_write_active_last=p_last_ccw;
-                }
-                else {
-                        /*      set up Tic CCWs         */
-
-                        tempCCW.cda=(__u32)__pa(&p_first_ccw->write);
-                        tempCCW.count=0;
-                        tempCCW.flags=0;
-                        tempCCW.cmd_code=CCW_CLAW_CMD_TIC;
-
-                        if (pEnd->write1) {
-
-                 /*
-                 * first set of ending CCW's is chained to the new write
-                 * chain, so the second set is chained to the active chain
-                 * Therefore modify the second set to point the new write chain.
-                 * make sure we update the CCW atomically
-                 * so channel does not fetch it when it's only half done
-                 */
-                                memcpy( &pEnd->write2_nop2, &tempCCW ,
-					sizeof(struct ccw1));
-                                privptr->p_write_active_last->w_TIC_1.cda=
-					(__u32)__pa(&p_first_ccw->write);
-                        }
-                        else {
-
-                        /*make sure we update the CCW atomically
-                         *so channel does not fetch it when it's only half done
-                         */
-                                memcpy(&pEnd->write1_nop2, &tempCCW ,
-					sizeof(struct ccw1));
-                                privptr->p_write_active_last->w_TIC_1.cda=
-				        (__u32)__pa(&p_first_ccw->write);
-
-                        } /* end if if (pEnd->write1) */
-
-                        privptr->p_write_active_last->next=p_first_ccw;
-                        privptr->p_write_active_last=p_last_ccw;
-                }
-
-        } /* endif (p_first_ccw!=NULL)  */
-        dev_kfree_skb_any(skb);
-        claw_strt_out_IO(dev );
-        /*      if write free count is zero , set NOBUFFER       */
-	if (privptr->write_free_count==0) {
-		claw_setbit_busy(TB_NOBUFFER,dev);
-        }
-Done2:
-	claw_clearbit_busy(TB_TX,dev);
-Done:
-	return(rc);
-}    /*    end of claw_hw_tx    */
-
-/*-------------------------------------------------------------------*
-*                                                                    *
-*     init_ccw_bk                                                    *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static int
-init_ccw_bk(struct net_device *dev)
-{
-
-        __u32   ccw_blocks_required;
-        __u32   ccw_blocks_perpage;
-        __u32   ccw_pages_required;
-        __u32   claw_reads_perpage=1;
-        __u32   claw_read_pages;
-        __u32   claw_writes_perpage=1;
-        __u32   claw_write_pages;
-        void    *p_buff=NULL;
-        struct ccwbk*p_free_chain;
-	struct ccwbk*p_buf;
-	struct ccwbk*p_last_CCWB;
-	struct ccwbk*p_first_CCWB;
-        struct endccw *p_endccw=NULL;
-	addr_t  real_address;
-	struct claw_privbk *privptr = dev->ml_priv;
-        struct clawh *pClawH=NULL;
-        addr_t   real_TIC_address;
-        int i,j;
-	CLAW_DBF_TEXT(4, trace, "init_ccw");
-
-        /*  initialize  statistics field */
-        privptr->active_link_ID=0;
-        /*  initialize  ccwbk pointers  */
-        privptr->p_write_free_chain=NULL;   /* pointer to free ccw chain*/
-        privptr->p_write_active_first=NULL; /* pointer to the first write ccw*/
-        privptr->p_write_active_last=NULL;  /* pointer to the last write ccw*/
-        privptr->p_read_active_first=NULL;  /* pointer to the first read ccw*/
-        privptr->p_read_active_last=NULL;   /* pointer to the last read ccw */
-        privptr->p_end_ccw=NULL;            /* pointer to ending ccw        */
-        privptr->p_claw_signal_blk=NULL;    /* pointer to signal block      */
-	privptr->buffs_alloc = 0;
-        memset(&privptr->end_ccw, 0x00, sizeof(struct endccw));
-        memset(&privptr->ctl_bk, 0x00, sizeof(struct clawctl));
-        /*  initialize  free write ccwbk counter  */
-        privptr->write_free_count=0;  /* number of free bufs on write chain */
-        p_last_CCWB = NULL;
-        p_first_CCWB= NULL;
-        /*
-        *  We need 1 CCW block for each read buffer, 1 for each
-        *  write buffer, plus 1 for ClawSignalBlock
-        */
-        ccw_blocks_required =
-		privptr->p_env->read_buffers+privptr->p_env->write_buffers+1;
-        /*
-        * compute number of CCW blocks that will fit in a page
-        */
-        ccw_blocks_perpage= PAGE_SIZE /  CCWBK_SIZE;
-        ccw_pages_required=
-		DIV_ROUND_UP(ccw_blocks_required, ccw_blocks_perpage);
-
-        /*
-         *  read and write sizes are set by 2 constants in claw.h
-	 *  4k and 32k.  Unpacked values other than 4k are not going to
-	 * provide good performance. With packing buffers support 32k
-	 * buffers are used.
-         */
-	if (privptr->p_env->read_size < PAGE_SIZE) {
-		claw_reads_perpage = PAGE_SIZE / privptr->p_env->read_size;
-		claw_read_pages = DIV_ROUND_UP(privptr->p_env->read_buffers,
-						claw_reads_perpage);
-         }
-         else {       /* > or equal  */
-		privptr->p_buff_pages_perread =
-			DIV_ROUND_UP(privptr->p_env->read_size, PAGE_SIZE);
-		claw_read_pages = privptr->p_env->read_buffers *
-					privptr->p_buff_pages_perread;
-         }
-        if (privptr->p_env->write_size < PAGE_SIZE) {
-		claw_writes_perpage =
-			PAGE_SIZE / privptr->p_env->write_size;
-		claw_write_pages = DIV_ROUND_UP(privptr->p_env->write_buffers,
-						claw_writes_perpage);
-
-        }
-        else {      /* >  or equal  */
-		privptr->p_buff_pages_perwrite =
-			DIV_ROUND_UP(privptr->p_env->read_size, PAGE_SIZE);
-		claw_write_pages = privptr->p_env->write_buffers *
-					privptr->p_buff_pages_perwrite;
-        }
-        /*
-        *               allocate ccw_pages_required
-        */
-        if (privptr->p_buff_ccw==NULL) {
-                privptr->p_buff_ccw=
-			(void *)__get_free_pages(__GFP_DMA,
-		        (int)pages_to_order_of_mag(ccw_pages_required ));
-                if (privptr->p_buff_ccw==NULL) {
-                        return -ENOMEM;
-                }
-                privptr->p_buff_ccw_num=ccw_pages_required;
-        }
-        memset(privptr->p_buff_ccw, 0x00,
-		privptr->p_buff_ccw_num * PAGE_SIZE);
-
-        /*
-        *               obtain ending ccw block address
-        *
-        */
-        privptr->p_end_ccw = (struct endccw *)&privptr->end_ccw;
-        real_address  = (__u32)__pa(privptr->p_end_ccw);
-        /*                              Initialize ending CCW block       */
-        p_endccw=privptr->p_end_ccw;
-        p_endccw->real=real_address;
-        p_endccw->write1=0x00;
-        p_endccw->read1=0x00;
-
-        /*      write1_nop1                                     */
-        p_endccw->write1_nop1.cmd_code = CCW_CLAW_CMD_NOP;
-        p_endccw->write1_nop1.flags       = CCW_FLAG_SLI | CCW_FLAG_CC;
-        p_endccw->write1_nop1.count       = 1;
-        p_endccw->write1_nop1.cda         = 0;
-
-        /*      write1_nop2                                     */
-        p_endccw->write1_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-        p_endccw->write1_nop2.flags        = CCW_FLAG_SLI | CCW_FLAG_SKIP;
-        p_endccw->write1_nop2.count      = 1;
-        p_endccw->write1_nop2.cda        = 0;
-
-        /*      write2_nop1                                     */
-        p_endccw->write2_nop1.cmd_code = CCW_CLAW_CMD_NOP;
-        p_endccw->write2_nop1.flags        = CCW_FLAG_SLI | CCW_FLAG_CC;
-        p_endccw->write2_nop1.count        = 1;
-        p_endccw->write2_nop1.cda          = 0;
-
-        /*      write2_nop2                                     */
-        p_endccw->write2_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-        p_endccw->write2_nop2.flags        = CCW_FLAG_SLI | CCW_FLAG_SKIP;
-        p_endccw->write2_nop2.count        = 1;
-        p_endccw->write2_nop2.cda          = 0;
-
-        /*      read1_nop1                                      */
-        p_endccw->read1_nop1.cmd_code = CCW_CLAW_CMD_NOP;
-        p_endccw->read1_nop1.flags        = CCW_FLAG_SLI | CCW_FLAG_CC;
-        p_endccw->read1_nop1.count        = 1;
-        p_endccw->read1_nop1.cda          = 0;
-
-        /*      read1_nop2                                      */
-        p_endccw->read1_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-        p_endccw->read1_nop2.flags        = CCW_FLAG_SLI | CCW_FLAG_SKIP;
-        p_endccw->read1_nop2.count        = 1;
-        p_endccw->read1_nop2.cda          = 0;
-
-        /*      read2_nop1                                      */
-        p_endccw->read2_nop1.cmd_code = CCW_CLAW_CMD_NOP;
-        p_endccw->read2_nop1.flags        = CCW_FLAG_SLI | CCW_FLAG_CC;
-        p_endccw->read2_nop1.count        = 1;
-        p_endccw->read2_nop1.cda          = 0;
-
-        /*      read2_nop2                                      */
-        p_endccw->read2_nop2.cmd_code = CCW_CLAW_CMD_READFF;
-        p_endccw->read2_nop2.flags        = CCW_FLAG_SLI | CCW_FLAG_SKIP;
-        p_endccw->read2_nop2.count        = 1;
-        p_endccw->read2_nop2.cda          = 0;
-
-        /*
-        *                               Build a chain of CCWs
-        *
-        */
-        p_buff=privptr->p_buff_ccw;
-
-        p_free_chain=NULL;
-        for (i=0 ; i < ccw_pages_required; i++ ) {
-                real_address  = (__u32)__pa(p_buff);
-                p_buf=p_buff;
-                for (j=0 ; j < ccw_blocks_perpage ; j++) {
-                        p_buf->next  = p_free_chain;
-                        p_free_chain = p_buf;
-                        p_buf->real=(__u32)__pa(p_buf);
-                        ++p_buf;
-                }
-                p_buff+=PAGE_SIZE;
-        }
-        /*
-        *                               Initialize ClawSignalBlock
-        *
-        */
-        if (privptr->p_claw_signal_blk==NULL) {
-                privptr->p_claw_signal_blk=p_free_chain;
-                p_free_chain=p_free_chain->next;
-                pClawH=(struct clawh *)privptr->p_claw_signal_blk;
-                pClawH->length=0xffff;
-                pClawH->opcode=0xff;
-                pClawH->flag=CLAW_BUSY;
-        }
-
-        /*
-        *               allocate write_pages_required and add to free chain
-        */
-        if (privptr->p_buff_write==NULL) {
-            if (privptr->p_env->write_size < PAGE_SIZE) {
-                privptr->p_buff_write=
-			(void *)__get_free_pages(__GFP_DMA,
-			(int)pages_to_order_of_mag(claw_write_pages ));
-                if (privptr->p_buff_write==NULL) {
-                        privptr->p_buff_ccw=NULL;
-                        return -ENOMEM;
-                }
-                /*
-                *                               Build CLAW write free chain
-                *
-                */
-
-                memset(privptr->p_buff_write, 0x00,
-			ccw_pages_required * PAGE_SIZE);
-                privptr->p_write_free_chain=NULL;
-
-                p_buff=privptr->p_buff_write;
-
-                for (i=0 ; i< privptr->p_env->write_buffers ; i++) {
-                        p_buf        = p_free_chain;      /*  get a CCW */
-                        p_free_chain = p_buf->next;
-                        p_buf->next  =privptr->p_write_free_chain;
-                        privptr->p_write_free_chain = p_buf;
-                        p_buf-> p_buffer	= (struct clawbuf *)p_buff;
-                        p_buf-> write.cda       = (__u32)__pa(p_buff);
-                        p_buf-> write.flags     = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> w_read_FF.cmd_code = CCW_CLAW_CMD_READFF;
-                        p_buf-> w_read_FF.flags   = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> w_read_FF.count   = 1;
-                        p_buf-> w_read_FF.cda     =
-				(__u32)__pa(&p_buf-> header.flag);
-                        p_buf-> w_TIC_1.cmd_code = CCW_CLAW_CMD_TIC;
-                        p_buf-> w_TIC_1.flags      = 0;
-                        p_buf-> w_TIC_1.count      = 0;
-
-			if (((unsigned long)p_buff +
-					    privptr->p_env->write_size) >=
-			   ((unsigned long)(p_buff+2*
-			    (privptr->p_env->write_size) - 1) & PAGE_MASK)) {
-				p_buff = p_buff+privptr->p_env->write_size;
-                        }
-                }
-           }
-           else      /*  Buffers are => PAGE_SIZE. 1 buff per get_free_pages */
-           {
-               privptr->p_write_free_chain=NULL;
-               for (i = 0; i< privptr->p_env->write_buffers ; i++) {
-                   p_buff=(void *)__get_free_pages(__GFP_DMA,
-		        (int)pages_to_order_of_mag(
-			privptr->p_buff_pages_perwrite) );
-                   if (p_buff==NULL) {
-                        free_pages((unsigned long)privptr->p_buff_ccw,
-			      (int)pages_to_order_of_mag(
-			      		privptr->p_buff_ccw_num));
-                        privptr->p_buff_ccw=NULL;
-			p_buf=privptr->p_buff_write;
-                        while (p_buf!=NULL) {
-                                free_pages((unsigned long)
-					p_buf->p_buffer,
-					(int)pages_to_order_of_mag(
-					privptr->p_buff_pages_perwrite));
-                                p_buf=p_buf->next;
-                        }
-                        return -ENOMEM;
-                   }  /* Error on get_pages   */
-                   memset(p_buff, 0x00, privptr->p_env->write_size );
-                   p_buf         = p_free_chain;
-                   p_free_chain  = p_buf->next;
-                   p_buf->next   = privptr->p_write_free_chain;
-                   privptr->p_write_free_chain = p_buf;
-                   privptr->p_buff_write = p_buf;
-                   p_buf->p_buffer=(struct clawbuf *)p_buff;
-                   p_buf-> write.cda     = (__u32)__pa(p_buff);
-                   p_buf-> write.flags   = CCW_FLAG_SLI | CCW_FLAG_CC;
-                   p_buf-> w_read_FF.cmd_code = CCW_CLAW_CMD_READFF;
-                   p_buf-> w_read_FF.flags    = CCW_FLAG_SLI | CCW_FLAG_CC;
-                   p_buf-> w_read_FF.count    = 1;
-                   p_buf-> w_read_FF.cda      =
-			(__u32)__pa(&p_buf-> header.flag);
-                   p_buf-> w_TIC_1.cmd_code = CCW_CLAW_CMD_TIC;
-                   p_buf-> w_TIC_1.flags   = 0;
-                   p_buf-> w_TIC_1.count   = 0;
-               }  /* for all write_buffers   */
-
-           }    /* else buffers are PAGE_SIZE or bigger */
-
-        }
-        privptr->p_buff_write_num=claw_write_pages;
-        privptr->write_free_count=privptr->p_env->write_buffers;
-
-
-        /*
-        *               allocate read_pages_required and chain to free chain
-        */
-        if (privptr->p_buff_read==NULL) {
-            if (privptr->p_env->read_size < PAGE_SIZE)  {
-                privptr->p_buff_read=
-			(void *)__get_free_pages(__GFP_DMA,
-			(int)pages_to_order_of_mag(claw_read_pages) );
-                if (privptr->p_buff_read==NULL) {
-                        free_pages((unsigned long)privptr->p_buff_ccw,
-				(int)pages_to_order_of_mag(
-					privptr->p_buff_ccw_num));
-			/* free the write pages size is < page size  */
-                        free_pages((unsigned long)privptr->p_buff_write,
-				(int)pages_to_order_of_mag(
-				privptr->p_buff_write_num));
-                        privptr->p_buff_ccw=NULL;
-                        privptr->p_buff_write=NULL;
-                        return -ENOMEM;
-                }
-                memset(privptr->p_buff_read, 0x00, claw_read_pages * PAGE_SIZE);
-                privptr->p_buff_read_num=claw_read_pages;
-                /*
-                *                               Build CLAW read free chain
-                *
-                */
-                p_buff=privptr->p_buff_read;
-                for (i=0 ; i< privptr->p_env->read_buffers ; i++) {
-                        p_buf        = p_free_chain;
-                        p_free_chain = p_buf->next;
-
-                        if (p_last_CCWB==NULL) {
-                                p_buf->next=NULL;
-                                real_TIC_address=0;
-                                p_last_CCWB=p_buf;
-                        }
-                        else {
-                                p_buf->next=p_first_CCWB;
-                                real_TIC_address=
-				(__u32)__pa(&p_first_CCWB -> read );
-                        }
-
-                        p_first_CCWB=p_buf;
-
-                        p_buf->p_buffer=(struct clawbuf *)p_buff;
-                        /*  initialize read command */
-                        p_buf-> read.cmd_code = CCW_CLAW_CMD_READ;
-                        p_buf-> read.cda = (__u32)__pa(p_buff);
-                        p_buf-> read.flags = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> read.count       = privptr->p_env->read_size;
-
-                        /*  initialize read_h command */
-                        p_buf-> read_h.cmd_code = CCW_CLAW_CMD_READHEADER;
-                        p_buf-> read_h.cda =
-				(__u32)__pa(&(p_buf->header));
-                        p_buf-> read_h.flags = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> read_h.count      = sizeof(struct clawh);
-
-                        /*  initialize Signal command */
-                        p_buf-> signal.cmd_code = CCW_CLAW_CMD_SIGNAL_SMOD;
-                        p_buf-> signal.cda =
-				(__u32)__pa(&(pClawH->flag));
-                        p_buf-> signal.flags = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> signal.count     = 1;
-
-                        /*  initialize r_TIC_1 command */
-                        p_buf-> r_TIC_1.cmd_code = CCW_CLAW_CMD_TIC;
-                        p_buf-> r_TIC_1.cda = (__u32)real_TIC_address;
-                        p_buf-> r_TIC_1.flags = 0;
-                        p_buf-> r_TIC_1.count      = 0;
-
-                        /*  initialize r_read_FF command */
-                        p_buf-> r_read_FF.cmd_code = CCW_CLAW_CMD_READFF;
-                        p_buf-> r_read_FF.cda =
-				(__u32)__pa(&(pClawH->flag));
-                        p_buf-> r_read_FF.flags =
-				CCW_FLAG_SLI | CCW_FLAG_CC | CCW_FLAG_PCI;
-                        p_buf-> r_read_FF.count    = 1;
-
-                        /*    initialize r_TIC_2          */
-                        memcpy(&p_buf->r_TIC_2,
-				&p_buf->r_TIC_1, sizeof(struct ccw1));
-
-                        /*     initialize Header     */
-                        p_buf->header.length=0xffff;
-                        p_buf->header.opcode=0xff;
-                        p_buf->header.flag=CLAW_PENDING;
-
-			if (((unsigned long)p_buff+privptr->p_env->read_size) >=
-			  ((unsigned long)(p_buff+2*(privptr->p_env->read_size)
-				 -1)
-			   & PAGE_MASK)) {
-                                p_buff= p_buff+privptr->p_env->read_size;
-                        }
-                        else {
-                                p_buff=
-				(void *)((unsigned long)
-					(p_buff+2*(privptr->p_env->read_size)-1)
-					 & PAGE_MASK) ;
-                        }
-                }   /* for read_buffers   */
-          }         /* read_size < PAGE_SIZE  */
-          else {  /* read Size >= PAGE_SIZE  */
-                for (i=0 ; i< privptr->p_env->read_buffers ; i++) {
-                        p_buff = (void *)__get_free_pages(__GFP_DMA,
-				(int)pages_to_order_of_mag(
-					privptr->p_buff_pages_perread));
-                        if (p_buff==NULL) {
-                                free_pages((unsigned long)privptr->p_buff_ccw,
-					(int)pages_to_order_of_mag(privptr->
-					p_buff_ccw_num));
-				/* free the write pages  */
-	                        p_buf=privptr->p_buff_write;
-                                while (p_buf!=NULL) {
-					free_pages(
-					    (unsigned long)p_buf->p_buffer,
-					    (int)pages_to_order_of_mag(
-					    privptr->p_buff_pages_perwrite));
-                                        p_buf=p_buf->next;
-                                }
-				/* free any read pages already alloc  */
-	                        p_buf=privptr->p_buff_read;
-                                while (p_buf!=NULL) {
-					free_pages(
-					    (unsigned long)p_buf->p_buffer,
-					    (int)pages_to_order_of_mag(
-					     privptr->p_buff_pages_perread));
-                                        p_buf=p_buf->next;
-                                }
-                                privptr->p_buff_ccw=NULL;
-                                privptr->p_buff_write=NULL;
-                                return -ENOMEM;
-                        }
-                        memset(p_buff, 0x00, privptr->p_env->read_size);
-                        p_buf        = p_free_chain;
-                        privptr->p_buff_read = p_buf;
-                        p_free_chain = p_buf->next;
-
-                        if (p_last_CCWB==NULL) {
-                                p_buf->next=NULL;
-                                real_TIC_address=0;
-                                p_last_CCWB=p_buf;
-                        }
-                        else {
-                                p_buf->next=p_first_CCWB;
-                                real_TIC_address=
-					(addr_t)__pa(
-						&p_first_CCWB -> read );
-                        }
-
-                        p_first_CCWB=p_buf;
-				/* save buff address */
-                        p_buf->p_buffer=(struct clawbuf *)p_buff;
-                        /*  initialize read command */
-                        p_buf-> read.cmd_code = CCW_CLAW_CMD_READ;
-                        p_buf-> read.cda = (__u32)__pa(p_buff);
-                        p_buf-> read.flags = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> read.count       = privptr->p_env->read_size;
-
-                        /*  initialize read_h command */
-                        p_buf-> read_h.cmd_code = CCW_CLAW_CMD_READHEADER;
-                        p_buf-> read_h.cda =
-				(__u32)__pa(&(p_buf->header));
-                        p_buf-> read_h.flags = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> read_h.count      = sizeof(struct clawh);
-
-                        /*  initialize Signal command */
-                        p_buf-> signal.cmd_code = CCW_CLAW_CMD_SIGNAL_SMOD;
-                        p_buf-> signal.cda =
-				(__u32)__pa(&(pClawH->flag));
-                        p_buf-> signal.flags = CCW_FLAG_SLI | CCW_FLAG_CC;
-                        p_buf-> signal.count     = 1;
-
-                        /*  initialize r_TIC_1 command */
-                        p_buf-> r_TIC_1.cmd_code = CCW_CLAW_CMD_TIC;
-                        p_buf-> r_TIC_1.cda = (__u32)real_TIC_address;
-                        p_buf-> r_TIC_1.flags = 0;
-                        p_buf-> r_TIC_1.count      = 0;
-
-                        /*  initialize r_read_FF command */
-                        p_buf-> r_read_FF.cmd_code = CCW_CLAW_CMD_READFF;
-                        p_buf-> r_read_FF.cda =
-				(__u32)__pa(&(pClawH->flag));
-                        p_buf-> r_read_FF.flags =
-				CCW_FLAG_SLI | CCW_FLAG_CC | CCW_FLAG_PCI;
-                        p_buf-> r_read_FF.count    = 1;
-
-                        /*    initialize r_TIC_2          */
-                        memcpy(&p_buf->r_TIC_2, &p_buf->r_TIC_1,
-				sizeof(struct ccw1));
-
-                        /*     initialize Header     */
-                        p_buf->header.length=0xffff;
-                        p_buf->header.opcode=0xff;
-                        p_buf->header.flag=CLAW_PENDING;
-
-                }    /* For read_buffers   */
-          }     /*  read_size >= PAGE_SIZE   */
-        }       /*  pBuffread = NULL */
-        add_claw_reads( dev  ,p_first_CCWB , p_last_CCWB);
-	privptr->buffs_alloc = 1;
-
-        return 0;
-}    /*    end of init_ccw_bk */
-
-/*-------------------------------------------------------------------*
-*                                                                    *
-*       probe_error                                                  *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static void
-probe_error( struct ccwgroup_device *cgdev)
-{
-	struct claw_privbk *privptr;
-
-	CLAW_DBF_TEXT(4, trace, "proberr");
-	privptr = dev_get_drvdata(&cgdev->dev);
-	if (privptr != NULL) {
-		dev_set_drvdata(&cgdev->dev, NULL);
-		kfree(privptr->p_env);
-		kfree(privptr->p_mtc_envelope);
-		kfree(privptr);
-	}
-}    /*    probe_error    */
-
-/*-------------------------------------------------------------------*
-*    claw_process_control                                            *
-*                                                                    *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static int
-claw_process_control( struct net_device *dev, struct ccwbk * p_ccw)
-{
-
-        struct clawbuf *p_buf;
-        struct clawctl  ctlbk;
-        struct clawctl *p_ctlbk;
-        char    temp_host_name[8];
-        char    temp_ws_name[8];
-        struct claw_privbk *privptr;
-        struct claw_env *p_env;
-        struct sysval *p_sysval;
-        struct conncmd *p_connect=NULL;
-        int rc;
-        struct chbk *p_ch = NULL;
-	struct device *tdev;
-	CLAW_DBF_TEXT(2, setup, "clw_cntl");
-        udelay(1000);  /* Wait a ms for the control packets to
-			*catch up to each other */
-	privptr = dev->ml_priv;
-        p_env=privptr->p_env;
-	tdev = &privptr->channel[READ_CHANNEL].cdev->dev;
-	memcpy( &temp_host_name, p_env->host_name, 8);
-        memcpy( &temp_ws_name, p_env->adapter_name , 8);
-	dev_info(tdev, "%s: CLAW device %.8s: "
-		"Received Control Packet\n",
-		dev->name, temp_ws_name);
-        if (privptr->release_pend==1) {
-                return 0;
-        }
-        p_buf=p_ccw->p_buffer;
-        p_ctlbk=&ctlbk;
-	if (p_env->packing == DO_PACKED) { /* packing in progress?*/
-		memcpy(p_ctlbk, &p_buf->buffer[4], sizeof(struct clawctl));
-	} else {
-		memcpy(p_ctlbk, p_buf, sizeof(struct clawctl));
-	}
-        switch (p_ctlbk->command)
-        {
-	case SYSTEM_VALIDATE_REQUEST:
-		if (p_ctlbk->version != CLAW_VERSION_ID) {
-			claw_snd_sys_validate_rsp(dev, p_ctlbk,
-				CLAW_RC_WRONG_VERSION);
-			dev_warn(tdev, "The communication peer of %s"
-				" uses an incorrect API version %d\n",
-				dev->name, p_ctlbk->version);
-		}
-		p_sysval = (struct sysval *)&(p_ctlbk->data);
-		dev_info(tdev, "%s: Recv Sys Validate Request: "
-			"Vers=%d,link_id=%d,Corr=%d,WS name=%.8s,"
-			"Host name=%.8s\n",
-			dev->name, p_ctlbk->version,
-			p_ctlbk->linkid,
-			p_ctlbk->correlator,
-			p_sysval->WS_name,
-			p_sysval->host_name);
-		if (memcmp(temp_host_name, p_sysval->host_name, 8)) {
-			claw_snd_sys_validate_rsp(dev, p_ctlbk,
-				CLAW_RC_NAME_MISMATCH);
-			CLAW_DBF_TEXT(2, setup, "HSTBAD");
-			CLAW_DBF_TEXT_(2, setup, "%s", p_sysval->host_name);
-			CLAW_DBF_TEXT_(2, setup, "%s", temp_host_name);
-			dev_warn(tdev,
-				"Host name %s for %s does not match the"
-				" remote adapter name %s\n",
-				p_sysval->host_name,
-				dev->name,
-				temp_host_name);
-		}
-		if (memcmp(temp_ws_name, p_sysval->WS_name, 8)) {
-			claw_snd_sys_validate_rsp(dev, p_ctlbk,
-				CLAW_RC_NAME_MISMATCH);
-			CLAW_DBF_TEXT(2, setup, "WSNBAD");
-			CLAW_DBF_TEXT_(2, setup, "%s", p_sysval->WS_name);
-			CLAW_DBF_TEXT_(2, setup, "%s", temp_ws_name);
-			dev_warn(tdev, "Adapter name %s for %s does not match"
-				" the remote host name %s\n",
-				p_sysval->WS_name,
-				dev->name,
-				temp_ws_name);
-		}
-		if ((p_sysval->write_frame_size < p_env->write_size) &&
-		    (p_env->packing == 0)) {
-			claw_snd_sys_validate_rsp(dev, p_ctlbk,
-				CLAW_RC_HOST_RCV_TOO_SMALL);
-			dev_warn(tdev,
-				"The local write buffer is smaller than the"
-				" remote read buffer\n");
-			CLAW_DBF_TEXT(2, setup, "wrtszbad");
-		}
-		if ((p_sysval->read_frame_size < p_env->read_size) &&
-		    (p_env->packing == 0)) {
-			claw_snd_sys_validate_rsp(dev, p_ctlbk,
-				CLAW_RC_HOST_RCV_TOO_SMALL);
-			dev_warn(tdev,
-				"The local read buffer is smaller than the"
-				" remote write buffer\n");
-			CLAW_DBF_TEXT(2, setup, "rdsizbad");
-		}
-		claw_snd_sys_validate_rsp(dev, p_ctlbk, 0);
-		dev_info(tdev,
-			"CLAW device %.8s: System validate"
-			" completed.\n", temp_ws_name);
-		dev_info(tdev,
-			"%s: sys Validate Rsize:%d Wsize:%d\n",
-			dev->name, p_sysval->read_frame_size,
-			p_sysval->write_frame_size);
-		privptr->system_validate_comp = 1;
-		if (strncmp(p_env->api_type, WS_APPL_NAME_PACKED, 6) == 0)
-			p_env->packing = PACKING_ASK;
-		claw_strt_conn_req(dev);
-		break;
-	case SYSTEM_VALIDATE_RESPONSE:
-		p_sysval = (struct sysval *)&(p_ctlbk->data);
-		dev_info(tdev,
-			"Settings for %s validated (version=%d, "
-			"remote device=%d, rc=%d, adapter name=%.8s, "
-			"host name=%.8s)\n",
-			dev->name,
-			p_ctlbk->version,
-			p_ctlbk->correlator,
-			p_ctlbk->rc,
-			p_sysval->WS_name,
-			p_sysval->host_name);
-		switch (p_ctlbk->rc) {
-		case 0:
-			dev_info(tdev, "%s: CLAW device "
-				"%.8s: System validate completed.\n",
-				dev->name, temp_ws_name);
-			if (privptr->system_validate_comp == 0)
-				claw_strt_conn_req(dev);
-			privptr->system_validate_comp = 1;
-			break;
-		case CLAW_RC_NAME_MISMATCH:
-			dev_warn(tdev, "Validating %s failed because of"
-				" a host or adapter name mismatch\n",
-				dev->name);
-			break;
-		case CLAW_RC_WRONG_VERSION:
-			dev_warn(tdev, "Validating %s failed because of a"
-				" version conflict\n",
-				dev->name);
-			break;
-		case CLAW_RC_HOST_RCV_TOO_SMALL:
-			dev_warn(tdev, "Validating %s failed because of a"
-				" frame size conflict\n",
-				dev->name);
-			break;
-		default:
-			dev_warn(tdev, "The communication peer of %s rejected"
-				" the connection\n",
-				 dev->name);
-			break;
-		}
-		break;
-
-	case CONNECTION_REQUEST:
-		p_connect = (struct conncmd *)&(p_ctlbk->data);
-		dev_info(tdev, "%s: Recv Conn Req: Vers=%d,link_id=%d,"
-			"Corr=%d,HOST appl=%.8s,WS appl=%.8s\n",
-			dev->name,
-			p_ctlbk->version,
-			p_ctlbk->linkid,
-			p_ctlbk->correlator,
-			p_connect->host_name,
-			p_connect->WS_name);
-		if (privptr->active_link_ID != 0) {
-			claw_snd_disc(dev, p_ctlbk);
-			dev_info(tdev, "%s rejected a connection request"
-				" because it is already active\n",
-				dev->name);
-		}
-		if (p_ctlbk->linkid != 1) {
-			claw_snd_disc(dev, p_ctlbk);
-			dev_info(tdev, "%s rejected a request to open multiple"
-				" connections\n",
-				dev->name);
-		}
-		rc = find_link(dev, p_connect->host_name, p_connect->WS_name);
-		if (rc != 0) {
-			claw_snd_disc(dev, p_ctlbk);
-			dev_info(tdev, "%s rejected a connection request"
-				" because of a type mismatch\n",
-				dev->name);
-		}
-		claw_send_control(dev,
-			CONNECTION_CONFIRM, p_ctlbk->linkid,
-			p_ctlbk->correlator,
-			0, p_connect->host_name,
-			p_connect->WS_name);
-		if (p_env->packing == PACKING_ASK) {
-			p_env->packing = PACK_SEND;
-			claw_snd_conn_req(dev, 0);
-		}
-		dev_info(tdev, "%s: CLAW device %.8s: Connection "
-			"completed link_id=%d.\n",
-			dev->name, temp_ws_name,
-			p_ctlbk->linkid);
-			privptr->active_link_ID = p_ctlbk->linkid;
-			p_ch = &privptr->channel[WRITE_CHANNEL];
-			wake_up(&p_ch->wait);  /* wake up claw_open ( WRITE) */
-		break;
-	case CONNECTION_RESPONSE:
-		p_connect = (struct conncmd *)&(p_ctlbk->data);
-		dev_info(tdev, "%s: Recv Conn Resp: Vers=%d,link_id=%d,"
-			"Corr=%d,RC=%d,Host appl=%.8s, WS appl=%.8s\n",
-			dev->name,
-			p_ctlbk->version,
-			p_ctlbk->linkid,
-			p_ctlbk->correlator,
-			p_ctlbk->rc,
-			p_connect->host_name,
-			p_connect->WS_name);
-
-		if (p_ctlbk->rc != 0) {
-			dev_warn(tdev, "The communication peer of %s rejected"
-				" a connection request\n",
-				dev->name);
-			return 1;
-		}
-		rc = find_link(dev,
-			p_connect->host_name, p_connect->WS_name);
-		if (rc != 0) {
-			claw_snd_disc(dev, p_ctlbk);
-			dev_warn(tdev, "The communication peer of %s"
-				" rejected a connection "
-				"request because of a type mismatch\n",
-				 dev->name);
-		}
-		/* should be until CONNECTION_CONFIRM */
-		privptr->active_link_ID = -(p_ctlbk->linkid);
-		break;
-	case CONNECTION_CONFIRM:
-		p_connect = (struct conncmd *)&(p_ctlbk->data);
-		dev_info(tdev,
-			"%s: Recv Conn Confirm:Vers=%d,link_id=%d,"
-			"Corr=%d,Host appl=%.8s,WS appl=%.8s\n",
-			dev->name,
-			p_ctlbk->version,
-			p_ctlbk->linkid,
-			p_ctlbk->correlator,
-			p_connect->host_name,
-			p_connect->WS_name);
-		if (p_ctlbk->linkid == -(privptr->active_link_ID)) {
-			privptr->active_link_ID = p_ctlbk->linkid;
-			if (p_env->packing > PACKING_ASK) {
-				dev_info(tdev,
-				"%s: Confirmed Now packing\n", dev->name);
-				p_env->packing = DO_PACKED;
-			}
-			p_ch = &privptr->channel[WRITE_CHANNEL];
-			wake_up(&p_ch->wait);
-		} else {
-			dev_warn(tdev, "Activating %s failed because of"
-				" an incorrect link ID=%d\n",
-				dev->name, p_ctlbk->linkid);
-			claw_snd_disc(dev, p_ctlbk);
-		}
-		break;
-	case DISCONNECT:
-		dev_info(tdev, "%s: Disconnect: "
-			"Vers=%d,link_id=%d,Corr=%d\n",
-			dev->name, p_ctlbk->version,
-			p_ctlbk->linkid, p_ctlbk->correlator);
-		if ((p_ctlbk->linkid == 2) &&
-		    (p_env->packing == PACK_SEND)) {
-			privptr->active_link_ID = 1;
-			p_env->packing = DO_PACKED;
-		} else
-			privptr->active_link_ID = 0;
-		break;
-	case CLAW_ERROR:
-		dev_warn(tdev, "The communication peer of %s failed\n",
-			dev->name);
-		break;
-	default:
-		dev_warn(tdev, "The communication peer of %s sent"
-			" an unknown command code\n",
-			dev->name);
-		break;
-        }
-
-        return 0;
-}   /*    end of claw_process_control    */
-
-
-/*-------------------------------------------------------------------*
-*               claw_send_control                                    *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static int
-claw_send_control(struct net_device *dev, __u8 type, __u8 link,
-	 __u8 correlator, __u8 rc, char *local_name, char *remote_name)
-{
-        struct claw_privbk 		*privptr;
-        struct clawctl                  *p_ctl;
-        struct sysval                   *p_sysval;
-        struct conncmd                  *p_connect;
-        struct sk_buff 			*skb;
-
-	CLAW_DBF_TEXT(2, setup, "sndcntl");
-	privptr = dev->ml_priv;
-        p_ctl=(struct clawctl *)&privptr->ctl_bk;
-
-        p_ctl->command=type;
-        p_ctl->version=CLAW_VERSION_ID;
-        p_ctl->linkid=link;
-        p_ctl->correlator=correlator;
-        p_ctl->rc=rc;
-
-        p_sysval=(struct sysval *)&p_ctl->data;
-        p_connect=(struct conncmd *)&p_ctl->data;
-
-        switch (p_ctl->command) {
-                case SYSTEM_VALIDATE_REQUEST:
-                case SYSTEM_VALIDATE_RESPONSE:
-                        memcpy(&p_sysval->host_name, local_name, 8);
-                        memcpy(&p_sysval->WS_name, remote_name, 8);
-			if (privptr->p_env->packing > 0) {
-				p_sysval->read_frame_size = DEF_PACK_BUFSIZE;
-				p_sysval->write_frame_size = DEF_PACK_BUFSIZE;
-			} else {
-				/* how big is the biggest group of packets */
-			   p_sysval->read_frame_size =
-				privptr->p_env->read_size;
-			   p_sysval->write_frame_size =
-				privptr->p_env->write_size;
-			}
-                        memset(&p_sysval->reserved, 0x00, 4);
-                        break;
-                case CONNECTION_REQUEST:
-                case CONNECTION_RESPONSE:
-                case CONNECTION_CONFIRM:
-                case DISCONNECT:
-                        memcpy(&p_sysval->host_name, local_name, 8);
-                        memcpy(&p_sysval->WS_name, remote_name, 8);
-			if (privptr->p_env->packing > 0) {
-			/* How big is the biggest packet */
-				p_connect->reserved1[0]=CLAW_FRAME_SIZE;
-                        	p_connect->reserved1[1]=CLAW_FRAME_SIZE;
-			} else {
-	                        memset(&p_connect->reserved1, 0x00, 4);
-        	                memset(&p_connect->reserved2, 0x00, 4);
-			}
-                        break;
-                default:
-                        break;
-        }
-
-        /*      write Control Record to the device                   */
-
-
-        skb = dev_alloc_skb(sizeof(struct clawctl));
-        if (!skb) {
-                return -ENOMEM;
-        }
-	memcpy(skb_put(skb, sizeof(struct clawctl)),
-		p_ctl, sizeof(struct clawctl));
-	if (privptr->p_env->packing >= PACK_SEND)
-		claw_hw_tx(skb, dev, 1);
-	else
-        	claw_hw_tx(skb, dev, 0);
-        return 0;
-}  /*   end of claw_send_control  */
-
-/*-------------------------------------------------------------------*
-*               claw_snd_conn_req                                    *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static int
-claw_snd_conn_req(struct net_device *dev, __u8 link)
-{
-        int                rc;
-	struct claw_privbk *privptr = dev->ml_priv;
-        struct clawctl 	   *p_ctl;
-
-	CLAW_DBF_TEXT(2, setup, "snd_conn");
-	rc = 1;
-        p_ctl=(struct clawctl *)&privptr->ctl_bk;
-	p_ctl->linkid = link;
-        if ( privptr->system_validate_comp==0x00 ) {
-                return rc;
-        }
-	if (privptr->p_env->packing == PACKING_ASK )
-		rc=claw_send_control(dev, CONNECTION_REQUEST,0,0,0,
-        		WS_APPL_NAME_PACKED, WS_APPL_NAME_PACKED);
-	if (privptr->p_env->packing == PACK_SEND)  {
-		rc=claw_send_control(dev, CONNECTION_REQUEST,0,0,0,
-        		WS_APPL_NAME_IP_NAME, WS_APPL_NAME_IP_NAME);
-	}
-	if (privptr->p_env->packing == 0)
-        	rc=claw_send_control(dev, CONNECTION_REQUEST,0,0,0,
-       			HOST_APPL_NAME, privptr->p_env->api_type);
-        return rc;
-
-}  /*  end of claw_snd_conn_req */
-
-
-/*-------------------------------------------------------------------*
-*               claw_snd_disc                                        *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static int
-claw_snd_disc(struct net_device *dev, struct clawctl * p_ctl)
-{
-        int rc;
-        struct conncmd *  p_connect;
-
-	CLAW_DBF_TEXT(2, setup, "snd_dsc");
-        p_connect=(struct conncmd *)&p_ctl->data;
-
-        rc=claw_send_control(dev, DISCONNECT, p_ctl->linkid,
-		p_ctl->correlator, 0,
-                p_connect->host_name, p_connect->WS_name);
-        return rc;
-}     /*   end of claw_snd_disc    */
-
-
-/*-------------------------------------------------------------------*
-*               claw_snd_sys_validate_rsp                            *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static int
-claw_snd_sys_validate_rsp(struct net_device *dev,
-	struct clawctl *p_ctl, __u32 return_code)
-{
-        struct claw_env *  p_env;
-        struct claw_privbk *privptr;
-        int    rc;
-
-	CLAW_DBF_TEXT(2, setup, "chkresp");
-	privptr = dev->ml_priv;
-        p_env=privptr->p_env;
-        rc=claw_send_control(dev, SYSTEM_VALIDATE_RESPONSE,
-		p_ctl->linkid,
-		p_ctl->correlator,
-                return_code,
-		p_env->host_name,
-		p_env->adapter_name  );
-        return rc;
-}     /*    end of claw_snd_sys_validate_rsp    */
-
-/*-------------------------------------------------------------------*
-*               claw_strt_conn_req                                   *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static int
-claw_strt_conn_req(struct net_device *dev )
-{
-        int rc;
-
-	CLAW_DBF_TEXT(2, setup, "conn_req");
-        rc=claw_snd_conn_req(dev, 1);
-        return rc;
-}    /*   end of claw_strt_conn_req   */
-
-
-
-/*-------------------------------------------------------------------*
- *   claw_stats                                                      *
- *-------------------------------------------------------------------*/
-
-static struct
-net_device_stats *claw_stats(struct net_device *dev)
-{
-        struct claw_privbk *privptr;
-
-	CLAW_DBF_TEXT(4, trace, "stats");
-	privptr = dev->ml_priv;
-        return &privptr->stats;
-}     /*   end of claw_stats   */
-
-
-/*-------------------------------------------------------------------*
-*       unpack_read                                                  *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static void
-unpack_read(struct net_device *dev )
-{
-        struct sk_buff *skb;
-        struct claw_privbk *privptr;
-	struct claw_env    *p_env;
-        struct ccwbk 	*p_this_ccw;
-        struct ccwbk 	*p_first_ccw;
-        struct ccwbk 	*p_last_ccw;
-	struct clawph 	*p_packh;
-	void		*p_packd;
-	struct clawctl 	*p_ctlrec=NULL;
-	struct device	*p_dev;
-
-        __u32	len_of_data;
-	__u32	pack_off;
-        __u8	link_num;
-        __u8 	mtc_this_frm=0;
-        __u32	bytes_to_mov;
-        int	i=0;
-	int     p=0;
-
-	CLAW_DBF_TEXT(4, trace, "unpkread");
-        p_first_ccw=NULL;
-        p_last_ccw=NULL;
-	p_packh=NULL;
-	p_packd=NULL;
-	privptr = dev->ml_priv;
-
-	p_dev = &privptr->channel[READ_CHANNEL].cdev->dev;
-	p_env = privptr->p_env;
-        p_this_ccw=privptr->p_read_active_first;
-	while (p_this_ccw!=NULL && p_this_ccw->header.flag!=CLAW_PENDING) {
-		pack_off = 0;
-		p = 0;
-		p_this_ccw->header.flag=CLAW_PENDING;
-		privptr->p_read_active_first=p_this_ccw->next;
-                p_this_ccw->next=NULL;
-		p_packh = (struct clawph *)p_this_ccw->p_buffer;
-		if ((p_env->packing == PACK_SEND) &&
-		    (p_packh->len == 32)           &&
-		    (p_packh->link_num == 0)) {   /* is it a packed ctl rec? */
-			p_packh++;  /* peek past pack header */
-			p_ctlrec = (struct clawctl *)p_packh;
-			p_packh--;  /* un peek */
-			if ((p_ctlrec->command == CONNECTION_RESPONSE) ||
-		            (p_ctlrec->command == CONNECTION_CONFIRM))
-				p_env->packing = DO_PACKED;
-		}
-		if (p_env->packing == DO_PACKED)
-			link_num=p_packh->link_num;
-		else
-	                link_num=p_this_ccw->header.opcode / 8;
-                if ((p_this_ccw->header.opcode & MORE_to_COME_FLAG)!=0) {
-                        mtc_this_frm=1;
-                        if (p_this_ccw->header.length!=
-				privptr->p_env->read_size ) {
-				dev_warn(p_dev,
-					"The communication peer of %s"
-					" sent a faulty"
-					" frame of length %02x\n",
-                                        dev->name, p_this_ccw->header.length);
-                        }
-                }
-
-                if (privptr->mtc_skipping) {
-                        /*
-                        *   We're in the mode of skipping past a
-			*   multi-frame message
-                        *   that we can't process for some reason or other.
-                        *   The first frame without the More-To-Come flag is
-			*   the last frame of the skipped message.
-                        */
-                        /*  in case of More-To-Come not set in this frame */
-                        if (mtc_this_frm==0) {
-                                privptr->mtc_skipping=0; /* Ok, the end */
-                                privptr->mtc_logical_link=-1;
-                        }
-                        goto NextFrame;
-                }
-
-                if (link_num==0) {
-                        claw_process_control(dev, p_this_ccw);
-			CLAW_DBF_TEXT(4, trace, "UnpkCntl");
-                        goto NextFrame;
-                }
-unpack_next:
-		if (p_env->packing == DO_PACKED) {
-			if (pack_off > p_env->read_size)
-				goto NextFrame;
-			p_packd = p_this_ccw->p_buffer+pack_off;
-			p_packh = (struct clawph *) p_packd;
-			if ((p_packh->len == 0) || /* done with this frame? */
-			    (p_packh->flag != 0))
-				goto NextFrame;
-			bytes_to_mov = p_packh->len;
-			pack_off += bytes_to_mov+sizeof(struct clawph);
-			p++;
-		} else {
-                	bytes_to_mov=p_this_ccw->header.length;
-		}
-                if (privptr->mtc_logical_link<0) {
-
-                /*
-                *  if More-To-Come is set in this frame then we don't know
-                *  length of entire message, and hence have to allocate
-		*  large buffer   */
-
-                /*      We are starting a new envelope  */
-                privptr->mtc_offset=0;
-                        privptr->mtc_logical_link=link_num;
-                }
-
-                if (bytes_to_mov > (MAX_ENVELOPE_SIZE- privptr->mtc_offset) ) {
-                        /*      error     */
-                        privptr->stats.rx_frame_errors++;
-                        goto NextFrame;
-                }
-		if (p_env->packing == DO_PACKED) {
-			memcpy( privptr->p_mtc_envelope+ privptr->mtc_offset,
-				p_packd+sizeof(struct clawph), bytes_to_mov);
-
-		} else	{
-                	memcpy( privptr->p_mtc_envelope+ privptr->mtc_offset,
-                        	p_this_ccw->p_buffer, bytes_to_mov);
-		}
-                if (mtc_this_frm==0) {
-                        len_of_data=privptr->mtc_offset+bytes_to_mov;
-                        skb=dev_alloc_skb(len_of_data);
-                        if (skb) {
-                                memcpy(skb_put(skb,len_of_data),
-					privptr->p_mtc_envelope,
-					len_of_data);
-                                skb->dev=dev;
-				skb_reset_mac_header(skb);
-                                skb->protocol=htons(ETH_P_IP);
-                                skb->ip_summed=CHECKSUM_UNNECESSARY;
-                                privptr->stats.rx_packets++;
-				privptr->stats.rx_bytes+=len_of_data;
-                                netif_rx(skb);
-                        }
-                        else {
-				dev_info(p_dev, "Allocating a buffer for"
-					" incoming data failed\n");
-                                privptr->stats.rx_dropped++;
-                        }
-                        privptr->mtc_offset=0;
-                        privptr->mtc_logical_link=-1;
-                }
-                else {
-                        privptr->mtc_offset+=bytes_to_mov;
-                }
-		if (p_env->packing == DO_PACKED)
-			goto unpack_next;
-NextFrame:
-                /*
-                *   Remove ThisCCWblock from active read queue, and add it
-                *   to queue of free blocks to be reused.
-                */
-                i++;
-                p_this_ccw->header.length=0xffff;
-                p_this_ccw->header.opcode=0xff;
-                /*
-                *       add this one to the free queue for later reuse
-                */
-                if (p_first_ccw==NULL) {
-                        p_first_ccw = p_this_ccw;
-                }
-                else {
-                        p_last_ccw->next = p_this_ccw;
-                }
-                p_last_ccw = p_this_ccw;
-                /*
-                *       chain to next block on active read queue
-                */
-                p_this_ccw = privptr->p_read_active_first;
-		CLAW_DBF_TEXT_(4, trace, "rxpkt %d", p);
-        } /* end of while */
-
-        /*      check validity                  */
-
-	CLAW_DBF_TEXT_(4, trace, "rxfrm %d", i);
-        add_claw_reads(dev, p_first_ccw, p_last_ccw);
-        claw_strt_read(dev, LOCK_YES);
-        return;
-}     /*  end of unpack_read   */
-
-/*-------------------------------------------------------------------*
-*       claw_strt_read                                               *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static void
-claw_strt_read (struct net_device *dev, int lock )
-{
-        int        rc = 0;
-        __u32      parm;
-        unsigned long  saveflags = 0;
-	struct claw_privbk *privptr = dev->ml_priv;
-        struct ccwbk*p_ccwbk;
-        struct chbk *p_ch;
-        struct clawh *p_clawh;
-	p_ch = &privptr->channel[READ_CHANNEL];
-
-	CLAW_DBF_TEXT(4, trace, "StRdNter");
-        p_clawh=(struct clawh *)privptr->p_claw_signal_blk;
-        p_clawh->flag=CLAW_IDLE;    /* 0x00 */
-
-        if ((privptr->p_write_active_first!=NULL &&
-             privptr->p_write_active_first->header.flag!=CLAW_PENDING) ||
-            (privptr->p_read_active_first!=NULL &&
-             privptr->p_read_active_first->header.flag!=CLAW_PENDING )) {
-                p_clawh->flag=CLAW_BUSY;    /* 0xff */
-        }
-        if (lock==LOCK_YES) {
-                spin_lock_irqsave(get_ccwdev_lock(p_ch->cdev), saveflags);
-        }
-        if (test_and_set_bit(0, (void *)&p_ch->IO_active) == 0) {
-		CLAW_DBF_TEXT(4, trace, "HotRead");
-                p_ccwbk=privptr->p_read_active_first;
-                parm = (unsigned long) p_ch;
-                rc = ccw_device_start (p_ch->cdev, &p_ccwbk->read, parm,
-				       0xff, 0);
-                if (rc != 0) {
-                        ccw_check_return_code(p_ch->cdev, rc);
-                }
-        }
-	else {
-		CLAW_DBF_TEXT(2, trace, "ReadAct");
-	}
-
-        if (lock==LOCK_YES) {
-                spin_unlock_irqrestore(get_ccwdev_lock(p_ch->cdev), saveflags);
-        }
-	CLAW_DBF_TEXT(4, trace, "StRdExit");
-        return;
-}       /*    end of claw_strt_read    */
-
-/*-------------------------------------------------------------------*
-*       claw_strt_out_IO                                             *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static void
-claw_strt_out_IO( struct net_device *dev )
-{
-        int             	rc = 0;
-        unsigned long   	parm;
-        struct claw_privbk 	*privptr;
-        struct chbk     	*p_ch;
-        struct ccwbk   	*p_first_ccw;
-
-	if (!dev) {
-		return;
-	}
-	privptr = (struct claw_privbk *)dev->ml_priv;
-	p_ch = &privptr->channel[WRITE_CHANNEL];
-
-	CLAW_DBF_TEXT(4, trace, "strt_io");
-        p_first_ccw=privptr->p_write_active_first;
-
-        if (p_ch->claw_state == CLAW_STOP)
-                return;
-        if (p_first_ccw == NULL) {
-                return;
-        }
-        if (test_and_set_bit(0, (void *)&p_ch->IO_active) == 0) {
-                parm = (unsigned long) p_ch;
-		CLAW_DBF_TEXT(2, trace, "StWrtIO");
-		rc = ccw_device_start(p_ch->cdev, &p_first_ccw->write, parm,
-				      0xff, 0);
-                if (rc != 0) {
-                        ccw_check_return_code(p_ch->cdev, rc);
-                }
-        }
-        dev->trans_start = jiffies;
-        return;
-}       /*    end of claw_strt_out_IO    */
-
-/*-------------------------------------------------------------------*
-*       Free write buffers                                           *
-*                                                                    *
-*--------------------------------------------------------------------*/
-
-static void
-claw_free_wrt_buf( struct net_device *dev )
-{
-
-	struct claw_privbk *privptr = (struct claw_privbk *)dev->ml_priv;
-	struct ccwbk*p_this_ccw;
-	struct ccwbk*p_next_ccw;
-
-	CLAW_DBF_TEXT(4, trace, "freewrtb");
-        /*  scan the write queue to free any completed write packets   */
-        p_this_ccw=privptr->p_write_active_first;
-        while ( (p_this_ccw!=NULL) && (p_this_ccw->header.flag!=CLAW_PENDING))
-        {
-                p_next_ccw = p_this_ccw->next;
-                if (((p_next_ccw!=NULL) &&
-		     (p_next_ccw->header.flag!=CLAW_PENDING)) ||
-                    ((p_this_ccw == privptr->p_write_active_last) &&
-                     (p_this_ccw->header.flag!=CLAW_PENDING))) {
-                        /* The next CCW is OK or this is  */
-			/* the last CCW...free it   @A1A  */
-                        privptr->p_write_active_first=p_this_ccw->next;
-			p_this_ccw->header.flag=CLAW_PENDING;
-                        p_this_ccw->next=privptr->p_write_free_chain;
-			privptr->p_write_free_chain=p_this_ccw;
-                        ++privptr->write_free_count;
-			privptr->stats.tx_bytes+= p_this_ccw->write.count;
-			p_this_ccw=privptr->p_write_active_first;
-                        privptr->stats.tx_packets++;
-                }
-                else {
-			break;
-                }
-        }
-        if (privptr->write_free_count!=0) {
-                claw_clearbit_busy(TB_NOBUFFER,dev);
-        }
-        /*   whole chain removed?   */
-        if (privptr->p_write_active_first==NULL) {
-                privptr->p_write_active_last=NULL;
-        }
-	CLAW_DBF_TEXT_(4, trace, "FWC=%d", privptr->write_free_count);
-        return;
-}
-
-/*-------------------------------------------------------------------*
-*       claw free netdevice                                          *
-*                                                                    *
-*--------------------------------------------------------------------*/
-static void
-claw_free_netdevice(struct net_device * dev, int free_dev)
-{
-	struct claw_privbk *privptr;
-
-	CLAW_DBF_TEXT(2, setup, "free_dev");
-	if (!dev)
-		return;
-	CLAW_DBF_TEXT_(2, setup, "%s", dev->name);
-	privptr = dev->ml_priv;
-	if (dev->flags & IFF_RUNNING)
-		claw_release(dev);
-	if (privptr) {
-		privptr->channel[READ_CHANNEL].ndev = NULL;  /* say it's free */
-	}
-	dev->ml_priv = NULL;
-#ifdef MODULE
-	if (free_dev) {
-		free_netdev(dev);
-	}
-#endif
-	CLAW_DBF_TEXT(2, setup, "free_ok");
-}
-
-/**
- * Claw init netdevice
- * Initialize everything of the net device except the name and the
- * channel structs.
- */
-static const struct net_device_ops claw_netdev_ops = {
-	.ndo_open		= claw_open,
-	.ndo_stop		= claw_release,
-	.ndo_get_stats		= claw_stats,
-	.ndo_start_xmit		= claw_tx,
-	.ndo_change_mtu		= claw_change_mtu,
-};
-
-static void
-claw_init_netdevice(struct net_device * dev)
-{
-	CLAW_DBF_TEXT(2, setup, "init_dev");
-	CLAW_DBF_TEXT_(2, setup, "%s", dev->name);
-	dev->mtu = CLAW_DEFAULT_MTU_SIZE;
-	dev->hard_header_len = 0;
-	dev->addr_len = 0;
-	dev->type = ARPHRD_SLIP;
-	dev->tx_queue_len = 1300;
-	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
-	dev->netdev_ops = &claw_netdev_ops;
-	CLAW_DBF_TEXT(2, setup, "initok");
-	return;
-}
-
-/**
- * Init a new channel in the privptr->channel[i].
- *
- * @param cdev  The ccw_device to be added.
- *
- * @return 0 on success, !0 on error.
- */
-static int
-add_channel(struct ccw_device *cdev,int i,struct claw_privbk *privptr)
-{
-	struct chbk *p_ch;
-	struct ccw_dev_id dev_id;
-
-	CLAW_DBF_TEXT_(2, setup, "%s", dev_name(&cdev->dev));
-	privptr->channel[i].flag  = i+1;   /* Read is 1 Write is 2 */
-	p_ch = &privptr->channel[i];
-	p_ch->cdev = cdev;
-	snprintf(p_ch->id, CLAW_ID_SIZE, "cl-%s", dev_name(&cdev->dev));
-	ccw_device_get_id(cdev, &dev_id);
-	p_ch->devno = dev_id.devno;
-	if ((p_ch->irb = kzalloc(sizeof (struct irb),GFP_KERNEL)) == NULL) {
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-
-/**
- *
- * Setup an interface.
- *
- * @param cgdev  Device to be setup.
- *
- * @returns 0 on success, !0 on failure.
- */
-static int
-claw_new_device(struct ccwgroup_device *cgdev)
-{
-	struct claw_privbk *privptr;
-	struct claw_env *p_env;
-	struct net_device *dev;
-	int ret;
-	struct ccw_dev_id dev_id;
-
-	dev_info(&cgdev->dev, "add for %s\n",
-		 dev_name(&cgdev->cdev[READ_CHANNEL]->dev));
-	CLAW_DBF_TEXT(2, setup, "new_dev");
-	privptr = dev_get_drvdata(&cgdev->dev);
-	dev_set_drvdata(&cgdev->cdev[READ_CHANNEL]->dev, privptr);
-	dev_set_drvdata(&cgdev->cdev[WRITE_CHANNEL]->dev, privptr);
-	if (!privptr)
-		return -ENODEV;
-	p_env = privptr->p_env;
-	ccw_device_get_id(cgdev->cdev[READ_CHANNEL], &dev_id);
-	p_env->devno[READ_CHANNEL] = dev_id.devno;
-	ccw_device_get_id(cgdev->cdev[WRITE_CHANNEL], &dev_id);
-	p_env->devno[WRITE_CHANNEL] = dev_id.devno;
-	ret = add_channel(cgdev->cdev[0],0,privptr);
-	if (ret == 0)
-		ret = add_channel(cgdev->cdev[1],1,privptr);
-	if (ret != 0) {
-		dev_warn(&cgdev->dev, "Creating a CLAW group device"
-			" failed with error code %d\n", ret);
-		goto out;
-	}
-	ret = ccw_device_set_online(cgdev->cdev[READ_CHANNEL]);
-	if (ret != 0) {
-		dev_warn(&cgdev->dev,
-			"Setting the read subchannel online"
-			" failed with error code %d\n", ret);
-		goto out;
-	}
-	ret = ccw_device_set_online(cgdev->cdev[WRITE_CHANNEL]);
-	if (ret != 0) {
-		dev_warn(&cgdev->dev,
-			"Setting the write subchannel online "
-			"failed with error code %d\n", ret);
-		goto out;
-	}
-	dev = alloc_netdev(0, "claw%d", NET_NAME_UNKNOWN, claw_init_netdevice);
-	if (!dev) {
-		dev_warn(&cgdev->dev,
-			"Activating the CLAW device failed\n");
-		goto out;
-	}
-	dev->ml_priv = privptr;
-	dev_set_drvdata(&cgdev->dev, privptr);
-	dev_set_drvdata(&cgdev->cdev[READ_CHANNEL]->dev, privptr);
-	dev_set_drvdata(&cgdev->cdev[WRITE_CHANNEL]->dev, privptr);
-	/* sysfs magic */
-        SET_NETDEV_DEV(dev, &cgdev->dev);
-	if (register_netdev(dev) != 0) {
-		claw_free_netdevice(dev, 1);
-		CLAW_DBF_TEXT(2, trace, "regfail");
-		goto out;
-	}
-	dev->flags &=~IFF_RUNNING;
-	if (privptr->buffs_alloc == 0) {
-	        ret=init_ccw_bk(dev);
-		if (ret !=0) {
-			unregister_netdev(dev);
-			claw_free_netdevice(dev,1);
-			CLAW_DBF_TEXT(2, trace, "ccwmem");
-			goto out;
-		}
-	}
-	privptr->channel[READ_CHANNEL].ndev = dev;
-	privptr->channel[WRITE_CHANNEL].ndev = dev;
-	privptr->p_env->ndev = dev;
-
-	dev_info(&cgdev->dev, "%s:readsize=%d  writesize=%d "
-		"readbuffer=%d writebuffer=%d read=0x%04x write=0x%04x\n",
-                dev->name, p_env->read_size,
-		p_env->write_size, p_env->read_buffers,
-		p_env->write_buffers, p_env->devno[READ_CHANNEL],
-		p_env->devno[WRITE_CHANNEL]);
-	dev_info(&cgdev->dev, "%s:host_name:%.8s, adapter_name "
-		":%.8s api_type: %.8s\n",
-                dev->name, p_env->host_name,
-		p_env->adapter_name , p_env->api_type);
-	return 0;
-out:
-	ccw_device_set_offline(cgdev->cdev[1]);
-	ccw_device_set_offline(cgdev->cdev[0]);
-	return -ENODEV;
-}
-
-static void
-claw_purge_skb_queue(struct sk_buff_head *q)
-{
-        struct sk_buff *skb;
-
-	CLAW_DBF_TEXT(4, trace, "purgque");
-        while ((skb = skb_dequeue(q))) {
-                atomic_dec(&skb->users);
-                dev_kfree_skb_any(skb);
-        }
-}
-
-/**
- * Shutdown an interface.
- *
- * @param cgdev  Device to be shut down.
- *
- * @returns 0 on success, !0 on failure.
- */
-static int
-claw_shutdown_device(struct ccwgroup_device *cgdev)
-{
-	struct claw_privbk *priv;
-	struct net_device *ndev;
-	int ret = 0;
-
-	CLAW_DBF_TEXT_(2, setup, "%s", dev_name(&cgdev->dev));
-	priv = dev_get_drvdata(&cgdev->dev);
-	if (!priv)
-		return -ENODEV;
-	ndev = priv->channel[READ_CHANNEL].ndev;
-	if (ndev) {
-		/* Close the device */
-		dev_info(&cgdev->dev, "%s: shutting down\n",
-			ndev->name);
-		if (ndev->flags & IFF_RUNNING)
-			ret = claw_release(ndev);
-		ndev->flags &=~IFF_RUNNING;
-		unregister_netdev(ndev);
-		ndev->ml_priv = NULL;  /* cgdev data, not ndev's to free */
-		claw_free_netdevice(ndev, 1);
-		priv->channel[READ_CHANNEL].ndev = NULL;
-		priv->channel[WRITE_CHANNEL].ndev = NULL;
-		priv->p_env->ndev = NULL;
-	}
-	ccw_device_set_offline(cgdev->cdev[1]);
-	ccw_device_set_offline(cgdev->cdev[0]);
-	return ret;
-}
-
-static void
-claw_remove_device(struct ccwgroup_device *cgdev)
-{
-	struct claw_privbk *priv;
-
-	CLAW_DBF_TEXT_(2, setup, "%s", dev_name(&cgdev->dev));
-	priv = dev_get_drvdata(&cgdev->dev);
-	dev_info(&cgdev->dev, " will be removed.\n");
-	if (cgdev->state == CCWGROUP_ONLINE)
-		claw_shutdown_device(cgdev);
-	kfree(priv->p_mtc_envelope);
-	priv->p_mtc_envelope=NULL;
-	kfree(priv->p_env);
-	priv->p_env=NULL;
-	kfree(priv->channel[0].irb);
-	priv->channel[0].irb=NULL;
-	kfree(priv->channel[1].irb);
-	priv->channel[1].irb=NULL;
-	kfree(priv);
-	dev_set_drvdata(&cgdev->dev, NULL);
-	dev_set_drvdata(&cgdev->cdev[READ_CHANNEL]->dev, NULL);
-	dev_set_drvdata(&cgdev->cdev[WRITE_CHANNEL]->dev, NULL);
-	put_device(&cgdev->dev);
-
-	return;
-}
-
-
-/*
- * sysfs attributes
- */
-static ssize_t
-claw_hname_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	return sprintf(buf, "%s\n",p_env->host_name);
-}
-
-static ssize_t
-claw_hname_write(struct device *dev, struct device_attribute *attr,
-	 const char *buf, size_t count)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	if (count > MAX_NAME_LEN+1)
-		return -EINVAL;
-	memset(p_env->host_name, 0x20, MAX_NAME_LEN);
-	strncpy(p_env->host_name,buf, count);
-	p_env->host_name[count-1] = 0x20;  /* clear extra 0x0a */
-	p_env->host_name[MAX_NAME_LEN] = 0x00;
-	CLAW_DBF_TEXT(2, setup, "HstnSet");
-	CLAW_DBF_TEXT_(2, setup, "%s", p_env->host_name);
-
-	return count;
-}
-
-static DEVICE_ATTR(host_name, 0644, claw_hname_show, claw_hname_write);
-
-static ssize_t
-claw_adname_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	return sprintf(buf, "%s\n", p_env->adapter_name);
-}
-
-static ssize_t
-claw_adname_write(struct device *dev, struct device_attribute *attr,
-	 const char *buf, size_t count)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	if (count > MAX_NAME_LEN+1)
-		return -EINVAL;
-	memset(p_env->adapter_name, 0x20, MAX_NAME_LEN);
-	strncpy(p_env->adapter_name,buf, count);
-	p_env->adapter_name[count-1] = 0x20; /* clear extra 0x0a */
-	p_env->adapter_name[MAX_NAME_LEN] = 0x00;
-	CLAW_DBF_TEXT(2, setup, "AdnSet");
-	CLAW_DBF_TEXT_(2, setup, "%s", p_env->adapter_name);
-
-	return count;
-}
-
-static DEVICE_ATTR(adapter_name, 0644, claw_adname_show, claw_adname_write);
-
-static ssize_t
-claw_apname_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	return sprintf(buf, "%s\n",
-		       p_env->api_type);
-}
-
-static ssize_t
-claw_apname_write(struct device *dev, struct device_attribute *attr,
-	const char *buf, size_t count)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	if (count > MAX_NAME_LEN+1)
-		return -EINVAL;
-	memset(p_env->api_type, 0x20, MAX_NAME_LEN);
-	strncpy(p_env->api_type,buf, count);
-	p_env->api_type[count-1] = 0x20;  /* we get a loose 0x0a */
-	p_env->api_type[MAX_NAME_LEN] = 0x00;
-	if(strncmp(p_env->api_type,WS_APPL_NAME_PACKED,6) == 0) {
-		p_env->read_size=DEF_PACK_BUFSIZE;
-		p_env->write_size=DEF_PACK_BUFSIZE;
-		p_env->packing=PACKING_ASK;
-		CLAW_DBF_TEXT(2, setup, "PACKING");
-	}
-	else {
-		p_env->packing=0;
-		p_env->read_size=CLAW_FRAME_SIZE;
-		p_env->write_size=CLAW_FRAME_SIZE;
-		CLAW_DBF_TEXT(2, setup, "ApiSet");
-	}
-	CLAW_DBF_TEXT_(2, setup, "%s", p_env->api_type);
-	return count;
-}
-
-static DEVICE_ATTR(api_type, 0644, claw_apname_show, claw_apname_write);
-
-static ssize_t
-claw_wbuff_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct claw_privbk *priv;
-	struct claw_env * p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	return sprintf(buf, "%d\n", p_env->write_buffers);
-}
-
-static ssize_t
-claw_wbuff_write(struct device *dev, struct device_attribute *attr,
-	const char *buf, size_t count)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-	int nnn,max;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	sscanf(buf, "%i", &nnn);
-	if (p_env->packing) {
-		max = 64;
-	}
-	else {
-		max = 512;
-	}
-	if ((nnn > max ) || (nnn < 2))
-		return -EINVAL;
-	p_env->write_buffers = nnn;
-	CLAW_DBF_TEXT(2, setup, "Wbufset");
-	CLAW_DBF_TEXT_(2, setup, "WB=%d", p_env->write_buffers);
-	return count;
-}
-
-static DEVICE_ATTR(write_buffer, 0644, claw_wbuff_show, claw_wbuff_write);
-
-static ssize_t
-claw_rbuff_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct claw_privbk *priv;
-	struct claw_env *  p_env;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	return sprintf(buf, "%d\n", p_env->read_buffers);
-}
-
-static ssize_t
-claw_rbuff_write(struct device *dev, struct device_attribute *attr,
-	const char *buf, size_t count)
-{
-	struct claw_privbk *priv;
-	struct claw_env *p_env;
-	int nnn,max;
-
-	priv = dev_get_drvdata(dev);
-	if (!priv)
-		return -ENODEV;
-	p_env = priv->p_env;
-	sscanf(buf, "%i", &nnn);
-	if (p_env->packing) {
-		max = 64;
-	}
-	else {
-		max = 512;
-	}
-	if ((nnn > max ) || (nnn < 2))
-		return -EINVAL;
-	p_env->read_buffers = nnn;
-	CLAW_DBF_TEXT(2, setup, "Rbufset");
-	CLAW_DBF_TEXT_(2, setup, "RB=%d", p_env->read_buffers);
-	return count;
-}
-static DEVICE_ATTR(read_buffer, 0644, claw_rbuff_show, claw_rbuff_write);
-
-static struct attribute *claw_attr[] = {
-	&dev_attr_read_buffer.attr,
-	&dev_attr_write_buffer.attr,
-	&dev_attr_adapter_name.attr,
-	&dev_attr_api_type.attr,
-	&dev_attr_host_name.attr,
-	NULL,
-};
-static struct attribute_group claw_attr_group = {
-	.attrs = claw_attr,
-};
-static const struct attribute_group *claw_attr_groups[] = {
-	&claw_attr_group,
-	NULL,
-};
-static const struct device_type claw_devtype = {
-	.name = "claw",
-	.groups = claw_attr_groups,
-};
-
-/*----------------------------------------------------------------*
- *   claw_probe 						  *
- *	this function is called for each CLAW device.		  *
- *----------------------------------------------------------------*/
-static int claw_probe(struct ccwgroup_device *cgdev)
-{
-	struct claw_privbk *privptr = NULL;
-
-	CLAW_DBF_TEXT(2, setup, "probe");
-	if (!get_device(&cgdev->dev))
-		return -ENODEV;
-	privptr = kzalloc(sizeof(struct claw_privbk), GFP_KERNEL);
-	dev_set_drvdata(&cgdev->dev, privptr);
-	if (privptr == NULL) {
-		probe_error(cgdev);
-		put_device(&cgdev->dev);
-		CLAW_DBF_TEXT_(2, setup, "probex%d", -ENOMEM);
-		return -ENOMEM;
-	}
-	privptr->p_mtc_envelope = kzalloc(MAX_ENVELOPE_SIZE, GFP_KERNEL);
-	privptr->p_env = kzalloc(sizeof(struct claw_env), GFP_KERNEL);
-	if ((privptr->p_mtc_envelope == NULL) || (privptr->p_env == NULL)) {
-		probe_error(cgdev);
-		put_device(&cgdev->dev);
-		CLAW_DBF_TEXT_(2, setup, "probex%d", -ENOMEM);
-		return -ENOMEM;
-	}
-	memcpy(privptr->p_env->adapter_name, WS_NAME_NOT_DEF, 8);
-	memcpy(privptr->p_env->host_name, WS_NAME_NOT_DEF, 8);
-	memcpy(privptr->p_env->api_type, WS_NAME_NOT_DEF, 8);
-	privptr->p_env->packing = 0;
-	privptr->p_env->write_buffers = 5;
-	privptr->p_env->read_buffers = 5;
-	privptr->p_env->read_size = CLAW_FRAME_SIZE;
-	privptr->p_env->write_size = CLAW_FRAME_SIZE;
-	privptr->p_env->p_priv = privptr;
-	cgdev->cdev[0]->handler = claw_irq_handler;
-	cgdev->cdev[1]->handler = claw_irq_handler;
-	cgdev->dev.type = &claw_devtype;
-	CLAW_DBF_TEXT(2, setup, "prbext 0");
-
-	return 0;
-}  /*  end of claw_probe       */
-
-/*--------------------------------------------------------------------*
-*    claw_init  and cleanup                                           *
-*---------------------------------------------------------------------*/
-
-static void __exit claw_cleanup(void)
-{
-	ccwgroup_driver_unregister(&claw_group_driver);
-	ccw_driver_unregister(&claw_ccw_driver);
-	root_device_unregister(claw_root_dev);
-	claw_unregister_debug_facility();
-	pr_info("Driver unloaded\n");
-}
-
-/**
- * Initialize module.
- * This is called just after the module is loaded.
- *
- * @return 0 on success, !0 on error.
- */
-static int __init claw_init(void)
-{
-	int ret = 0;
-
-	pr_info("Loading %s\n", version);
-	ret = claw_register_debug_facility();
-	if (ret) {
-		pr_err("Registering with the S/390 debug feature"
-			" failed with error code %d\n", ret);
-		goto out_err;
-	}
-	CLAW_DBF_TEXT(2, setup, "init_mod");
-	claw_root_dev = root_device_register("claw");
-	ret = PTR_ERR_OR_ZERO(claw_root_dev);
-	if (ret)
-		goto register_err;
-	ret = ccw_driver_register(&claw_ccw_driver);
-	if (ret)
-		goto ccw_err;
-	claw_group_driver.driver.groups = claw_drv_attr_groups;
-	ret = ccwgroup_driver_register(&claw_group_driver);
-	if (ret)
-		goto ccwgroup_err;
-	return 0;
-
-ccwgroup_err:
-	ccw_driver_unregister(&claw_ccw_driver);
-ccw_err:
-	root_device_unregister(claw_root_dev);
-register_err:
-	CLAW_DBF_TEXT(2, setup, "init_bad");
-	claw_unregister_debug_facility();
-out_err:
-	pr_err("Initializing the claw device driver failed\n");
-	return ret;
-}
-
-module_init(claw_init);
-module_exit(claw_cleanup);
-
-MODULE_AUTHOR("Andy Richter <richtera@us.ibm.com>");
-MODULE_DESCRIPTION("Linux for System z CLAW Driver\n" \
-			"Copyright IBM Corp. 2000, 2008\n");
-MODULE_LICENSE("GPL");
diff --git a/drivers/s390/net/claw.h b/drivers/s390/net/claw.h
deleted file mode 100644
index 3339b9b607b3..000000000000
--- a/drivers/s390/net/claw.h
+++ /dev/null
@@ -1,348 +0,0 @@
-/*******************************************************
-*  Define constants                                    *
-*                                                      *
-********************************************************/
-
-/*-----------------------------------------------------*
-*     CCW command codes for CLAW protocol              *
-*------------------------------------------------------*/
-
-#define CCW_CLAW_CMD_WRITE           0x01      /* write - not including link */
-#define CCW_CLAW_CMD_READ            0x02      /* read */
-#define CCW_CLAW_CMD_NOP             0x03      /* NOP */
-#define CCW_CLAW_CMD_SENSE           0x04      /* Sense */
-#define CCW_CLAW_CMD_SIGNAL_SMOD     0x05      /* Signal Status Modifier */
-#define CCW_CLAW_CMD_TIC             0x08      /* TIC */
-#define CCW_CLAW_CMD_READHEADER      0x12      /* read header data */
-#define CCW_CLAW_CMD_READFF          0x22      /* read an FF */
-#define CCW_CLAW_CMD_SENSEID         0xe4      /* Sense ID */
-
-
-/*-----------------------------------------------------*
-*    CLAW Unique constants                             *
-*------------------------------------------------------*/
-
-#define MORE_to_COME_FLAG       0x04   /* OR with write CCW in case of m-t-c */
-#define CLAW_IDLE               0x00   /* flag to indicate CLAW is idle */
-#define CLAW_BUSY               0xff   /* flag to indicate CLAW is busy */
-#define CLAW_PENDING            0x00   /* flag to indicate i/o is pending */
-#define CLAW_COMPLETE           0xff   /* flag to indicate i/o completed */
-
-/*-----------------------------------------------------*
-*     CLAW control command code                        *
-*------------------------------------------------------*/
-
-#define SYSTEM_VALIDATE_REQUEST   0x01  /* System Validate request */
-#define SYSTEM_VALIDATE_RESPONSE  0x02  /* System Validate response */
-#define CONNECTION_REQUEST        0x21  /* Connection request */
-#define CONNECTION_RESPONSE       0x22  /* Connection response */
-#define CONNECTION_CONFIRM        0x23  /* Connection confirm */
-#define DISCONNECT                0x24  /* Disconnect */
-#define CLAW_ERROR                0x41  /* CLAW error message */
-#define CLAW_VERSION_ID           2     /* CLAW version ID */
-
-/*-----------------------------------------------------*
-*  CLAW adater sense bytes                             *
-*------------------------------------------------------*/
-
-#define CLAW_ADAPTER_SENSE_BYTE 0x41   /* Stop command issued to adapter */
-
-/*-----------------------------------------------------*
-*      CLAW control command return codes               *
-*------------------------------------------------------*/
-
-#define CLAW_RC_NAME_MISMATCH       166  /*  names do not match */
-#define CLAW_RC_WRONG_VERSION       167  /*  wrong CLAW version number */
-#define CLAW_RC_HOST_RCV_TOO_SMALL  180  /*  Host maximum receive is   */
-					 /*  less than Linux on zSeries*/
-                                         /*  transmit size             */
-
-/*-----------------------------------------------------*
-*      CLAW Constants application name                 *
-*------------------------------------------------------*/
-
-#define HOST_APPL_NAME          "TCPIP   "
-#define WS_APPL_NAME_IP_LINK    "TCPIP   "
-#define WS_APPL_NAME_IP_NAME	"IP      "
-#define WS_APPL_NAME_API_LINK   "API     "
-#define WS_APPL_NAME_PACKED     "PACKED  "
-#define WS_NAME_NOT_DEF         "NOT_DEF "
-#define PACKING_ASK		1
-#define PACK_SEND		2
-#define DO_PACKED		3
-
-#define MAX_ENVELOPE_SIZE       65536
-#define CLAW_DEFAULT_MTU_SIZE   4096
-#define DEF_PACK_BUFSIZE	32768
-#define READ_CHANNEL		0
-#define WRITE_CHANNEL		1
-
-#define TB_TX                   0          /* sk buffer handling in process  */
-#define TB_STOP                 1          /* network device stop in process */
-#define TB_RETRY                2          /* retry in process               */
-#define TB_NOBUFFER             3          /* no buffer on free queue        */
-#define CLAW_MAX_LINK_ID        1
-#define CLAW_MAX_DEV            256        /*      max claw devices          */
-#define MAX_NAME_LEN            8          /* host name, adapter name length */
-#define CLAW_FRAME_SIZE         4096
-#define CLAW_ID_SIZE		20+3
-
-/* state machine codes used in claw_irq_handler */
-
-#define CLAW_STOP                0
-#define CLAW_START_HALT_IO       1
-#define CLAW_START_SENSEID       2
-#define CLAW_START_READ          3
-#define CLAW_START_WRITE         4
-
-/*-----------------------------------------------------*
-*    Lock flag                                         *
-*------------------------------------------------------*/
-#define LOCK_YES             0
-#define LOCK_NO              1
-
-/*-----------------------------------------------------*
-*    DBF Debug macros                                  *
-*------------------------------------------------------*/
-#define CLAW_DBF_TEXT(level, name, text) \
-	do { \
-		debug_text_event(claw_dbf_##name, level, text); \
-	} while (0)
-
-#define CLAW_DBF_HEX(level,name,addr,len) \
-do { \
-	debug_event(claw_dbf_##name,level,(void*)(addr),len); \
-} while (0)
-
-#define CLAW_DBF_TEXT_(level,name,text...) \
-	do { \
-		if (debug_level_enabled(claw_dbf_##name, level)) { \
-			sprintf(debug_buffer, text); \
-			debug_text_event(claw_dbf_##name, level, \
-						debug_buffer); \
-		} \
-	} while (0)
-
-/**
- * Enum for classifying detected devices.
- */
-enum claw_channel_types {
-	/* Device is not a channel  */
-	claw_channel_type_none,
-
-	/* Device is a CLAW channel device */
-	claw_channel_type_claw
-};
-
-
-/*******************************************************
-*  Define Control Blocks                               *
-*                                                      *
-********************************************************/
-
-/*------------------------------------------------------*/
-/*     CLAW header                                      */
-/*------------------------------------------------------*/
-
-struct clawh {
-        __u16  length;     /* length of data read by preceding read CCW */
-        __u8   opcode;     /* equivalent read CCW */
-        __u8   flag;       /* flag of FF to indicate read was completed */
-};
-
-/*------------------------------------------------------*/
-/*     CLAW Packing header   4 bytes                    */
-/*------------------------------------------------------*/
-struct clawph {
-       __u16 len;  	/* Length of Packed Data Area   */
-       __u8  flag;  	/* Reserved not used            */
-       __u8  link_num;	/* Link ID                      */
-};
-
-/*------------------------------------------------------*/
-/*     CLAW Ending struct ccwbk                         */
-/*------------------------------------------------------*/
-struct endccw {
-	__u32     real;            /* real address of this block */
-       __u8      write1;          /* write 1 is active */
-        __u8      read1;           /* read 1 is active  */
-        __u16     reserved;        /* reserved for future use */
-        struct ccw1    write1_nop1;
-        struct ccw1    write1_nop2;
-        struct ccw1    write2_nop1;
-        struct ccw1    write2_nop2;
-        struct ccw1    read1_nop1;
-        struct ccw1    read1_nop2;
-        struct ccw1    read2_nop1;
-        struct ccw1    read2_nop2;
-};
-
-/*------------------------------------------------------*/
-/*     CLAW struct ccwbk                                       */
-/*------------------------------------------------------*/
-struct ccwbk {
-        void   *next;        /* pointer to next ccw block */
-        __u32     real;         /* real address of this ccw */
-        void      *p_buffer;    /* virtual address of data */
-        struct clawh     header;       /* claw header */
-        struct ccw1    write;   /* write CCW    */
-        struct ccw1    w_read_FF; /* read FF */
-        struct ccw1    w_TIC_1;        /* TIC */
-        struct ccw1    read;         /* read CCW  */
-        struct ccw1    read_h;        /* read header */
-        struct ccw1    signal;       /* signal SMOD  */
-        struct ccw1    r_TIC_1;        /* TIC1 */
-        struct ccw1    r_read_FF;      /* read FF  */
-        struct ccw1    r_TIC_2;        /* TIC2 */
-};
-
-/*------------------------------------------------------*/
-/*     CLAW control block                               */
-/*------------------------------------------------------*/
-struct clawctl {
-        __u8    command;      /* control command */
-        __u8    version;      /* CLAW protocol version */
-        __u8    linkid;       /* link ID   */
-        __u8    correlator;   /* correlator */
-        __u8    rc;           /* return code */
-        __u8    reserved1;    /* reserved */
-        __u8    reserved2;    /* reserved */
-        __u8    reserved3;    /* reserved */
-        __u8    data[24];     /* command specific fields */
-};
-
-/*------------------------------------------------------*/
-/*     Data for SYSTEMVALIDATE command                  */
-/*------------------------------------------------------*/
-struct sysval  {
-        char    WS_name[8];        /* Workstation System name  */
-        char    host_name[8];      /* Host system name     */
-        __u16   read_frame_size;   /* read frame size */
-        __u16   write_frame_size;  /* write frame size */
-        __u8    reserved[4];       /* reserved */
-};
-
-/*------------------------------------------------------*/
-/*     Data for Connect command                         */
-/*------------------------------------------------------*/
-struct conncmd  {
-        char     WS_name[8];       /* Workstation application name  */
-        char     host_name[8];     /* Host application name      */
-        __u16    reserved1[2];     /* read frame size */
-        __u8     reserved2[4];     /* reserved  */
-};
-
-/*------------------------------------------------------*/
-/*     Data for CLAW error                              */
-/*------------------------------------------------------*/
-struct clawwerror  {
-        char      reserved1[8];   /* reserved */
-        char      reserved2[8];   /* reserved  */
-        char      reserved3[8];   /* reserved  */
-};
-
-/*------------------------------------------------------*/
-/*     Data buffer for CLAW                             */
-/*------------------------------------------------------*/
-struct clawbuf  {
-       char      buffer[MAX_ENVELOPE_SIZE];   /* data buffer */
-};
-
-/*------------------------------------------------------*/
-/*     Channel control block for read and write channel */
-/*------------------------------------------------------*/
-
-struct chbk {
-        unsigned int        devno;
-        int                 irq;
-	char 		    id[CLAW_ID_SIZE];
-       __u32               IO_active;
-        __u8                claw_state;
-        struct irb          *irb;
-       	struct ccw_device   *cdev;  /* pointer to the channel device */
-	struct net_device   *ndev;
-        wait_queue_head_t   wait;
-        struct tasklet_struct    tasklet;
-        struct timer_list   timer;
-        unsigned long       flag_a;    /* atomic flags */
-#define CLAW_BH_ACTIVE      0
-        unsigned long       flag_b;    /* atomic flags */
-#define CLAW_WRITE_ACTIVE   0
-        __u8                last_dstat;
-        __u8                flag;
-	struct sk_buff_head collect_queue;
-	spinlock_t collect_lock;
-#define CLAW_WRITE      0x02      /* - Set if this is a write channel */
-#define CLAW_READ	0x01      /* - Set if this is a read channel  */
-#define CLAW_TIMER      0x80      /* - Set if timer made the wake_up  */
-};
-
-/*--------------------------------------------------------------*
-*           CLAW  environment block                             *
-*---------------------------------------------------------------*/
-
-struct claw_env {
-        unsigned int            devno[2];       /* device number */
-        char                    host_name[9];   /* Host name */
-        char                    adapter_name [9]; /* adapter name */
-        char                    api_type[9];    /* TCPIP, API or PACKED */
-        void                    *p_priv;        /* privptr */
-        __u16                   read_buffers;   /* read buffer number */
-        __u16                   write_buffers;  /* write buffer number */
-        __u16                   read_size;      /* read buffer size */
-        __u16                   write_size;     /* write buffer size */
-        __u16                   dev_id;         /* device ident */
-	__u8			packing;	/* are we packing? */
-        __u8                    in_use;         /* device active flag */
-        struct net_device       *ndev;    	/* backward ptr to the net dev*/
-};
-
-/*--------------------------------------------------------------*
-*           CLAW  main control block                            *
-*---------------------------------------------------------------*/
-
-struct claw_privbk {
-        void *p_buff_ccw;
-        __u32      p_buff_ccw_num;
-        void  *p_buff_read;
-        __u32      p_buff_read_num;
-        __u32      p_buff_pages_perread;
-        void  *p_buff_write;
-        __u32      p_buff_write_num;
-        __u32      p_buff_pages_perwrite;
-        long       active_link_ID;           /* Active logical link ID */
-        struct ccwbk *p_write_free_chain;     /* pointer to free ccw chain */
-        struct ccwbk *p_write_active_first;   /* ptr to the first write ccw */
-        struct ccwbk *p_write_active_last;    /* ptr to the last write ccw */
-        struct ccwbk *p_read_active_first;    /* ptr to the first read ccw */
-        struct ccwbk *p_read_active_last;     /* ptr to the last read ccw */
-        struct endccw *p_end_ccw;              /*ptr to ending ccw */
-        struct ccwbk *p_claw_signal_blk;      /* ptr to signal block */
-        __u32      write_free_count;       /* number of free bufs for write */
-	struct     net_device_stats  stats; /* 	 device status    */
-        struct chbk channel[2];            /* Channel control blocks */
-        __u8       mtc_skipping;
-        int        mtc_offset;
-        int        mtc_logical_link;
-        void       *p_mtc_envelope;
-	struct	   sk_buff	*pk_skb;	/* packing buffer    */
-	int	   pk_cnt;
-        struct clawctl ctl_bk;
-        struct claw_env *p_env;
-        __u8       system_validate_comp;
-        __u8       release_pend;
-        __u8      checksum_received_ip_pkts;
-	__u8      buffs_alloc;
-        struct endccw  end_ccw;
-        unsigned long  tbusy;
-
-};
-
-
-/************************************************************/
-/* define global constants                                  */
-/************************************************************/
-
-#define CCWBK_SIZE sizeof(struct ccwbk)
-
-
diff --git a/drivers/s390/net/ctcm_mpc.c b/drivers/s390/net/ctcm_mpc.c
index 2dbc77b5137b..edf16bfba8ee 100644
--- a/drivers/s390/net/ctcm_mpc.c
+++ b/drivers/s390/net/ctcm_mpc.c
@@ -130,11 +130,7 @@ void ctcmpc_dumpit(char *buf, int len)
 	__u32	ct, sw, rm, dup;
 	char	*ptr, *rptr;
 	char	tbuf[82], tdup[82];
-	#ifdef CONFIG_64BIT
 	char	addr[22];
-	#else
-	char	addr[12];
-	#endif
 	char	boff[12];
 	char	bhex[82], duphex[82];
 	char	basc[40];
@@ -147,11 +143,7 @@ void ctcmpc_dumpit(char *buf, int len)
 
 	for (ct = 0; ct < len; ct++, ptr++, rptr++) {
 		if (sw == 0) {
-			#ifdef CONFIG_64BIT
 			sprintf(addr, "%16.16llx", (__u64)rptr);
-			#else
-			sprintf(addr, "%8.8X", (__u32)rptr);
-			#endif
 
 			sprintf(boff, "%4.4X", (__u32)ct);
 			bhex[0] = '\0';
@@ -162,11 +154,7 @@ void ctcmpc_dumpit(char *buf, int len)
 		if (sw == 8)
 			strcat(bhex, "	");
 
-		#if CONFIG_64BIT
 		sprintf(tbuf, "%2.2llX", (__u64)*ptr);
-		#else
-		sprintf(tbuf, "%2.2X", (__u32)*ptr);
-		#endif
 
 		tbuf[2] = '\0';
 		strcat(bhex, tbuf);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 642c77c76b84..3466d3cb7647 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -4218,7 +4218,7 @@ void qeth_setadp_promisc_mode(struct qeth_card *card)
 	QETH_CARD_TEXT_(card, 4, "mode:%x", mode);
 
 	iob = qeth_get_adapter_cmd(card, IPA_SETADP_SET_PROMISC_MODE,
-			sizeof(struct qeth_ipacmd_setadpparms));
+			sizeof(struct qeth_ipacmd_setadpparms_hdr) + 8);
 	if (!iob)
 		return;
 	cmd = (struct qeth_ipa_cmd *)(iob->data + IPA_PDU_HEADER_SIZE);
@@ -4290,7 +4290,8 @@ int qeth_setadpparms_change_macaddr(struct qeth_card *card)
 	QETH_CARD_TEXT(card, 4, "chgmac");
 
 	iob = qeth_get_adapter_cmd(card, IPA_SETADP_ALTER_MAC_ADDRESS,
-				   sizeof(struct qeth_ipacmd_setadpparms));
+				   sizeof(struct qeth_ipacmd_setadpparms_hdr) +
+				   sizeof(struct qeth_change_addr));
 	if (!iob)
 		return -ENOMEM;
 	cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
diff --git a/drivers/s390/scsi/zfcp_erp.c b/drivers/s390/scsi/zfcp_erp.c
index 2c5d4567d1da..acde3f5d6e9e 100644
--- a/drivers/s390/scsi/zfcp_erp.c
+++ b/drivers/s390/scsi/zfcp_erp.c
@@ -738,11 +738,11 @@ static int zfcp_erp_adapter_strategy_open_fsf(struct zfcp_erp_action *act)
 		return ZFCP_ERP_FAILED;
 
 	if (mempool_resize(act->adapter->pool.sr_data,
-			   act->adapter->stat_read_buf_num, GFP_KERNEL))
+			   act->adapter->stat_read_buf_num))
 		return ZFCP_ERP_FAILED;
 
 	if (mempool_resize(act->adapter->pool.status_read_req,
-			   act->adapter->stat_read_buf_num, GFP_KERNEL))
+			   act->adapter->stat_read_buf_num))
 		return ZFCP_ERP_FAILED;
 
 	atomic_set(&act->adapter->stat_miss, act->adapter->stat_read_buf_num);
diff --git a/drivers/sbus/char/bbc_envctrl.c b/drivers/sbus/char/bbc_envctrl.c
index 0787b9756165..228c782d6433 100644
--- a/drivers/sbus/char/bbc_envctrl.c
+++ b/drivers/sbus/char/bbc_envctrl.c
@@ -160,8 +160,7 @@ static void do_envctrl_shutdown(struct bbc_cpu_temperature *tp)
 	printk(KERN_CRIT "kenvctrld: Shutting down the system now.\n");
 
 	shutting_down = 1;
-	if (orderly_poweroff(true) < 0)
-		printk(KERN_CRIT "envctrl: shutdown execution failed\n");
+	orderly_poweroff(true);
 }
 
 #define WARN_INTERVAL	(30 * HZ)
diff --git a/drivers/sbus/char/envctrl.c b/drivers/sbus/char/envctrl.c
index e244cf3d9ec8..5609b602c54d 100644
--- a/drivers/sbus/char/envctrl.c
+++ b/drivers/sbus/char/envctrl.c
@@ -970,18 +970,13 @@ static struct i2c_child_t *envctrl_get_i2c_child(unsigned char mon_type)
 static void envctrl_do_shutdown(void)
 {
 	static int inprog = 0;
-	int ret;
 
 	if (inprog != 0)
 		return;
 
 	inprog = 1;
 	printk(KERN_CRIT "kenvctrld: WARNING: Shutting down the system now.\n");
-	ret = orderly_poweroff(true);
-	if (ret < 0) {
-		printk(KERN_CRIT "kenvctrld: WARNING: system shutdown failed!\n"); 
-		inprog = 0;  /* unlikely to succeed, but we could try again */
-	}
+	orderly_poweroff(true);
 }
 
 static struct task_struct *kenvctrld_task;
diff --git a/drivers/scsi/be2iscsi/be_cmds.c b/drivers/scsi/be2iscsi/be_cmds.c
index 80d97f3d2ed9..1028760b8a22 100644
--- a/drivers/scsi/be2iscsi/be_cmds.c
+++ b/drivers/scsi/be2iscsi/be_cmds.c
@@ -237,7 +237,7 @@ int beiscsi_mccq_compl(struct beiscsi_hba *phba,
 			beiscsi_log(phba, KERN_WARNING,
 				    BEISCSI_LOG_INIT | BEISCSI_LOG_EH |
 				    BEISCSI_LOG_CONFIG,
-				    "BC_%d : Insufficent Buffer Error "
+				    "BC_%d : Insufficient Buffer Error "
 				    "Resp_Len : %d Actual_Resp_Len : %d\n",
 				    mbx_resp_hdr->response_length,
 				    mbx_resp_hdr->actual_resp_len);
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/ch.c b/drivers/scsi/ch.c
index 0045742fab7d..dad959fcf6d8 100644
--- a/drivers/scsi/ch.c
+++ b/drivers/scsi/ch.c
@@ -339,7 +339,7 @@ ch_readconfig(scsi_changer *ch)
 			ch->firsts[CHET_DT],
 			ch->counts[CHET_DT]);
 	} else {
-		VPRINTK(KERN_INFO, "reading element address assigment page failed!\n");
+		VPRINTK(KERN_INFO, "reading element address assignment page failed!\n");
 	}
 
 	/* vendor specific element types */
diff --git a/drivers/scsi/csiostor/csio_init.c b/drivers/scsi/csiostor/csio_init.c
index d9631e15f7b5..dbe416ff46c2 100644
--- a/drivers/scsi/csiostor/csio_init.c
+++ b/drivers/scsi/csiostor/csio_init.c
@@ -1172,7 +1172,7 @@ static struct pci_error_handlers csio_err_handler = {
  *  Macros needed to support the PCI Device ID Table ...
  */
 #define CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN \
-	static struct pci_device_id csio_pci_tbl[] = {
+	static const struct pci_device_id csio_pci_tbl[] = {
 /* Define for FCoE uses PF6 */
 #define CH_PCI_DEVICE_ID_FUNCTION	0x6
 
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/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index 488c3929f19a..0cccd6033feb 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -186,7 +186,7 @@ static int _osd_get_print_system_info(struct osd_dev *od,
 
 		if (unlikely(len > sizeof(odi->systemid))) {
 			OSD_ERR("OSD Target error: OSD_SYSTEM_ID too long(%d). "
-				"device idetification might not work\n", len);
+				"device identification might not work\n", len);
 			len = sizeof(odi->systemid);
 		}
 		odi->systemid_len = len;
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index e59f25bff7ab..5bb57c5282c9 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -5364,7 +5364,7 @@ qla2x00_load_risc(scsi_qla_host_t *vha, uint32_t *srisc_addr)
 	blob = qla2x00_request_firmware(vha);
 	if (!blob) {
 		ql_log(ql_log_info, vha, 0x0083,
-		    "Fimware image unavailable.\n");
+		    "Firmware image unavailable.\n");
 		ql_log(ql_log_info, vha, 0x0084,
 		    "Firmware images can be retrieved from: "QLA_FW_URL ".\n");
 		return QLA_FUNCTION_FAILED;
@@ -5467,7 +5467,7 @@ qla24xx_load_risc_blob(scsi_qla_host_t *vha, uint32_t *srisc_addr)
 	blob = qla2x00_request_firmware(vha);
 	if (!blob) {
 		ql_log(ql_log_warn, vha, 0x0090,
-		    "Fimware image unavailable.\n");
+		    "Firmware image unavailable.\n");
 		ql_log(ql_log_warn, vha, 0x0091,
 		    "Firmware images can be retrieved from: "
 		    QLA_FW_URL ".\n");
diff --git a/drivers/scsi/qla2xxx/qla_mid.c b/drivers/scsi/qla2xxx/qla_mid.c
index 5c2e0317f1c0..ca3804e34833 100644
--- a/drivers/scsi/qla2xxx/qla_mid.c
+++ b/drivers/scsi/qla2xxx/qla_mid.c
@@ -788,7 +788,7 @@ qla25xx_create_rsp_que(struct qla_hw_data *ha, uint16_t options,
 		rsp->msix = &ha->msix_entries[que_id + 1];
 	else
 		ql_log(ql_log_warn, base_vha, 0x00e3,
-		    "MSIX not enalbled.\n");
+		    "MSIX not enabled.\n");
 
 	ha->rsp_q_map[que_id] = rsp;
 	rsp->rid = rid;
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index 54cb2ac9339b..7d2b18f2675c 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -1274,7 +1274,7 @@ qla82xx_pinit_from_rom(scsi_qla_host_t *vha)
 
 		if (off == ADDR_ERROR) {
 			ql_log(ql_log_fatal, vha, 0x0116,
-			    "Unknow addr: 0x%08lx.\n", buf[i].addr);
+			    "Unknown addr: 0x%08lx.\n", buf[i].addr);
 			continue;
 		}
 
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index cce1cbc1a927..5319b3cb219e 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -446,11 +446,11 @@ static int qla25xx_setup_mode(struct scsi_qla_host *vha)
 		}
 		ha->flags.cpu_affinity_enabled = 1;
 		ql_dbg(ql_dbg_multiq, vha, 0xc007,
-		    "CPU affinity mode enalbed, "
+		    "CPU affinity mode enabled, "
 		    "no. of response queues:%d no. of request queues:%d.\n",
 		    ha->max_rsp_queues, ha->max_req_queues);
 		ql_dbg(ql_dbg_init, vha, 0x00e9,
-		    "CPU affinity mode enalbed, "
+		    "CPU affinity mode enabled, "
 		    "no. of response queues:%d no. of request queues:%d.\n",
 		    ha->max_rsp_queues, ha->max_req_queues);
 	}
@@ -4102,7 +4102,7 @@ qla83xx_schedule_work(scsi_qla_host_t *base_vha, int work_code)
 		break;
 	default:
 		ql_log(ql_log_warn, base_vha, 0xb05f,
-		    "Unknow work-code=0x%x.\n", work_code);
+		    "Unknown work-code=0x%x.\n", work_code);
 	}
 
 	return;
@@ -4702,7 +4702,7 @@ qla83xx_idc_state_handler(scsi_qla_host_t *base_vha)
 			break;
 		default:
 			ql_log(ql_log_warn, base_vha, 0xb071,
-			    "Unknow Device State: %x.\n", dev_state);
+			    "Unknown Device State: %x.\n", dev_state);
 			qla83xx_idc_unlock(base_vha, 0);
 			qla8xxx_dev_failed_handler(base_vha);
 			rval = QLA_FUNCTION_FAILED;
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/sg.c b/drivers/scsi/sg.c
index 2270bd51f9c2..9d7b7db75e4b 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -33,7 +33,6 @@ static int sg_version_num = 30536;	/* 2 digits for each component */
 #include <linux/sched.h>
 #include <linux/string.h>
 #include <linux/mm.h>
-#include <linux/aio.h>
 #include <linux/errno.h>
 #include <linux/mtio.h>
 #include <linux/ioctl.h>
@@ -51,6 +50,7 @@ static int sg_version_num = 30536;	/* 2 digits for each component */
 #include <linux/mutex.h>
 #include <linux/atomic.h>
 #include <linux/ratelimit.h>
+#include <linux/uio.h>
 
 #include "scsi.h"
 #include <scsi/scsi_dbg.h>
@@ -1745,17 +1745,14 @@ sg_start_req(Sg_request *srp, unsigned char *cmd)
 	}
 
 	if (iov_count) {
-		int size = sizeof(struct iovec) * iov_count;
-		struct iovec *iov;
+		struct iovec *iov = NULL;
 		struct iov_iter i;
 
-		iov = memdup_user(hp->dxferp, size);
-		if (IS_ERR(iov))
-			return PTR_ERR(iov);
+		res = import_iovec(rw, hp->dxferp, iov_count, 0, &iov, &i);
+		if (res < 0)
+			return res;
 
-		iov_iter_init(&i, rw, iov, iov_count,
-			      min_t(size_t, hp->dxfer_len,
-				    iov_length(iov, iov_count)));
+		iov_iter_truncate(&i, hp->dxfer_len);
 
 		res = blk_rq_map_user_iov(q, rq, md, &i, GFP_ATOMIC);
 		kfree(iov);
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/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 06de34001c66..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.
  */
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 3ce39d10fafb..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,35 +140,33 @@ 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)
@@ -156,35 +186,33 @@ 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)
@@ -196,37 +224,36 @@ static struct dma_async_tx_descriptor *dw_spi_dma_prepare_rx(struct dw_spi *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_writel(dws, DW_SPI_DMARDLR, 0xf);
+	dw_writel(dws, DW_SPI_DMATDLR, 0x10);
 
-	dw_writew(dws, DW_SPI_DMARDLR, 0xf);
-	dw_writew(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);
 
-	spi_enable_chip(dws, 1);
+	dws->transfer_handler = dma_transfer;
+
+	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);
+	/* Prepare the TX dma transfer */
+	txdesc = dw_spi_dma_prepare_tx(dws, xfer);
 
-	/* 2. Prepare the TX dma transfer */
-	txdesc = dw_spi_dma_prepare_tx(dws);
-
-	/* 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) {
@@ -244,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
 
@@ -280,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.c b/drivers/spi/spi-dw.c
index 4847afba89f4..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,
@@ -622,11 +499,11 @@ static void spi_hw_init(struct device *dev, struct dw_spi *dws)
 		u32 fifo;
 
 		for (fifo = 1; fifo < 256; fifo++) {
-			dw_writew(dws, DW_SPI_TXFLTR, fifo);
-			if (fifo != dw_readw(dws, DW_SPI_TXFLTR))
+			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 == 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 e649bc7d4c08..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,8 +499,6 @@ 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;
 
 	if (xfer->len > SPFI_TRANSACTION_TSIZE_MASK) {
@@ -466,23 +508,6 @@ static int img_spfi_transfer_one(struct spi_master *master,
 		return -EINVAL;
 	}
 
-	/*
-	 * 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);
-	}
-
 	img_spfi_config(master, spi, xfer);
 	if (master->can_dma && master->can_dma(master, spi, xfer))
 		ret = img_spfi_start_dma(master, spi, xfer);
@@ -492,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)
 {
@@ -591,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 ee513a85296b..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
 
@@ -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.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/ssb/driver_pcicore.c b/drivers/ssb/driver_pcicore.c
index d75b72ba2672..15a7ee3859dd 100644
--- a/drivers/ssb/driver_pcicore.c
+++ b/drivers/ssb/driver_pcicore.c
@@ -357,6 +357,15 @@ static void ssb_pcicore_init_hostmode(struct ssb_pcicore *pc)
 	pcicore_write32(pc, SSB_PCICORE_SBTOPCI2,
 			SSB_PCICORE_SBTOPCI_MEM | SSB_PCI_DMA);
 
+	/*
+	 * Accessing PCI config without a proper delay after devices reset (not
+	 * GPIO reset) was causing reboots on WRT300N v1.0.
+	 * Tested delay 850 us lowered reboot chance to 50-80%, 1000 us fixed it
+	 * completely. Flushing all writes was also tested but with no luck.
+	 */
+	if (pc->dev->bus->chip_id == 0x4704)
+		usleep_range(1000, 2000);
+
 	/* Enable PCI bridge BAR0 prefetch and burst */
 	val = PCI_COMMAND_MASTER | PCI_COMMAND_MEMORY;
 	ssb_extpci_write_config(pc, 0, 0, 0, PCI_COMMAND, &val, 2);
diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c
index 1e180c400f17..a48a7439a206 100644
--- a/drivers/ssb/main.c
+++ b/drivers/ssb/main.c
@@ -1135,6 +1135,8 @@ static u32 ssb_tmslow_reject_bitmask(struct ssb_device *dev)
 	case SSB_IDLOW_SSBREV_25:     /* TODO - find the proper REJECT bit */
 	case SSB_IDLOW_SSBREV_27:     /* same here */
 		return SSB_TMSLOW_REJECT;	/* this is a guess */
+	case SSB_IDLOW_SSBREV:
+		break;
 	default:
 		WARN(1, KERN_INFO "ssb: Backplane Revision 0x%.8X\n", rev);
 	}
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/ashmem.c b/drivers/staging/android/ashmem.c
index d140b733940c..c5c037ccf32c 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -310,7 +310,7 @@ static ssize_t ashmem_read(struct file *file, char __user *buf,
 	 * be destroyed until all references to the file are dropped and
 	 * ashmem_release is called.
 	 */
-	ret = asma->file->f_op->read(asma->file, buf, len, pos);
+	ret = __vfs_read(asma->file, buf, len, pos);
 	if (ret >= 0) {
 		/** Update backing file pos, since f_ops->read() doesn't */
 		asma->file->f_pos = *pos;
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 3f72451d2de0..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,
@@ -508,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,
@@ -646,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);
 
@@ -767,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;
 	}
 
@@ -786,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);
 
@@ -798,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;
@@ -815,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);
@@ -828,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 */
 
@@ -856,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;
@@ -877,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)
@@ -898,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 */
@@ -977,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++;
@@ -1073,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 e856f01ca077..6ba71d114a95 100644
--- a/drivers/staging/comedi/drivers/comedi_isadma.c
+++ b/drivers/staging/comedi/drivers/comedi_isadma.c
@@ -235,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..304ebff119ee 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;
@@ -110,24 +108,16 @@ static int serial2002_tty_write(struct file *f, unsigned char *buf, int count)
 {
 	const char __user *p = (__force const char __user *)buf;
 	int result;
+	loff_t offset = 0;
 	mm_segment_t oldfs;
 
 	oldfs = get_fs();
 	set_fs(KERNEL_DS);
-	f->f_pos = 0;
-	result = f->f_op->write(f, p, count, &f->f_pos);
+	result = __vfs_write(f, p, count, &offset);
 	set_fs(oldfs);
 	return result;
 }
 
-static int serial2002_tty_readb(struct file *f, unsigned char *buf)
-{
-	char __user *p = (__force char __user *)buf;
-
-	f->f_pos = 0;
-	return f->f_op->read(f, p, 1, &f->f_pos);
-}
-
 static void serial2002_tty_read_poll_wait(struct file *f, int timeout)
 {
 	struct poll_wqueues table;
@@ -145,8 +135,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);
@@ -163,13 +153,15 @@ static int serial2002_tty_read(struct file *f, int timeout)
 	result = -1;
 	if (!IS_ERR(f)) {
 		mm_segment_t oldfs;
+		char __user *p = (__force char __user *)&ch;
+		loff_t offset = 0;
 
 		oldfs = get_fs();
 		set_fs(KERNEL_DS);
 		if (f->f_op->poll) {
 			serial2002_tty_read_poll_wait(f, timeout);
 
-			if (serial2002_tty_readb(f, &ch) == 1)
+			if (__vfs_read(f, p, 1, &offset) == 1)
 				result = ch;
 		} else {
 			/* Device does not support poll, busy wait */
@@ -180,7 +172,7 @@ static int serial2002_tty_read(struct file *f, int timeout)
 				if (retries >= timeout)
 					break;
 
-				if (serial2002_tty_readb(f, &ch) == 1) {
+				if (__vfs_read(f, p, 1, &offset) == 1) {
 					result = ch;
 					break;
 				}
@@ -300,7 +292,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/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 816174388f13..d7c5223f1c3e 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -243,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;
@@ -268,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 | \
@@ -295,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
@@ -308,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) \
@@ -477,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));
@@ -532,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));
 }
@@ -850,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);
@@ -993,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);
 
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/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/Kconfig b/drivers/staging/lustre/lustre/Kconfig
index 6725467ef4d0..62c7bba75274 100644
--- a/drivers/staging/lustre/lustre/Kconfig
+++ b/drivers/staging/lustre/lustre/Kconfig
@@ -10,6 +10,7 @@ config LUSTRE_FS
 	select CRYPTO_SHA1
 	select CRYPTO_SHA256
 	select CRYPTO_SHA512
+	depends on MULTIUSER
 	help
 	  This option enables Lustre file system client support. Choose Y
 	  here if you want to access a Lustre file system cluster. To compile
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/linux/lustre_patchless_compat.h b/drivers/staging/lustre/lustre/include/linux/lustre_patchless_compat.h
index a260e99a4447..d72605864b0a 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_patchless_compat.h
+++ b/drivers/staging/lustre/lustre/include/linux/lustre_patchless_compat.h
@@ -55,7 +55,9 @@ truncate_complete_page(struct address_space *mapping, struct page *page)
 	if (PagePrivate(page))
 		page->mapping->a_ops->invalidatepage(page, 0, PAGE_CACHE_SIZE);
 
-	cancel_dirty_page(page, PAGE_SIZE);
+	if (TestClearPageDirty(page))
+		account_page_cleaned(page, mapping);
+
 	ClearPageMappedToDisk(page);
 	ll_delete_from_page_cache(page);
 }
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..529062ea112b 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),
@@ -3128,9 +3135,7 @@ int ll_inode_permission(struct inode *inode, int mask)
 
 /* -o localflock - only provides locally consistent flock locks */
 struct file_operations ll_file_operations = {
-	.read	   = new_sync_read,
 	.read_iter = ll_file_read_iter,
-	.write	  = new_sync_write,
 	.write_iter = ll_file_write_iter,
 	.unlocked_ioctl = ll_file_ioctl,
 	.open	   = ll_file_open,
@@ -3143,9 +3148,7 @@ struct file_operations ll_file_operations = {
 };
 
 struct file_operations ll_file_operations_flock = {
-	.read	   = new_sync_read,
 	.read_iter    = ll_file_read_iter,
-	.write	  = new_sync_write,
 	.write_iter   = ll_file_write_iter,
 	.unlocked_ioctl = ll_file_ioctl,
 	.open	   = ll_file_open,
@@ -3161,9 +3164,7 @@ struct file_operations ll_file_operations_flock = {
 
 /* These are for -o noflock - to return ENOSYS on flock calls */
 struct file_operations ll_file_operations_noflock = {
-	.read	   = new_sync_read,
 	.read_iter    = ll_file_read_iter,
-	.write	  = new_sync_write,
 	.write_iter   = ll_file_write_iter,
 	.unlocked_ioctl = ll_file_ioctl,
 	.open	   = ll_file_open,
@@ -3233,6 +3234,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 +3259,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_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 2af1d7286250..e7422f5c9c6f 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -938,10 +938,8 @@ struct ll_cl_context {
 };
 
 struct vvp_thread_info {
-	struct iovec	 vti_local_iov;
 	struct vvp_io_args   vti_args;
 	struct ra_io_arg     vti_ria;
-	struct kiocb	 vti_kiocb;
 	struct ll_cl_context vti_io_ctx;
 };
 
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..bc95ce89af06 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",
@@ -2580,6 +2574,7 @@ static const struct net_device_ops rtw_cfg80211_monitor_if_ops = {
 };
 
 static int rtw_cfg80211_add_monitor_if(struct rtw_adapter *padapter, char *name,
+				       unsigned char name_assign_type,
 				       struct net_device **ndev)
 {
 	int ret = 0;
@@ -2612,6 +2607,7 @@ static int rtw_cfg80211_add_monitor_if(struct rtw_adapter *padapter, char *name,
 	mon_ndev->type = ARPHRD_IEEE80211_RADIOTAP;
 	strncpy(mon_ndev->name, name, IFNAMSIZ);
 	mon_ndev->name[IFNAMSIZ - 1] = 0;
+	mon_ndev->name_assign_type = name_assign_type;
 	mon_ndev->destructor = rtw_ndev_destructor;
 
 	mon_ndev->netdev_ops = &rtw_cfg80211_monitor_if_ops;
@@ -2654,6 +2650,7 @@ out:
 
 static struct wireless_dev *
 cfg80211_rtw_add_virtual_intf(struct wiphy *wiphy, const char *name,
+			      unsigned char name_assign_type,
 			      enum nl80211_iftype type, u32 *flags,
 			      struct vif_params *params)
 {
@@ -2673,7 +2670,8 @@ cfg80211_rtw_add_virtual_intf(struct wiphy *wiphy, const char *name,
 		break;
 	case NL80211_IFTYPE_MONITOR:
 		ret =
-		    rtw_cfg80211_add_monitor_if(padapter, (char *)name, &ndev);
+		    rtw_cfg80211_add_monitor_if(padapter, (char *)name,
+						name_assign_type, &ndev);
 		break;
 
 	case NL80211_IFTYPE_P2P_CLIENT:
@@ -2877,7 +2875,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 +3051,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..cde2494ad896 100644
--- a/drivers/staging/unisys/include/timskmod.h
+++ b/drivers/staging/unisys/include/timskmod.h
@@ -46,7 +46,6 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/fcntl.h>
-#include <linux/aio.h>
 #include <linux/workqueue.h>
 #include <linux/kthread.h>
 #include <linux/seq_file.h>
@@ -68,15 +67,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 +77,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 +89,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/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 174d3bcf8bd7..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;
 	}
 
diff --git a/drivers/tty/goldfish.c b/drivers/tty/goldfish.c
index 967b2c2b7cf1..0655fecf8240 100644
--- a/drivers/tty/goldfish.c
+++ b/drivers/tty/goldfish.c
@@ -229,7 +229,6 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 {
 	struct goldfish_tty *qtty;
 	int ret = -EINVAL;
-	int i;
 	struct resource *r;
 	struct device *ttydev;
 	void __iomem *base;
@@ -293,7 +292,6 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 	mutex_unlock(&goldfish_tty_lock);
 	return 0;
 
-	tty_unregister_device(goldfish_tty_driver, i);
 err_tty_register_device_failed:
 	free_irq(irq, pdev);
 err_request_irq_failed:
diff --git a/drivers/tty/ipwireless/hardware.c b/drivers/tty/ipwireless/hardware.c
index 5c77e1eac4ee..ad7031a4f3c4 100644
--- a/drivers/tty/ipwireless/hardware.c
+++ b/drivers/tty/ipwireless/hardware.c
@@ -1455,7 +1455,7 @@ static void __handle_setup_get_version_rsp(struct ipw_hardware *hw)
 			return;
 		}
 
-		set_RTS(hw, PRIO_SETUP, channel_idx,
+		ret = set_RTS(hw, PRIO_SETUP, channel_idx,
 			(hw->control_lines [channel_idx] &
 			 IPW_CONTROL_LINE_RTS) != 0);
 		if (ret) {
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index 2ab229ddee38..6ae5b8560e4d 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -119,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);
+		 */
 	}
 }
 
@@ -163,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 */
@@ -187,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);
+		 */
 	}
 }
 
diff --git a/drivers/tty/serial/8250/8250_fintek.c b/drivers/tty/serial/8250/8250_fintek.c
index 1e6899bc9429..5815e81b5fc6 100644
--- a/drivers/tty/serial/8250/8250_fintek.c
+++ b/drivers/tty/serial/8250/8250_fintek.c
@@ -234,18 +234,7 @@ static struct pnp_driver fintek_8250_driver = {
 	.id_table	= fintek_dev_table,
 };
 
-static int fintek_8250_init(void)
-{
-	return pnp_register_driver(&fintek_8250_driver);
-}
-module_init(fintek_8250_init);
-
-static void fintek_8250_exit(void)
-{
-	pnp_unregister_driver(&fintek_8250_driver);
-}
-module_exit(fintek_8250_exit);
-
+module_pnp_driver(fintek_8250_driver);
 MODULE_DESCRIPTION("Fintek F812164 module");
 MODULE_AUTHOR("Ricardo Ribalda <ricardo.ribalda@gmail.com>");
 MODULE_LICENSE("GPL");
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/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/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/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 683617714e7c..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);
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 11635537c052..4b0448c26810 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -2408,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 52e0c4e5e48e..edba5348be18 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -325,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;
@@ -600,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/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 175c9956cbe3..6bdb57069044 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -23,6 +23,7 @@
 #include <linux/export.h>
 #include <linux/hid.h>
 #include <linux/module.h>
+#include <linux/uio.h>
 #include <asm/unaligned.h>
 
 #include <linux/usb/composite.h>
@@ -655,9 +656,10 @@ static void ffs_user_copy_worker(struct work_struct *work)
 		unuse_mm(io_data->mm);
 	}
 
-	aio_complete(io_data->kiocb, ret, ret);
+	io_data->kiocb->ki_complete(io_data->kiocb, ret, ret);
 
-	if (io_data->ffs->ffs_eventfd && !io_data->kiocb->ki_eventfd)
+	if (io_data->ffs->ffs_eventfd &&
+	    !(io_data->kiocb->ki_flags & IOCB_EVENTFD))
 		eventfd_signal(io_data->ffs->ffs_eventfd, 1);
 
 	usb_ep_free_request(io_data->ep, io_data->req);
@@ -1059,8 +1061,6 @@ static const struct file_operations ffs_epfile_operations = {
 	.llseek =	no_llseek,
 
 	.open =		ffs_epfile_open,
-	.write =	new_sync_write,
-	.read =		new_sync_read,
 	.write_iter =	ffs_epfile_write_iter,
 	.read_iter =	ffs_epfile_read_iter,
 	.release =	ffs_epfile_release,
diff --git a/drivers/usb/gadget/function/f_hid.c b/drivers/usb/gadget/function/f_hid.c
index a2612fb79eff..13dfc9915b1d 100644
--- a/drivers/usb/gadget/function/f_hid.c
+++ b/drivers/usb/gadget/function/f_hid.c
@@ -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_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 e3dae47baef3..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),
-};
-
-static 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),
-};
-
-static 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/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/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/inode.c b/drivers/usb/gadget/legacy/inode.c
index 200f9a584064..6af58c6dba5e 100644
--- a/drivers/usb/gadget/legacy/inode.c
+++ b/drivers/usb/gadget/legacy/inode.c
@@ -26,6 +26,7 @@
 #include <linux/poll.h>
 #include <linux/mmu_context.h>
 #include <linux/aio.h>
+#include <linux/uio.h>
 
 #include <linux/device.h>
 #include <linux/moduleparam.h>
@@ -469,7 +470,7 @@ static void ep_user_copy_worker(struct work_struct *work)
 		ret = -EFAULT;
 
 	/* completing the iocb can drop the ctx and mm, don't touch mm after */
-	aio_complete(iocb, ret, ret);
+	iocb->ki_complete(iocb, ret, ret);
 
 	kfree(priv->buf);
 	kfree(priv->to_free);
@@ -497,7 +498,8 @@ static void ep_aio_complete(struct usb_ep *ep, struct usb_request *req)
 		kfree(priv);
 		iocb->private = NULL;
 		/* aio_complete() reports bytes-transferred _and_ faults */
-		aio_complete(iocb, req->actual ? req->actual : req->status,
+
+		iocb->ki_complete(iocb, req->actual ? req->actual : req->status,
 				req->status);
 	} else {
 		/* ep_copy_to_user() won't report both; we hide some faults */
@@ -697,8 +699,6 @@ static const struct file_operations ep_io_operations = {
 	.open =		ep_open,
 	.release =	ep_release,
 	.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,
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 fd53c9ebd662..2af32e26fafc 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -115,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) {
@@ -130,7 +131,6 @@ 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) {
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 0e11d61408ff..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>
 
@@ -155,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);
 
@@ -184,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 5fb66db89e05..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]);
@@ -1729,7 +1729,7 @@ static void xhci_cleanup_halted_endpoint(struct xhci_hcd *xhci,
 	if (!command)
 		return;
 
-	ep->ep_state |= EP_HALTED | EP_RECENTLY_HALTED;
+	ep->ep_state |= EP_HALTED;
 	ep->stopped_stream = stream_id;
 
 	xhci_queue_reset_ep(xhci, command, slot_id, ep_index);
@@ -2809,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-trace.h b/drivers/usb/host/xhci-trace.h
index dde3959b7a33..59c05653b2ea 100644
--- a/drivers/usb/host/xhci-trace.h
+++ b/drivers/usb/host/xhci-trace.h
@@ -14,6 +14,13 @@
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM xhci-hcd
 
+/*
+ * The TRACE_SYSTEM_VAR defaults to TRACE_SYSTEM, but must be a
+ * legitimate C variable. It is not exported to user space.
+ */
+#undef TRACE_SYSTEM_VAR
+#define TRACE_SYSTEM_VAR xhci_hcd
+
 #if !defined(__XHCI_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
 #define __XHCI_TRACE_H
 
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index b06d1a53652d..ec8ac1674854 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -1338,12 +1338,6 @@ int xhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
 		goto exit;
 	}
 
-	/* Reject urb if endpoint is in soft reset, queue must stay empty */
-	if (xhci->devs[slot_id]->eps[ep_index].ep_state & EP_CONFIG_PENDING) {
-		xhci_warn(xhci, "Can't enqueue URB while ep is in soft reset\n");
-		ret = -EINVAL;
-	}
-
 	if (usb_endpoint_xfer_isoc(&urb->ep->desc))
 		size = urb->number_of_packets;
 	else
@@ -2954,36 +2948,23 @@ void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
 	}
 }
 
-/* Called after clearing a halted device. USB core should have sent the control
+/* Called when clearing halted device. The core should have sent the control
  * message to clear the device halt condition. The host side of the halt should
- * already be cleared with a reset endpoint command issued immediately when the
- * STALL tx event was received.
+ * already be cleared with a reset endpoint command issued when the STALL tx
+ * event was received.
+ *
+ * Context: in_interrupt
  */
 
 void xhci_endpoint_reset(struct usb_hcd *hcd,
 		struct usb_host_endpoint *ep)
 {
 	struct xhci_hcd *xhci;
-	struct usb_device *udev;
-	struct xhci_virt_device *virt_dev;
-	struct xhci_virt_ep *virt_ep;
-	struct xhci_input_control_ctx *ctrl_ctx;
-	struct xhci_command *command;
-	unsigned int ep_index, ep_state;
-	unsigned long flags;
-	u32 ep_flag;
 
 	xhci = hcd_to_xhci(hcd);
-	udev = (struct usb_device *) ep->hcpriv;
-	if (!ep->hcpriv)
-		return;
-	virt_dev = xhci->devs[udev->slot_id];
-	ep_index = xhci_get_endpoint_index(&ep->desc);
-	virt_ep = &virt_dev->eps[ep_index];
-	ep_state = virt_ep->ep_state;
 
 	/*
-	 * Implement the config ep command in xhci 4.6.8 additional note:
+	 * We might need to implement the config ep cmd in xhci 4.8.1 note:
 	 * The Reset Endpoint Command may only be issued to endpoints in the
 	 * Halted state. If software wishes reset the Data Toggle or Sequence
 	 * Number of an endpoint that isn't in the Halted state, then software
@@ -2991,72 +2972,9 @@ void xhci_endpoint_reset(struct usb_hcd *hcd,
 	 * for the target endpoint. that is in the Stopped state.
 	 */
 
-	if (ep_state & SET_DEQ_PENDING || ep_state & EP_RECENTLY_HALTED) {
-		virt_ep->ep_state &= ~EP_RECENTLY_HALTED;
-		xhci_dbg(xhci, "ep recently halted, no toggle reset needed\n");
-		return;
-	}
-
-	/* Only interrupt and bulk ep's use Data toggle, USB2 spec 5.5.4-> */
-	if (usb_endpoint_xfer_control(&ep->desc) ||
-	    usb_endpoint_xfer_isoc(&ep->desc))
-		return;
-
-	ep_flag = xhci_get_endpoint_flag(&ep->desc);
-
-	if (ep_flag == SLOT_FLAG || ep_flag == EP0_FLAG)
-		return;
-
-	command = xhci_alloc_command(xhci, true, true, GFP_NOWAIT);
-	if (!command) {
-		xhci_err(xhci, "Could not allocate xHCI command structure.\n");
-		return;
-	}
-
-	spin_lock_irqsave(&xhci->lock, flags);
-
-	/* block ringing ep doorbell */
-	virt_ep->ep_state |= EP_CONFIG_PENDING;
-
-	/*
-	 * Make sure endpoint ring is empty before resetting the toggle/seq.
-	 * Driver is required to synchronously cancel all transfer request.
-	 *
-	 * xhci 4.6.6 says we can issue a configure endpoint command on a
-	 * running endpoint ring as long as it's idle (queue empty)
-	 */
-
-	if (!list_empty(&virt_ep->ring->td_list)) {
-		dev_err(&udev->dev, "EP not empty, refuse reset\n");
-		spin_unlock_irqrestore(&xhci->lock, flags);
-		goto cleanup;
-	}
-
-	xhci_dbg(xhci, "Reset toggle/seq for slot %d, ep_index: %d\n",
-		 udev->slot_id, ep_index);
-
-	ctrl_ctx = xhci_get_input_control_ctx(command->in_ctx);
-	if (!ctrl_ctx) {
-		xhci_err(xhci, "Could not get input context, bad type. virt_dev: %p, in_ctx %p\n",
-			 virt_dev, virt_dev->in_ctx);
-		spin_unlock_irqrestore(&xhci->lock, flags);
-		goto cleanup;
-	}
-	xhci_setup_input_ctx_for_config_ep(xhci, command->in_ctx,
-					   virt_dev->out_ctx, ctrl_ctx,
-					   ep_flag, ep_flag);
-	xhci_endpoint_copy(xhci, command->in_ctx, virt_dev->out_ctx, ep_index);
-
-	xhci_queue_configure_endpoint(xhci, command, command->in_ctx->dma,
-				     udev->slot_id, false);
-	xhci_ring_cmd_db(xhci);
-	spin_unlock_irqrestore(&xhci->lock, flags);
-
-	wait_for_completion(command->completion);
-
-cleanup:
-	virt_ep->ep_state &= ~EP_CONFIG_PENDING;
-	xhci_free_command(xhci, command);
+	/* For now just print debug to follow the situation */
+	xhci_dbg(xhci, "Endpoint 0x%x ep reset callback called\n",
+		 ep->desc.bEndpointAddress);
 }
 
 static int xhci_check_streams_endpoint(struct xhci_hcd *xhci,
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 265ab1771d24..8e421b89632d 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -865,8 +865,6 @@ struct xhci_virt_ep {
 #define EP_HAS_STREAMS		(1 << 4)
 /* Transitioning the endpoint to not using streams, don't enqueue URBs */
 #define EP_GETTING_NO_STREAMS	(1 << 5)
-#define EP_RECENTLY_HALTED	(1 << 6)
-#define EP_CONFIG_PENDING	(1 << 7)
 	/* ----  Related to URB cancellation ---- */
 	struct list_head	cancelled_td_list;
 	struct xhci_td		*stopped_td;
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 3cb98b1d5d29..ac31d19cc54b 100644
--- a/drivers/usb/isp1760/isp1760-hcd.c
+++ b/drivers/usb/isp1760/isp1760-hcd.c
@@ -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 067920f2d570..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,14 +978,33 @@ 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);
 }
 
 /*
@@ -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;
 
-	ret  = musb_platform_reset(musb);
-	if (ret)
+	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_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);
 }
 
@@ -2087,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);
 
@@ -2183,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)
@@ -2249,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);
@@ -2463,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));
 	}
 
 	/*
@@ -2506,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 a900c9877195..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,7 +432,7 @@ 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);
 
@@ -497,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);
@@ -571,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;
@@ -622,37 +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, error;
+	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);
-		error = phy_power_off(musb->phy);
-		if (error)
-			dev_err(dev, "phy shutdown failed: %i\n", error);
-		usleep_range(100, 200);
-		usb_phy_init(musb->xceiv);
-		error = phy_power_on(musb->phy);
-		if (error)
-			dev_err(dev, "phy powerup failed: %i\n", error);
+		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 = {
@@ -665,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);
@@ -737,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;
 
@@ -802,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)
@@ -873,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);
@@ -929,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_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/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/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/ch341.c b/drivers/usb/serial/ch341.c
index ede4f5fcfadd..c73808f095bb 100644
--- a/drivers/usb/serial/ch341.c
+++ b/drivers/usb/serial/ch341.c
@@ -325,7 +325,6 @@ static int ch341_open(struct tty_struct *tty, struct usb_serial_port *port)
 	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/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 3086dec0ef53..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:
 	 */
@@ -1883,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 56b1b55c4751..4e4f46f3c89c 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -561,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 **/
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/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 82570425fdfe..c85ea530085f 100644
--- a/drivers/usb/storage/unusual_uas.h
+++ b/drivers/usb/storage/unusual_uas.h
@@ -113,6 +113,13 @@ 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",
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/Kconfig b/drivers/vfio/Kconfig
index 14e27ab32456..7d092ddc8119 100644
--- a/drivers/vfio/Kconfig
+++ b/drivers/vfio/Kconfig
@@ -13,6 +13,11 @@ config VFIO_SPAPR_EEH
 	depends on EEH && VFIO_IOMMU_SPAPR_TCE
 	default n
 
+config VFIO_VIRQFD
+	tristate
+	depends on VFIO && EVENTFD
+	default n
+
 menuconfig VFIO
 	tristate "VFIO Non-Privileged userspace driver framework"
 	depends on IOMMU_API
@@ -27,3 +32,4 @@ menuconfig VFIO
 	  If you don't know what to do here, say N.
 
 source "drivers/vfio/pci/Kconfig"
+source "drivers/vfio/platform/Kconfig"
diff --git a/drivers/vfio/Makefile b/drivers/vfio/Makefile
index 0b035b12600a..7b8a31f63fea 100644
--- a/drivers/vfio/Makefile
+++ b/drivers/vfio/Makefile
@@ -1,5 +1,9 @@
+vfio_virqfd-y := virqfd.o
+
 obj-$(CONFIG_VFIO) += vfio.o
+obj-$(CONFIG_VFIO_VIRQFD) += vfio_virqfd.o
 obj-$(CONFIG_VFIO_IOMMU_TYPE1) += vfio_iommu_type1.o
 obj-$(CONFIG_VFIO_IOMMU_SPAPR_TCE) += vfio_iommu_spapr_tce.o
 obj-$(CONFIG_VFIO_SPAPR_EEH) += vfio_spapr_eeh.o
 obj-$(CONFIG_VFIO_PCI) += pci/
+obj-$(CONFIG_VFIO_PLATFORM) += platform/
diff --git a/drivers/vfio/pci/Kconfig b/drivers/vfio/pci/Kconfig
index c6bb5da2d2a7..579d83bf5358 100644
--- a/drivers/vfio/pci/Kconfig
+++ b/drivers/vfio/pci/Kconfig
@@ -1,6 +1,7 @@
 config VFIO_PCI
 	tristate "VFIO support for PCI devices"
 	depends on VFIO && PCI && EVENTFD
+	select VFIO_VIRQFD
 	help
 	  Support for the PCI VFIO bus driver.  This is required to make
 	  use of PCI drivers using the VFIO framework.
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index f8a186381ae8..69fab0fd15ae 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -11,6 +11,8 @@
  * Author: Tom Lyon, pugs@cisco.com
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/device.h>
 #include <linux/eventfd.h>
 #include <linux/file.h>
@@ -25,6 +27,7 @@
 #include <linux/types.h>
 #include <linux/uaccess.h>
 #include <linux/vfio.h>
+#include <linux/vgaarb.h>
 
 #include "vfio_pci_private.h"
 
@@ -32,13 +35,81 @@
 #define DRIVER_AUTHOR   "Alex Williamson <alex.williamson@redhat.com>"
 #define DRIVER_DESC     "VFIO PCI - User Level meta-driver"
 
+static char ids[1024] __initdata;
+module_param_string(ids, ids, sizeof(ids), 0);
+MODULE_PARM_DESC(ids, "Initial PCI IDs to add to the vfio driver, format is \"vendor:device[:subvendor[:subdevice[:class[:class_mask]]]]\" and multiple comma separated entries can be specified");
+
 static bool nointxmask;
 module_param_named(nointxmask, nointxmask, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(nointxmask,
 		  "Disable support for PCI 2.3 style INTx masking.  If this resolves problems for specific devices, report lspci -vvvxxx to linux-pci@vger.kernel.org so the device can be fixed automatically via the broken_intx_masking flag.");
 
+#ifdef CONFIG_VFIO_PCI_VGA
+static bool disable_vga;
+module_param(disable_vga, bool, S_IRUGO);
+MODULE_PARM_DESC(disable_vga, "Disable VGA resource access through vfio-pci");
+#endif
+
+static bool disable_idle_d3;
+module_param(disable_idle_d3, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(disable_idle_d3,
+		 "Disable using the PCI D3 low power state for idle, unused devices");
+
 static DEFINE_MUTEX(driver_lock);
 
+static inline bool vfio_vga_disabled(void)
+{
+#ifdef CONFIG_VFIO_PCI_VGA
+	return disable_vga;
+#else
+	return true;
+#endif
+}
+
+/*
+ * Our VGA arbiter participation is limited since we don't know anything
+ * about the device itself.  However, if the device is the only VGA device
+ * downstream of a bridge and VFIO VGA support is disabled, then we can
+ * safely return legacy VGA IO and memory as not decoded since the user
+ * has no way to get to it and routing can be disabled externally at the
+ * bridge.
+ */
+static unsigned int vfio_pci_set_vga_decode(void *opaque, bool single_vga)
+{
+	struct vfio_pci_device *vdev = opaque;
+	struct pci_dev *tmp = NULL, *pdev = vdev->pdev;
+	unsigned char max_busnr;
+	unsigned int decodes;
+
+	if (single_vga || !vfio_vga_disabled() || pci_is_root_bus(pdev->bus))
+		return VGA_RSRC_NORMAL_IO | VGA_RSRC_NORMAL_MEM |
+		       VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM;
+
+	max_busnr = pci_bus_max_busnr(pdev->bus);
+	decodes = VGA_RSRC_NORMAL_IO | VGA_RSRC_NORMAL_MEM;
+
+	while ((tmp = pci_get_class(PCI_CLASS_DISPLAY_VGA << 8, tmp)) != NULL) {
+		if (tmp == pdev ||
+		    pci_domain_nr(tmp->bus) != pci_domain_nr(pdev->bus) ||
+		    pci_is_root_bus(tmp->bus))
+			continue;
+
+		if (tmp->bus->number >= pdev->bus->number &&
+		    tmp->bus->number <= max_busnr) {
+			pci_dev_put(tmp);
+			decodes |= VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM;
+			break;
+		}
+	}
+
+	return decodes;
+}
+
+static inline bool vfio_pci_is_vga(struct pci_dev *pdev)
+{
+	return (pdev->class >> 8) == PCI_CLASS_DISPLAY_VGA;
+}
+
 static void vfio_pci_try_bus_reset(struct vfio_pci_device *vdev);
 
 static int vfio_pci_enable(struct vfio_pci_device *vdev)
@@ -48,6 +119,8 @@ static int vfio_pci_enable(struct vfio_pci_device *vdev)
 	u16 cmd;
 	u8 msix_pos;
 
+	pci_set_power_state(pdev, PCI_D0);
+
 	/* Don't allow our initial saved state to include busmaster */
 	pci_clear_master(pdev);
 
@@ -93,10 +166,8 @@ static int vfio_pci_enable(struct vfio_pci_device *vdev)
 	} else
 		vdev->msix_bar = 0xFF;
 
-#ifdef CONFIG_VFIO_PCI_VGA
-	if ((pdev->class >> 8) == PCI_CLASS_DISPLAY_VGA)
+	if (!vfio_vga_disabled() && vfio_pci_is_vga(pdev))
 		vdev->has_vga = true;
-#endif
 
 	return 0;
 }
@@ -153,20 +224,17 @@ static void vfio_pci_disable(struct vfio_pci_device *vdev)
 	 * Try to reset the device.  The success of this is dependent on
 	 * being able to lock the device, which is not always possible.
 	 */
-	if (vdev->reset_works) {
-		int ret = pci_try_reset_function(pdev);
-		if (ret)
-			pr_warn("%s: Failed to reset device %s (%d)\n",
-				__func__, dev_name(&pdev->dev), ret);
-		else
-			vdev->needs_reset = false;
-	}
+	if (vdev->reset_works && !pci_try_reset_function(pdev))
+		vdev->needs_reset = false;
 
 	pci_restore_state(pdev);
 out:
 	pci_disable_device(pdev);
 
 	vfio_pci_try_bus_reset(vdev);
+
+	if (!disable_idle_d3)
+		pci_set_power_state(pdev, PCI_D3hot);
 }
 
 static void vfio_pci_release(void *device_data)
@@ -885,6 +953,27 @@ static int vfio_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (ret) {
 		iommu_group_put(group);
 		kfree(vdev);
+		return ret;
+	}
+
+	if (vfio_pci_is_vga(pdev)) {
+		vga_client_register(pdev, vdev, NULL, vfio_pci_set_vga_decode);
+		vga_set_legacy_decoding(pdev,
+					vfio_pci_set_vga_decode(vdev, false));
+	}
+
+	if (!disable_idle_d3) {
+		/*
+		 * pci-core sets the device power state to an unknown value at
+		 * bootup and after being removed from a driver.  The only
+		 * transition it allows from this unknown state is to D0, which
+		 * typically happens when a driver calls pci_enable_device().
+		 * We're not ready to enable the device yet, but we do want to
+		 * be able to get to D3.  Therefore first do a D0 transition
+		 * before going to D3.
+		 */
+		pci_set_power_state(pdev, PCI_D0);
+		pci_set_power_state(pdev, PCI_D3hot);
 	}
 
 	return ret;
@@ -895,10 +984,21 @@ static void vfio_pci_remove(struct pci_dev *pdev)
 	struct vfio_pci_device *vdev;
 
 	vdev = vfio_del_group_dev(&pdev->dev);
-	if (vdev) {
-		iommu_group_put(pdev->dev.iommu_group);
-		kfree(vdev);
+	if (!vdev)
+		return;
+
+	iommu_group_put(pdev->dev.iommu_group);
+	kfree(vdev);
+
+	if (vfio_pci_is_vga(pdev)) {
+		vga_client_register(pdev, NULL, NULL, NULL);
+		vga_set_legacy_decoding(pdev,
+				VGA_RSRC_NORMAL_IO | VGA_RSRC_NORMAL_MEM |
+				VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM);
 	}
+
+	if (!disable_idle_d3)
+		pci_set_power_state(pdev, PCI_D0);
 }
 
 static pci_ers_result_t vfio_pci_aer_err_detected(struct pci_dev *pdev,
@@ -1017,10 +1117,13 @@ static void vfio_pci_try_bus_reset(struct vfio_pci_device *vdev)
 
 put_devs:
 	for (i = 0; i < devs.cur_index; i++) {
-		if (!ret) {
-			tmp = vfio_device_data(devs.devices[i]);
+		tmp = vfio_device_data(devs.devices[i]);
+		if (!ret)
 			tmp->needs_reset = false;
-		}
+
+		if (!tmp->refcnt && !disable_idle_d3)
+			pci_set_power_state(tmp->pdev, PCI_D3hot);
+
 		vfio_device_put(devs.devices[i]);
 	}
 
@@ -1030,10 +1133,50 @@ put_devs:
 static void __exit vfio_pci_cleanup(void)
 {
 	pci_unregister_driver(&vfio_pci_driver);
-	vfio_pci_virqfd_exit();
 	vfio_pci_uninit_perm_bits();
 }
 
+static void __init vfio_pci_fill_ids(void)
+{
+	char *p, *id;
+	int rc;
+
+	/* no ids passed actually */
+	if (ids[0] == '\0')
+		return;
+
+	/* add ids specified in the module parameter */
+	p = ids;
+	while ((id = strsep(&p, ","))) {
+		unsigned int vendor, device, subvendor = PCI_ANY_ID,
+			subdevice = PCI_ANY_ID, class = 0, class_mask = 0;
+		int fields;
+
+		if (!strlen(id))
+			continue;
+
+		fields = sscanf(id, "%x:%x:%x:%x:%x:%x",
+				&vendor, &device, &subvendor, &subdevice,
+				&class, &class_mask);
+
+		if (fields < 2) {
+			pr_warn("invalid id string \"%s\"\n", id);
+			continue;
+		}
+
+		rc = pci_add_dynid(&vfio_pci_driver, vendor, device,
+				   subvendor, subdevice, class, class_mask, 0);
+		if (rc)
+			pr_warn("failed to add dynamic id [%04hx:%04hx[%04hx:%04hx]] class %#08x/%08x (%d)\n",
+				vendor, device, subvendor, subdevice,
+				class, class_mask, rc);
+		else
+			pr_info("add [%04hx:%04hx[%04hx:%04hx]] class %#08x/%08x\n",
+				vendor, device, subvendor, subdevice,
+				class, class_mask);
+	}
+}
+
 static int __init vfio_pci_init(void)
 {
 	int ret;
@@ -1043,21 +1186,16 @@ static int __init vfio_pci_init(void)
 	if (ret)
 		return ret;
 
-	/* Start the virqfd cleanup handler */
-	ret = vfio_pci_virqfd_init();
-	if (ret)
-		goto out_virqfd;
-
 	/* Register and scan for devices */
 	ret = pci_register_driver(&vfio_pci_driver);
 	if (ret)
 		goto out_driver;
 
+	vfio_pci_fill_ids();
+
 	return 0;
 
 out_driver:
-	vfio_pci_virqfd_exit();
-out_virqfd:
 	vfio_pci_uninit_perm_bits();
 	return ret;
 }
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
index 2027a27546ef..1f577b4ac126 100644
--- a/drivers/vfio/pci/vfio_pci_intrs.c
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -19,230 +19,19 @@
 #include <linux/msi.h>
 #include <linux/pci.h>
 #include <linux/file.h>
-#include <linux/poll.h>
 #include <linux/vfio.h>
 #include <linux/wait.h>
-#include <linux/workqueue.h>
 #include <linux/slab.h>
 
 #include "vfio_pci_private.h"
 
 /*
- * IRQfd - generic
- */
-struct virqfd {
-	struct vfio_pci_device	*vdev;
-	struct eventfd_ctx	*eventfd;
-	int			(*handler)(struct vfio_pci_device *, void *);
-	void			(*thread)(struct vfio_pci_device *, void *);
-	void			*data;
-	struct work_struct	inject;
-	wait_queue_t		wait;
-	poll_table		pt;
-	struct work_struct	shutdown;
-	struct virqfd		**pvirqfd;
-};
-
-static struct workqueue_struct *vfio_irqfd_cleanup_wq;
-
-int __init vfio_pci_virqfd_init(void)
-{
-	vfio_irqfd_cleanup_wq =
-		create_singlethread_workqueue("vfio-irqfd-cleanup");
-	if (!vfio_irqfd_cleanup_wq)
-		return -ENOMEM;
-
-	return 0;
-}
-
-void vfio_pci_virqfd_exit(void)
-{
-	destroy_workqueue(vfio_irqfd_cleanup_wq);
-}
-
-static void virqfd_deactivate(struct virqfd *virqfd)
-{
-	queue_work(vfio_irqfd_cleanup_wq, &virqfd->shutdown);
-}
-
-static int virqfd_wakeup(wait_queue_t *wait, unsigned mode, int sync, void *key)
-{
-	struct virqfd *virqfd = container_of(wait, struct virqfd, wait);
-	unsigned long flags = (unsigned long)key;
-
-	if (flags & POLLIN) {
-		/* An event has been signaled, call function */
-		if ((!virqfd->handler ||
-		     virqfd->handler(virqfd->vdev, virqfd->data)) &&
-		    virqfd->thread)
-			schedule_work(&virqfd->inject);
-	}
-
-	if (flags & POLLHUP) {
-		unsigned long flags;
-		spin_lock_irqsave(&virqfd->vdev->irqlock, flags);
-
-		/*
-		 * The eventfd is closing, if the virqfd has not yet been
-		 * queued for release, as determined by testing whether the
-		 * vdev pointer to it is still valid, queue it now.  As
-		 * with kvm irqfds, we know we won't race against the virqfd
-		 * going away because we hold wqh->lock to get here.
-		 */
-		if (*(virqfd->pvirqfd) == virqfd) {
-			*(virqfd->pvirqfd) = NULL;
-			virqfd_deactivate(virqfd);
-		}
-
-		spin_unlock_irqrestore(&virqfd->vdev->irqlock, flags);
-	}
-
-	return 0;
-}
-
-static void virqfd_ptable_queue_proc(struct file *file,
-				     wait_queue_head_t *wqh, poll_table *pt)
-{
-	struct virqfd *virqfd = container_of(pt, struct virqfd, pt);
-	add_wait_queue(wqh, &virqfd->wait);
-}
-
-static void virqfd_shutdown(struct work_struct *work)
-{
-	struct virqfd *virqfd = container_of(work, struct virqfd, shutdown);
-	u64 cnt;
-
-	eventfd_ctx_remove_wait_queue(virqfd->eventfd, &virqfd->wait, &cnt);
-	flush_work(&virqfd->inject);
-	eventfd_ctx_put(virqfd->eventfd);
-
-	kfree(virqfd);
-}
-
-static void virqfd_inject(struct work_struct *work)
-{
-	struct virqfd *virqfd = container_of(work, struct virqfd, inject);
-	if (virqfd->thread)
-		virqfd->thread(virqfd->vdev, virqfd->data);
-}
-
-static int virqfd_enable(struct vfio_pci_device *vdev,
-			 int (*handler)(struct vfio_pci_device *, void *),
-			 void (*thread)(struct vfio_pci_device *, void *),
-			 void *data, struct virqfd **pvirqfd, int fd)
-{
-	struct fd irqfd;
-	struct eventfd_ctx *ctx;
-	struct virqfd *virqfd;
-	int ret = 0;
-	unsigned int events;
-
-	virqfd = kzalloc(sizeof(*virqfd), GFP_KERNEL);
-	if (!virqfd)
-		return -ENOMEM;
-
-	virqfd->pvirqfd = pvirqfd;
-	virqfd->vdev = vdev;
-	virqfd->handler = handler;
-	virqfd->thread = thread;
-	virqfd->data = data;
-
-	INIT_WORK(&virqfd->shutdown, virqfd_shutdown);
-	INIT_WORK(&virqfd->inject, virqfd_inject);
-
-	irqfd = fdget(fd);
-	if (!irqfd.file) {
-		ret = -EBADF;
-		goto err_fd;
-	}
-
-	ctx = eventfd_ctx_fileget(irqfd.file);
-	if (IS_ERR(ctx)) {
-		ret = PTR_ERR(ctx);
-		goto err_ctx;
-	}
-
-	virqfd->eventfd = ctx;
-
-	/*
-	 * virqfds can be released by closing the eventfd or directly
-	 * through ioctl.  These are both done through a workqueue, so
-	 * we update the pointer to the virqfd under lock to avoid
-	 * pushing multiple jobs to release the same virqfd.
-	 */
-	spin_lock_irq(&vdev->irqlock);
-
-	if (*pvirqfd) {
-		spin_unlock_irq(&vdev->irqlock);
-		ret = -EBUSY;
-		goto err_busy;
-	}
-	*pvirqfd = virqfd;
-
-	spin_unlock_irq(&vdev->irqlock);
-
-	/*
-	 * Install our own custom wake-up handling so we are notified via
-	 * a callback whenever someone signals the underlying eventfd.
-	 */
-	init_waitqueue_func_entry(&virqfd->wait, virqfd_wakeup);
-	init_poll_funcptr(&virqfd->pt, virqfd_ptable_queue_proc);
-
-	events = irqfd.file->f_op->poll(irqfd.file, &virqfd->pt);
-
-	/*
-	 * Check if there was an event already pending on the eventfd
-	 * before we registered and trigger it as if we didn't miss it.
-	 */
-	if (events & POLLIN) {
-		if ((!handler || handler(vdev, data)) && thread)
-			schedule_work(&virqfd->inject);
-	}
-
-	/*
-	 * Do not drop the file until the irqfd is fully initialized,
-	 * otherwise we might race against the POLLHUP.
-	 */
-	fdput(irqfd);
-
-	return 0;
-err_busy:
-	eventfd_ctx_put(ctx);
-err_ctx:
-	fdput(irqfd);
-err_fd:
-	kfree(virqfd);
-
-	return ret;
-}
-
-static void virqfd_disable(struct vfio_pci_device *vdev,
-			   struct virqfd **pvirqfd)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&vdev->irqlock, flags);
-
-	if (*pvirqfd) {
-		virqfd_deactivate(*pvirqfd);
-		*pvirqfd = NULL;
-	}
-
-	spin_unlock_irqrestore(&vdev->irqlock, flags);
-
-	/*
-	 * Block until we know all outstanding shutdown jobs have completed.
-	 * Even if we don't queue the job, flush the wq to be sure it's
-	 * been released.
-	 */
-	flush_workqueue(vfio_irqfd_cleanup_wq);
-}
-
-/*
  * INTx
  */
-static void vfio_send_intx_eventfd(struct vfio_pci_device *vdev, void *unused)
+static void vfio_send_intx_eventfd(void *opaque, void *unused)
 {
+	struct vfio_pci_device *vdev = opaque;
+
 	if (likely(is_intx(vdev) && !vdev->virq_disabled))
 		eventfd_signal(vdev->ctx[0].trigger, 1);
 }
@@ -285,9 +74,9 @@ void vfio_pci_intx_mask(struct vfio_pci_device *vdev)
  * a signal is necessary, which can then be handled via a work queue
  * or directly depending on the caller.
  */
-static int vfio_pci_intx_unmask_handler(struct vfio_pci_device *vdev,
-					void *unused)
+static int vfio_pci_intx_unmask_handler(void *opaque, void *unused)
 {
+	struct vfio_pci_device *vdev = opaque;
 	struct pci_dev *pdev = vdev->pdev;
 	unsigned long flags;
 	int ret = 0;
@@ -440,8 +229,8 @@ static int vfio_intx_set_signal(struct vfio_pci_device *vdev, int fd)
 static void vfio_intx_disable(struct vfio_pci_device *vdev)
 {
 	vfio_intx_set_signal(vdev, -1);
-	virqfd_disable(vdev, &vdev->ctx[0].unmask);
-	virqfd_disable(vdev, &vdev->ctx[0].mask);
+	vfio_virqfd_disable(&vdev->ctx[0].unmask);
+	vfio_virqfd_disable(&vdev->ctx[0].mask);
 	vdev->irq_type = VFIO_PCI_NUM_IRQS;
 	vdev->num_ctx = 0;
 	kfree(vdev->ctx);
@@ -605,8 +394,8 @@ static void vfio_msi_disable(struct vfio_pci_device *vdev, bool msix)
 	vfio_msi_set_block(vdev, 0, vdev->num_ctx, NULL, msix);
 
 	for (i = 0; i < vdev->num_ctx; i++) {
-		virqfd_disable(vdev, &vdev->ctx[i].unmask);
-		virqfd_disable(vdev, &vdev->ctx[i].mask);
+		vfio_virqfd_disable(&vdev->ctx[i].unmask);
+		vfio_virqfd_disable(&vdev->ctx[i].mask);
 	}
 
 	if (msix) {
@@ -639,11 +428,12 @@ static int vfio_pci_set_intx_unmask(struct vfio_pci_device *vdev,
 	} else if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
 		int32_t fd = *(int32_t *)data;
 		if (fd >= 0)
-			return virqfd_enable(vdev, vfio_pci_intx_unmask_handler,
-					     vfio_send_intx_eventfd, NULL,
-					     &vdev->ctx[0].unmask, fd);
+			return vfio_virqfd_enable((void *) vdev,
+						  vfio_pci_intx_unmask_handler,
+						  vfio_send_intx_eventfd, NULL,
+						  &vdev->ctx[0].unmask, fd);
 
-		virqfd_disable(vdev, &vdev->ctx[0].unmask);
+		vfio_virqfd_disable(&vdev->ctx[0].unmask);
 	}
 
 	return 0;
diff --git a/drivers/vfio/pci/vfio_pci_private.h b/drivers/vfio/pci/vfio_pci_private.h
index c9f9b323f152..ae0e1b4c1711 100644
--- a/drivers/vfio/pci/vfio_pci_private.h
+++ b/drivers/vfio/pci/vfio_pci_private.h
@@ -87,9 +87,6 @@ extern ssize_t vfio_pci_vga_rw(struct vfio_pci_device *vdev, char __user *buf,
 extern int vfio_pci_init_perm_bits(void);
 extern void vfio_pci_uninit_perm_bits(void);
 
-extern int vfio_pci_virqfd_init(void);
-extern void vfio_pci_virqfd_exit(void);
-
 extern int vfio_config_init(struct vfio_pci_device *vdev);
 extern void vfio_config_free(struct vfio_pci_device *vdev);
 #endif /* VFIO_PCI_PRIVATE_H */
diff --git a/drivers/vfio/platform/Kconfig b/drivers/vfio/platform/Kconfig
new file mode 100644
index 000000000000..9a4403e2a36c
--- /dev/null
+++ b/drivers/vfio/platform/Kconfig
@@ -0,0 +1,20 @@
+config VFIO_PLATFORM
+	tristate "VFIO support for platform devices"
+	depends on VFIO && EVENTFD && ARM
+	select VFIO_VIRQFD
+	help
+	  Support for platform devices with VFIO. This is required to make
+	  use of platform devices present on the system using the VFIO
+	  framework.
+
+	  If you don't know what to do here, say N.
+
+config VFIO_AMBA
+	tristate "VFIO support for AMBA devices"
+	depends on VFIO_PLATFORM && ARM_AMBA
+	help
+	  Support for ARM AMBA devices with VFIO. This is required to make
+	  use of ARM AMBA devices present on the system using the VFIO
+	  framework.
+
+	  If you don't know what to do here, say N.
diff --git a/drivers/vfio/platform/Makefile b/drivers/vfio/platform/Makefile
new file mode 100644
index 000000000000..81de144c0eaa
--- /dev/null
+++ b/drivers/vfio/platform/Makefile
@@ -0,0 +1,8 @@
+
+vfio-platform-y := vfio_platform.o vfio_platform_common.o vfio_platform_irq.o
+
+obj-$(CONFIG_VFIO_PLATFORM) += vfio-platform.o
+
+vfio-amba-y := vfio_amba.o
+
+obj-$(CONFIG_VFIO_AMBA) += vfio-amba.o
diff --git a/drivers/vfio/platform/vfio_amba.c b/drivers/vfio/platform/vfio_amba.c
new file mode 100644
index 000000000000..ff0331f72526
--- /dev/null
+++ b/drivers/vfio/platform/vfio_amba.c
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) 2013 - Virtual Open Systems
+ * Author: Antonios Motakis <a.motakis@virtualopensystems.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/vfio.h>
+#include <linux/amba/bus.h>
+
+#include "vfio_platform_private.h"
+
+#define DRIVER_VERSION  "0.10"
+#define DRIVER_AUTHOR   "Antonios Motakis <a.motakis@virtualopensystems.com>"
+#define DRIVER_DESC     "VFIO for AMBA devices - User Level meta-driver"
+
+/* probing devices from the AMBA bus */
+
+static struct resource *get_amba_resource(struct vfio_platform_device *vdev,
+					  int i)
+{
+	struct amba_device *adev = (struct amba_device *) vdev->opaque;
+
+	if (i == 0)
+		return &adev->res;
+
+	return NULL;
+}
+
+static int get_amba_irq(struct vfio_platform_device *vdev, int i)
+{
+	struct amba_device *adev = (struct amba_device *) vdev->opaque;
+	int ret = 0;
+
+	if (i < AMBA_NR_IRQS)
+		ret = adev->irq[i];
+
+	/* zero is an unset IRQ for AMBA devices */
+	return ret ? ret : -ENXIO;
+}
+
+static int vfio_amba_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	struct vfio_platform_device *vdev;
+	int ret;
+
+	vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
+	if (!vdev)
+		return -ENOMEM;
+
+	vdev->name = kasprintf(GFP_KERNEL, "vfio-amba-%08x", adev->periphid);
+	if (!vdev->name) {
+		kfree(vdev);
+		return -ENOMEM;
+	}
+
+	vdev->opaque = (void *) adev;
+	vdev->flags = VFIO_DEVICE_FLAGS_AMBA;
+	vdev->get_resource = get_amba_resource;
+	vdev->get_irq = get_amba_irq;
+
+	ret = vfio_platform_probe_common(vdev, &adev->dev);
+	if (ret) {
+		kfree(vdev->name);
+		kfree(vdev);
+	}
+
+	return ret;
+}
+
+static int vfio_amba_remove(struct amba_device *adev)
+{
+	struct vfio_platform_device *vdev;
+
+	vdev = vfio_platform_remove_common(&adev->dev);
+	if (vdev) {
+		kfree(vdev->name);
+		kfree(vdev);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static struct amba_id pl330_ids[] = {
+	{ 0, 0 },
+};
+
+MODULE_DEVICE_TABLE(amba, pl330_ids);
+
+static struct amba_driver vfio_amba_driver = {
+	.probe = vfio_amba_probe,
+	.remove = vfio_amba_remove,
+	.id_table = pl330_ids,
+	.drv = {
+		.name = "vfio-amba",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_amba_driver(vfio_amba_driver);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/vfio/platform/vfio_platform.c b/drivers/vfio/platform/vfio_platform.c
new file mode 100644
index 000000000000..cef645c83996
--- /dev/null
+++ b/drivers/vfio/platform/vfio_platform.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2013 - Virtual Open Systems
+ * Author: Antonios Motakis <a.motakis@virtualopensystems.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/vfio.h>
+#include <linux/platform_device.h>
+
+#include "vfio_platform_private.h"
+
+#define DRIVER_VERSION  "0.10"
+#define DRIVER_AUTHOR   "Antonios Motakis <a.motakis@virtualopensystems.com>"
+#define DRIVER_DESC     "VFIO for platform devices - User Level meta-driver"
+
+/* probing devices from the linux platform bus */
+
+static struct resource *get_platform_resource(struct vfio_platform_device *vdev,
+					      int num)
+{
+	struct platform_device *dev = (struct platform_device *) vdev->opaque;
+	int i;
+
+	for (i = 0; i < dev->num_resources; i++) {
+		struct resource *r = &dev->resource[i];
+
+		if (resource_type(r) & (IORESOURCE_MEM|IORESOURCE_IO)) {
+			if (!num)
+				return r;
+
+			num--;
+		}
+	}
+	return NULL;
+}
+
+static int get_platform_irq(struct vfio_platform_device *vdev, int i)
+{
+	struct platform_device *pdev = (struct platform_device *) vdev->opaque;
+
+	return platform_get_irq(pdev, i);
+}
+
+static int vfio_platform_probe(struct platform_device *pdev)
+{
+	struct vfio_platform_device *vdev;
+	int ret;
+
+	vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
+	if (!vdev)
+		return -ENOMEM;
+
+	vdev->opaque = (void *) pdev;
+	vdev->name = pdev->name;
+	vdev->flags = VFIO_DEVICE_FLAGS_PLATFORM;
+	vdev->get_resource = get_platform_resource;
+	vdev->get_irq = get_platform_irq;
+
+	ret = vfio_platform_probe_common(vdev, &pdev->dev);
+	if (ret)
+		kfree(vdev);
+
+	return ret;
+}
+
+static int vfio_platform_remove(struct platform_device *pdev)
+{
+	struct vfio_platform_device *vdev;
+
+	vdev = vfio_platform_remove_common(&pdev->dev);
+	if (vdev) {
+		kfree(vdev);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static struct platform_driver vfio_platform_driver = {
+	.probe		= vfio_platform_probe,
+	.remove		= vfio_platform_remove,
+	.driver	= {
+		.name	= "vfio-platform",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(vfio_platform_driver);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/vfio/platform/vfio_platform_common.c b/drivers/vfio/platform/vfio_platform_common.c
new file mode 100644
index 000000000000..abcff7a1aa66
--- /dev/null
+++ b/drivers/vfio/platform/vfio_platform_common.c
@@ -0,0 +1,521 @@
+/*
+ * Copyright (C) 2013 - Virtual Open Systems
+ * Author: Antonios Motakis <a.motakis@virtualopensystems.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/iommu.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/vfio.h>
+
+#include "vfio_platform_private.h"
+
+static DEFINE_MUTEX(driver_lock);
+
+static int vfio_platform_regions_init(struct vfio_platform_device *vdev)
+{
+	int cnt = 0, i;
+
+	while (vdev->get_resource(vdev, cnt))
+		cnt++;
+
+	vdev->regions = kcalloc(cnt, sizeof(struct vfio_platform_region),
+				GFP_KERNEL);
+	if (!vdev->regions)
+		return -ENOMEM;
+
+	for (i = 0; i < cnt;  i++) {
+		struct resource *res =
+			vdev->get_resource(vdev, i);
+
+		if (!res)
+			goto err;
+
+		vdev->regions[i].addr = res->start;
+		vdev->regions[i].size = resource_size(res);
+		vdev->regions[i].flags = 0;
+
+		switch (resource_type(res)) {
+		case IORESOURCE_MEM:
+			vdev->regions[i].type = VFIO_PLATFORM_REGION_TYPE_MMIO;
+			vdev->regions[i].flags |= VFIO_REGION_INFO_FLAG_READ;
+			if (!(res->flags & IORESOURCE_READONLY))
+				vdev->regions[i].flags |=
+					VFIO_REGION_INFO_FLAG_WRITE;
+
+			/*
+			 * Only regions addressed with PAGE granularity may be
+			 * MMAPed securely.
+			 */
+			if (!(vdev->regions[i].addr & ~PAGE_MASK) &&
+					!(vdev->regions[i].size & ~PAGE_MASK))
+				vdev->regions[i].flags |=
+					VFIO_REGION_INFO_FLAG_MMAP;
+
+			break;
+		case IORESOURCE_IO:
+			vdev->regions[i].type = VFIO_PLATFORM_REGION_TYPE_PIO;
+			break;
+		default:
+			goto err;
+		}
+	}
+
+	vdev->num_regions = cnt;
+
+	return 0;
+err:
+	kfree(vdev->regions);
+	return -EINVAL;
+}
+
+static void vfio_platform_regions_cleanup(struct vfio_platform_device *vdev)
+{
+	int i;
+
+	for (i = 0; i < vdev->num_regions; i++)
+		iounmap(vdev->regions[i].ioaddr);
+
+	vdev->num_regions = 0;
+	kfree(vdev->regions);
+}
+
+static void vfio_platform_release(void *device_data)
+{
+	struct vfio_platform_device *vdev = device_data;
+
+	mutex_lock(&driver_lock);
+
+	if (!(--vdev->refcnt)) {
+		vfio_platform_regions_cleanup(vdev);
+		vfio_platform_irq_cleanup(vdev);
+	}
+
+	mutex_unlock(&driver_lock);
+
+	module_put(THIS_MODULE);
+}
+
+static int vfio_platform_open(void *device_data)
+{
+	struct vfio_platform_device *vdev = device_data;
+	int ret;
+
+	if (!try_module_get(THIS_MODULE))
+		return -ENODEV;
+
+	mutex_lock(&driver_lock);
+
+	if (!vdev->refcnt) {
+		ret = vfio_platform_regions_init(vdev);
+		if (ret)
+			goto err_reg;
+
+		ret = vfio_platform_irq_init(vdev);
+		if (ret)
+			goto err_irq;
+	}
+
+	vdev->refcnt++;
+
+	mutex_unlock(&driver_lock);
+	return 0;
+
+err_irq:
+	vfio_platform_regions_cleanup(vdev);
+err_reg:
+	mutex_unlock(&driver_lock);
+	module_put(THIS_MODULE);
+	return ret;
+}
+
+static long vfio_platform_ioctl(void *device_data,
+				unsigned int cmd, unsigned long arg)
+{
+	struct vfio_platform_device *vdev = device_data;
+	unsigned long minsz;
+
+	if (cmd == VFIO_DEVICE_GET_INFO) {
+		struct vfio_device_info info;
+
+		minsz = offsetofend(struct vfio_device_info, num_irqs);
+
+		if (copy_from_user(&info, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (info.argsz < minsz)
+			return -EINVAL;
+
+		info.flags = vdev->flags;
+		info.num_regions = vdev->num_regions;
+		info.num_irqs = vdev->num_irqs;
+
+		return copy_to_user((void __user *)arg, &info, minsz);
+
+	} else if (cmd == VFIO_DEVICE_GET_REGION_INFO) {
+		struct vfio_region_info info;
+
+		minsz = offsetofend(struct vfio_region_info, offset);
+
+		if (copy_from_user(&info, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (info.argsz < minsz)
+			return -EINVAL;
+
+		if (info.index >= vdev->num_regions)
+			return -EINVAL;
+
+		/* map offset to the physical address  */
+		info.offset = VFIO_PLATFORM_INDEX_TO_OFFSET(info.index);
+		info.size = vdev->regions[info.index].size;
+		info.flags = vdev->regions[info.index].flags;
+
+		return copy_to_user((void __user *)arg, &info, minsz);
+
+	} else if (cmd == VFIO_DEVICE_GET_IRQ_INFO) {
+		struct vfio_irq_info info;
+
+		minsz = offsetofend(struct vfio_irq_info, count);
+
+		if (copy_from_user(&info, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (info.argsz < minsz)
+			return -EINVAL;
+
+		if (info.index >= vdev->num_irqs)
+			return -EINVAL;
+
+		info.flags = vdev->irqs[info.index].flags;
+		info.count = vdev->irqs[info.index].count;
+
+		return copy_to_user((void __user *)arg, &info, minsz);
+
+	} else if (cmd == VFIO_DEVICE_SET_IRQS) {
+		struct vfio_irq_set hdr;
+		u8 *data = NULL;
+		int ret = 0;
+
+		minsz = offsetofend(struct vfio_irq_set, count);
+
+		if (copy_from_user(&hdr, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (hdr.argsz < minsz)
+			return -EINVAL;
+
+		if (hdr.index >= vdev->num_irqs)
+			return -EINVAL;
+
+		if (hdr.flags & ~(VFIO_IRQ_SET_DATA_TYPE_MASK |
+				  VFIO_IRQ_SET_ACTION_TYPE_MASK))
+			return -EINVAL;
+
+		if (!(hdr.flags & VFIO_IRQ_SET_DATA_NONE)) {
+			size_t size;
+
+			if (hdr.flags & VFIO_IRQ_SET_DATA_BOOL)
+				size = sizeof(uint8_t);
+			else if (hdr.flags & VFIO_IRQ_SET_DATA_EVENTFD)
+				size = sizeof(int32_t);
+			else
+				return -EINVAL;
+
+			if (hdr.argsz - minsz < size)
+				return -EINVAL;
+
+			data = memdup_user((void __user *)(arg + minsz), size);
+			if (IS_ERR(data))
+				return PTR_ERR(data);
+		}
+
+		mutex_lock(&vdev->igate);
+
+		ret = vfio_platform_set_irqs_ioctl(vdev, hdr.flags, hdr.index,
+						   hdr.start, hdr.count, data);
+		mutex_unlock(&vdev->igate);
+		kfree(data);
+
+		return ret;
+
+	} else if (cmd == VFIO_DEVICE_RESET)
+		return -EINVAL;
+
+	return -ENOTTY;
+}
+
+static ssize_t vfio_platform_read_mmio(struct vfio_platform_region reg,
+				       char __user *buf, size_t count,
+				       loff_t off)
+{
+	unsigned int done = 0;
+
+	if (!reg.ioaddr) {
+		reg.ioaddr =
+			ioremap_nocache(reg.addr, reg.size);
+
+		if (!reg.ioaddr)
+			return -ENOMEM;
+	}
+
+	while (count) {
+		size_t filled;
+
+		if (count >= 4 && !(off % 4)) {
+			u32 val;
+
+			val = ioread32(reg.ioaddr + off);
+			if (copy_to_user(buf, &val, 4))
+				goto err;
+
+			filled = 4;
+		} else if (count >= 2 && !(off % 2)) {
+			u16 val;
+
+			val = ioread16(reg.ioaddr + off);
+			if (copy_to_user(buf, &val, 2))
+				goto err;
+
+			filled = 2;
+		} else {
+			u8 val;
+
+			val = ioread8(reg.ioaddr + off);
+			if (copy_to_user(buf, &val, 1))
+				goto err;
+
+			filled = 1;
+		}
+
+
+		count -= filled;
+		done += filled;
+		off += filled;
+		buf += filled;
+	}
+
+	return done;
+err:
+	return -EFAULT;
+}
+
+static ssize_t vfio_platform_read(void *device_data, char __user *buf,
+				  size_t count, loff_t *ppos)
+{
+	struct vfio_platform_device *vdev = device_data;
+	unsigned int index = VFIO_PLATFORM_OFFSET_TO_INDEX(*ppos);
+	loff_t off = *ppos & VFIO_PLATFORM_OFFSET_MASK;
+
+	if (index >= vdev->num_regions)
+		return -EINVAL;
+
+	if (!(vdev->regions[index].flags & VFIO_REGION_INFO_FLAG_READ))
+		return -EINVAL;
+
+	if (vdev->regions[index].type & VFIO_PLATFORM_REGION_TYPE_MMIO)
+		return vfio_platform_read_mmio(vdev->regions[index],
+							buf, count, off);
+	else if (vdev->regions[index].type & VFIO_PLATFORM_REGION_TYPE_PIO)
+		return -EINVAL; /* not implemented */
+
+	return -EINVAL;
+}
+
+static ssize_t vfio_platform_write_mmio(struct vfio_platform_region reg,
+					const char __user *buf, size_t count,
+					loff_t off)
+{
+	unsigned int done = 0;
+
+	if (!reg.ioaddr) {
+		reg.ioaddr =
+			ioremap_nocache(reg.addr, reg.size);
+
+		if (!reg.ioaddr)
+			return -ENOMEM;
+	}
+
+	while (count) {
+		size_t filled;
+
+		if (count >= 4 && !(off % 4)) {
+			u32 val;
+
+			if (copy_from_user(&val, buf, 4))
+				goto err;
+			iowrite32(val, reg.ioaddr + off);
+
+			filled = 4;
+		} else if (count >= 2 && !(off % 2)) {
+			u16 val;
+
+			if (copy_from_user(&val, buf, 2))
+				goto err;
+			iowrite16(val, reg.ioaddr + off);
+
+			filled = 2;
+		} else {
+			u8 val;
+
+			if (copy_from_user(&val, buf, 1))
+				goto err;
+			iowrite8(val, reg.ioaddr + off);
+
+			filled = 1;
+		}
+
+		count -= filled;
+		done += filled;
+		off += filled;
+		buf += filled;
+	}
+
+	return done;
+err:
+	return -EFAULT;
+}
+
+static ssize_t vfio_platform_write(void *device_data, const char __user *buf,
+				   size_t count, loff_t *ppos)
+{
+	struct vfio_platform_device *vdev = device_data;
+	unsigned int index = VFIO_PLATFORM_OFFSET_TO_INDEX(*ppos);
+	loff_t off = *ppos & VFIO_PLATFORM_OFFSET_MASK;
+
+	if (index >= vdev->num_regions)
+		return -EINVAL;
+
+	if (!(vdev->regions[index].flags & VFIO_REGION_INFO_FLAG_WRITE))
+		return -EINVAL;
+
+	if (vdev->regions[index].type & VFIO_PLATFORM_REGION_TYPE_MMIO)
+		return vfio_platform_write_mmio(vdev->regions[index],
+							buf, count, off);
+	else if (vdev->regions[index].type & VFIO_PLATFORM_REGION_TYPE_PIO)
+		return -EINVAL; /* not implemented */
+
+	return -EINVAL;
+}
+
+static int vfio_platform_mmap_mmio(struct vfio_platform_region region,
+				   struct vm_area_struct *vma)
+{
+	u64 req_len, pgoff, req_start;
+
+	req_len = vma->vm_end - vma->vm_start;
+	pgoff = vma->vm_pgoff &
+		((1U << (VFIO_PLATFORM_OFFSET_SHIFT - PAGE_SHIFT)) - 1);
+	req_start = pgoff << PAGE_SHIFT;
+
+	if (region.size < PAGE_SIZE || req_start + req_len > region.size)
+		return -EINVAL;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_pgoff = (region.addr >> PAGE_SHIFT) + pgoff;
+
+	return remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+			       req_len, vma->vm_page_prot);
+}
+
+static int vfio_platform_mmap(void *device_data, struct vm_area_struct *vma)
+{
+	struct vfio_platform_device *vdev = device_data;
+	unsigned int index;
+
+	index = vma->vm_pgoff >> (VFIO_PLATFORM_OFFSET_SHIFT - PAGE_SHIFT);
+
+	if (vma->vm_end < vma->vm_start)
+		return -EINVAL;
+	if (!(vma->vm_flags & VM_SHARED))
+		return -EINVAL;
+	if (index >= vdev->num_regions)
+		return -EINVAL;
+	if (vma->vm_start & ~PAGE_MASK)
+		return -EINVAL;
+	if (vma->vm_end & ~PAGE_MASK)
+		return -EINVAL;
+
+	if (!(vdev->regions[index].flags & VFIO_REGION_INFO_FLAG_MMAP))
+		return -EINVAL;
+
+	if (!(vdev->regions[index].flags & VFIO_REGION_INFO_FLAG_READ)
+			&& (vma->vm_flags & VM_READ))
+		return -EINVAL;
+
+	if (!(vdev->regions[index].flags & VFIO_REGION_INFO_FLAG_WRITE)
+			&& (vma->vm_flags & VM_WRITE))
+		return -EINVAL;
+
+	vma->vm_private_data = vdev;
+
+	if (vdev->regions[index].type & VFIO_PLATFORM_REGION_TYPE_MMIO)
+		return vfio_platform_mmap_mmio(vdev->regions[index], vma);
+
+	else if (vdev->regions[index].type & VFIO_PLATFORM_REGION_TYPE_PIO)
+		return -EINVAL; /* not implemented */
+
+	return -EINVAL;
+}
+
+static const struct vfio_device_ops vfio_platform_ops = {
+	.name		= "vfio-platform",
+	.open		= vfio_platform_open,
+	.release	= vfio_platform_release,
+	.ioctl		= vfio_platform_ioctl,
+	.read		= vfio_platform_read,
+	.write		= vfio_platform_write,
+	.mmap		= vfio_platform_mmap,
+};
+
+int vfio_platform_probe_common(struct vfio_platform_device *vdev,
+			       struct device *dev)
+{
+	struct iommu_group *group;
+	int ret;
+
+	if (!vdev)
+		return -EINVAL;
+
+	group = iommu_group_get(dev);
+	if (!group) {
+		pr_err("VFIO: No IOMMU group for device %s\n", vdev->name);
+		return -EINVAL;
+	}
+
+	ret = vfio_add_group_dev(dev, &vfio_platform_ops, vdev);
+	if (ret) {
+		iommu_group_put(group);
+		return ret;
+	}
+
+	mutex_init(&vdev->igate);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vfio_platform_probe_common);
+
+struct vfio_platform_device *vfio_platform_remove_common(struct device *dev)
+{
+	struct vfio_platform_device *vdev;
+
+	vdev = vfio_del_group_dev(dev);
+	if (vdev)
+		iommu_group_put(dev->iommu_group);
+
+	return vdev;
+}
+EXPORT_SYMBOL_GPL(vfio_platform_remove_common);
diff --git a/drivers/vfio/platform/vfio_platform_irq.c b/drivers/vfio/platform/vfio_platform_irq.c
new file mode 100644
index 000000000000..88bba57b30a8
--- /dev/null
+++ b/drivers/vfio/platform/vfio_platform_irq.c
@@ -0,0 +1,336 @@
+/*
+ * VFIO platform devices interrupt handling
+ *
+ * Copyright (C) 2013 - Virtual Open Systems
+ * Author: Antonios Motakis <a.motakis@virtualopensystems.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/eventfd.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/vfio.h>
+#include <linux/irq.h>
+
+#include "vfio_platform_private.h"
+
+static void vfio_platform_mask(struct vfio_platform_irq *irq_ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&irq_ctx->lock, flags);
+
+	if (!irq_ctx->masked) {
+		disable_irq_nosync(irq_ctx->hwirq);
+		irq_ctx->masked = true;
+	}
+
+	spin_unlock_irqrestore(&irq_ctx->lock, flags);
+}
+
+static int vfio_platform_mask_handler(void *opaque, void *unused)
+{
+	struct vfio_platform_irq *irq_ctx = opaque;
+
+	vfio_platform_mask(irq_ctx);
+
+	return 0;
+}
+
+static int vfio_platform_set_irq_mask(struct vfio_platform_device *vdev,
+				      unsigned index, unsigned start,
+				      unsigned count, uint32_t flags,
+				      void *data)
+{
+	if (start != 0 || count != 1)
+		return -EINVAL;
+
+	if (!(vdev->irqs[index].flags & VFIO_IRQ_INFO_MASKABLE))
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
+		int32_t fd = *(int32_t *)data;
+
+		if (fd >= 0)
+			return vfio_virqfd_enable((void *) &vdev->irqs[index],
+						  vfio_platform_mask_handler,
+						  NULL, NULL,
+						  &vdev->irqs[index].mask, fd);
+
+		vfio_virqfd_disable(&vdev->irqs[index].mask);
+		return 0;
+	}
+
+	if (flags & VFIO_IRQ_SET_DATA_NONE) {
+		vfio_platform_mask(&vdev->irqs[index]);
+
+	} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+		uint8_t mask = *(uint8_t *)data;
+
+		if (mask)
+			vfio_platform_mask(&vdev->irqs[index]);
+	}
+
+	return 0;
+}
+
+static void vfio_platform_unmask(struct vfio_platform_irq *irq_ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&irq_ctx->lock, flags);
+
+	if (irq_ctx->masked) {
+		enable_irq(irq_ctx->hwirq);
+		irq_ctx->masked = false;
+	}
+
+	spin_unlock_irqrestore(&irq_ctx->lock, flags);
+}
+
+static int vfio_platform_unmask_handler(void *opaque, void *unused)
+{
+	struct vfio_platform_irq *irq_ctx = opaque;
+
+	vfio_platform_unmask(irq_ctx);
+
+	return 0;
+}
+
+static int vfio_platform_set_irq_unmask(struct vfio_platform_device *vdev,
+					unsigned index, unsigned start,
+					unsigned count, uint32_t flags,
+					void *data)
+{
+	if (start != 0 || count != 1)
+		return -EINVAL;
+
+	if (!(vdev->irqs[index].flags & VFIO_IRQ_INFO_MASKABLE))
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
+		int32_t fd = *(int32_t *)data;
+
+		if (fd >= 0)
+			return vfio_virqfd_enable((void *) &vdev->irqs[index],
+						  vfio_platform_unmask_handler,
+						  NULL, NULL,
+						  &vdev->irqs[index].unmask,
+						  fd);
+
+		vfio_virqfd_disable(&vdev->irqs[index].unmask);
+		return 0;
+	}
+
+	if (flags & VFIO_IRQ_SET_DATA_NONE) {
+		vfio_platform_unmask(&vdev->irqs[index]);
+
+	} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+		uint8_t unmask = *(uint8_t *)data;
+
+		if (unmask)
+			vfio_platform_unmask(&vdev->irqs[index]);
+	}
+
+	return 0;
+}
+
+static irqreturn_t vfio_automasked_irq_handler(int irq, void *dev_id)
+{
+	struct vfio_platform_irq *irq_ctx = dev_id;
+	unsigned long flags;
+	int ret = IRQ_NONE;
+
+	spin_lock_irqsave(&irq_ctx->lock, flags);
+
+	if (!irq_ctx->masked) {
+		ret = IRQ_HANDLED;
+
+		/* automask maskable interrupts */
+		disable_irq_nosync(irq_ctx->hwirq);
+		irq_ctx->masked = true;
+	}
+
+	spin_unlock_irqrestore(&irq_ctx->lock, flags);
+
+	if (ret == IRQ_HANDLED)
+		eventfd_signal(irq_ctx->trigger, 1);
+
+	return ret;
+}
+
+static irqreturn_t vfio_irq_handler(int irq, void *dev_id)
+{
+	struct vfio_platform_irq *irq_ctx = dev_id;
+
+	eventfd_signal(irq_ctx->trigger, 1);
+
+	return IRQ_HANDLED;
+}
+
+static int vfio_set_trigger(struct vfio_platform_device *vdev, int index,
+			    int fd, irq_handler_t handler)
+{
+	struct vfio_platform_irq *irq = &vdev->irqs[index];
+	struct eventfd_ctx *trigger;
+	int ret;
+
+	if (irq->trigger) {
+		free_irq(irq->hwirq, irq);
+		kfree(irq->name);
+		eventfd_ctx_put(irq->trigger);
+		irq->trigger = NULL;
+	}
+
+	if (fd < 0) /* Disable only */
+		return 0;
+
+	irq->name = kasprintf(GFP_KERNEL, "vfio-irq[%d](%s)",
+						irq->hwirq, vdev->name);
+	if (!irq->name)
+		return -ENOMEM;
+
+	trigger = eventfd_ctx_fdget(fd);
+	if (IS_ERR(trigger)) {
+		kfree(irq->name);
+		return PTR_ERR(trigger);
+	}
+
+	irq->trigger = trigger;
+
+	irq_set_status_flags(irq->hwirq, IRQ_NOAUTOEN);
+	ret = request_irq(irq->hwirq, handler, 0, irq->name, irq);
+	if (ret) {
+		kfree(irq->name);
+		eventfd_ctx_put(trigger);
+		irq->trigger = NULL;
+		return ret;
+	}
+
+	if (!irq->masked)
+		enable_irq(irq->hwirq);
+
+	return 0;
+}
+
+static int vfio_platform_set_irq_trigger(struct vfio_platform_device *vdev,
+					 unsigned index, unsigned start,
+					 unsigned count, uint32_t flags,
+					 void *data)
+{
+	struct vfio_platform_irq *irq = &vdev->irqs[index];
+	irq_handler_t handler;
+
+	if (vdev->irqs[index].flags & VFIO_IRQ_INFO_AUTOMASKED)
+		handler = vfio_automasked_irq_handler;
+	else
+		handler = vfio_irq_handler;
+
+	if (!count && (flags & VFIO_IRQ_SET_DATA_NONE))
+		return vfio_set_trigger(vdev, index, -1, handler);
+
+	if (start != 0 || count != 1)
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
+		int32_t fd = *(int32_t *)data;
+
+		return vfio_set_trigger(vdev, index, fd, handler);
+	}
+
+	if (flags & VFIO_IRQ_SET_DATA_NONE) {
+		handler(irq->hwirq, irq);
+
+	} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+		uint8_t trigger = *(uint8_t *)data;
+
+		if (trigger)
+			handler(irq->hwirq, irq);
+	}
+
+	return 0;
+}
+
+int vfio_platform_set_irqs_ioctl(struct vfio_platform_device *vdev,
+				 uint32_t flags, unsigned index, unsigned start,
+				 unsigned count, void *data)
+{
+	int (*func)(struct vfio_platform_device *vdev, unsigned index,
+		    unsigned start, unsigned count, uint32_t flags,
+		    void *data) = NULL;
+
+	switch (flags & VFIO_IRQ_SET_ACTION_TYPE_MASK) {
+	case VFIO_IRQ_SET_ACTION_MASK:
+		func = vfio_platform_set_irq_mask;
+		break;
+	case VFIO_IRQ_SET_ACTION_UNMASK:
+		func = vfio_platform_set_irq_unmask;
+		break;
+	case VFIO_IRQ_SET_ACTION_TRIGGER:
+		func = vfio_platform_set_irq_trigger;
+		break;
+	}
+
+	if (!func)
+		return -ENOTTY;
+
+	return func(vdev, index, start, count, flags, data);
+}
+
+int vfio_platform_irq_init(struct vfio_platform_device *vdev)
+{
+	int cnt = 0, i;
+
+	while (vdev->get_irq(vdev, cnt) >= 0)
+		cnt++;
+
+	vdev->irqs = kcalloc(cnt, sizeof(struct vfio_platform_irq), GFP_KERNEL);
+	if (!vdev->irqs)
+		return -ENOMEM;
+
+	for (i = 0; i < cnt; i++) {
+		int hwirq = vdev->get_irq(vdev, i);
+
+		if (hwirq < 0)
+			goto err;
+
+		spin_lock_init(&vdev->irqs[i].lock);
+
+		vdev->irqs[i].flags = VFIO_IRQ_INFO_EVENTFD;
+
+		if (irq_get_trigger_type(hwirq) & IRQ_TYPE_LEVEL_MASK)
+			vdev->irqs[i].flags |= VFIO_IRQ_INFO_MASKABLE
+						| VFIO_IRQ_INFO_AUTOMASKED;
+
+		vdev->irqs[i].count = 1;
+		vdev->irqs[i].hwirq = hwirq;
+		vdev->irqs[i].masked = false;
+	}
+
+	vdev->num_irqs = cnt;
+
+	return 0;
+err:
+	kfree(vdev->irqs);
+	return -EINVAL;
+}
+
+void vfio_platform_irq_cleanup(struct vfio_platform_device *vdev)
+{
+	int i;
+
+	for (i = 0; i < vdev->num_irqs; i++)
+		vfio_set_trigger(vdev, i, -1, NULL);
+
+	vdev->num_irqs = 0;
+	kfree(vdev->irqs);
+}
diff --git a/drivers/vfio/platform/vfio_platform_private.h b/drivers/vfio/platform/vfio_platform_private.h
new file mode 100644
index 000000000000..5d31e0473406
--- /dev/null
+++ b/drivers/vfio/platform/vfio_platform_private.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2013 - Virtual Open Systems
+ * Author: Antonios Motakis <a.motakis@virtualopensystems.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef VFIO_PLATFORM_PRIVATE_H
+#define VFIO_PLATFORM_PRIVATE_H
+
+#include <linux/types.h>
+#include <linux/interrupt.h>
+
+#define VFIO_PLATFORM_OFFSET_SHIFT   40
+#define VFIO_PLATFORM_OFFSET_MASK (((u64)(1) << VFIO_PLATFORM_OFFSET_SHIFT) - 1)
+
+#define VFIO_PLATFORM_OFFSET_TO_INDEX(off)	\
+	(off >> VFIO_PLATFORM_OFFSET_SHIFT)
+
+#define VFIO_PLATFORM_INDEX_TO_OFFSET(index)	\
+	((u64)(index) << VFIO_PLATFORM_OFFSET_SHIFT)
+
+struct vfio_platform_irq {
+	u32			flags;
+	u32			count;
+	int			hwirq;
+	char			*name;
+	struct eventfd_ctx	*trigger;
+	bool			masked;
+	spinlock_t		lock;
+	struct virqfd		*unmask;
+	struct virqfd		*mask;
+};
+
+struct vfio_platform_region {
+	u64			addr;
+	resource_size_t		size;
+	u32			flags;
+	u32			type;
+#define VFIO_PLATFORM_REGION_TYPE_MMIO	1
+#define VFIO_PLATFORM_REGION_TYPE_PIO	2
+	void __iomem		*ioaddr;
+};
+
+struct vfio_platform_device {
+	struct vfio_platform_region	*regions;
+	u32				num_regions;
+	struct vfio_platform_irq	*irqs;
+	u32				num_irqs;
+	int				refcnt;
+	struct mutex			igate;
+
+	/*
+	 * These fields should be filled by the bus specific binder
+	 */
+	void		*opaque;
+	const char	*name;
+	uint32_t	flags;
+	/* callbacks to discover device resources */
+	struct resource*
+		(*get_resource)(struct vfio_platform_device *vdev, int i);
+	int	(*get_irq)(struct vfio_platform_device *vdev, int i);
+};
+
+extern int vfio_platform_probe_common(struct vfio_platform_device *vdev,
+				      struct device *dev);
+extern struct vfio_platform_device *vfio_platform_remove_common
+				     (struct device *dev);
+
+extern int vfio_platform_irq_init(struct vfio_platform_device *vdev);
+extern void vfio_platform_irq_cleanup(struct vfio_platform_device *vdev);
+
+extern int vfio_platform_set_irqs_ioctl(struct vfio_platform_device *vdev,
+					uint32_t flags, unsigned index,
+					unsigned start, unsigned count,
+					void *data);
+
+#endif /* VFIO_PLATFORM_PRIVATE_H */
diff --git a/drivers/vfio/vfio.c b/drivers/vfio/vfio.c
index 4cde85501444..0d336625ac71 100644
--- a/drivers/vfio/vfio.c
+++ b/drivers/vfio/vfio.c
@@ -234,22 +234,21 @@ static struct vfio_group *vfio_create_group(struct iommu_group *iommu_group)
 
 	mutex_lock(&vfio.group_lock);
 
-	minor = vfio_alloc_group_minor(group);
-	if (minor < 0) {
-		vfio_group_unlock_and_free(group);
-		return ERR_PTR(minor);
-	}
-
 	/* Did we race creating this group? */
 	list_for_each_entry(tmp, &vfio.group_list, vfio_next) {
 		if (tmp->iommu_group == iommu_group) {
 			vfio_group_get(tmp);
-			vfio_free_group_minor(minor);
 			vfio_group_unlock_and_free(group);
 			return tmp;
 		}
 	}
 
+	minor = vfio_alloc_group_minor(group);
+	if (minor < 0) {
+		vfio_group_unlock_and_free(group);
+		return ERR_PTR(minor);
+	}
+
 	dev = device_create(vfio.class, NULL,
 			    MKDEV(MAJOR(vfio.group_devt), minor),
 			    group, "%d", iommu_group_id(iommu_group));
diff --git a/drivers/vfio/virqfd.c b/drivers/vfio/virqfd.c
new file mode 100644
index 000000000000..27c89cd5d70b
--- /dev/null
+++ b/drivers/vfio/virqfd.c
@@ -0,0 +1,226 @@
+/*
+ * VFIO generic eventfd code for IRQFD support.
+ * Derived from drivers/vfio/pci/vfio_pci_intrs.c
+ *
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.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/vfio.h>
+#include <linux/eventfd.h>
+#include <linux/file.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#define DRIVER_VERSION  "0.1"
+#define DRIVER_AUTHOR   "Alex Williamson <alex.williamson@redhat.com>"
+#define DRIVER_DESC     "IRQFD support for VFIO bus drivers"
+
+static struct workqueue_struct *vfio_irqfd_cleanup_wq;
+static DEFINE_SPINLOCK(virqfd_lock);
+
+static int __init vfio_virqfd_init(void)
+{
+	vfio_irqfd_cleanup_wq =
+		create_singlethread_workqueue("vfio-irqfd-cleanup");
+	if (!vfio_irqfd_cleanup_wq)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void __exit vfio_virqfd_exit(void)
+{
+	destroy_workqueue(vfio_irqfd_cleanup_wq);
+}
+
+static void virqfd_deactivate(struct virqfd *virqfd)
+{
+	queue_work(vfio_irqfd_cleanup_wq, &virqfd->shutdown);
+}
+
+static int virqfd_wakeup(wait_queue_t *wait, unsigned mode, int sync, void *key)
+{
+	struct virqfd *virqfd = container_of(wait, struct virqfd, wait);
+	unsigned long flags = (unsigned long)key;
+
+	if (flags & POLLIN) {
+		/* An event has been signaled, call function */
+		if ((!virqfd->handler ||
+		     virqfd->handler(virqfd->opaque, virqfd->data)) &&
+		    virqfd->thread)
+			schedule_work(&virqfd->inject);
+	}
+
+	if (flags & POLLHUP) {
+		unsigned long flags;
+		spin_lock_irqsave(&virqfd_lock, flags);
+
+		/*
+		 * The eventfd is closing, if the virqfd has not yet been
+		 * queued for release, as determined by testing whether the
+		 * virqfd pointer to it is still valid, queue it now.  As
+		 * with kvm irqfds, we know we won't race against the virqfd
+		 * going away because we hold the lock to get here.
+		 */
+		if (*(virqfd->pvirqfd) == virqfd) {
+			*(virqfd->pvirqfd) = NULL;
+			virqfd_deactivate(virqfd);
+		}
+
+		spin_unlock_irqrestore(&virqfd_lock, flags);
+	}
+
+	return 0;
+}
+
+static void virqfd_ptable_queue_proc(struct file *file,
+				     wait_queue_head_t *wqh, poll_table *pt)
+{
+	struct virqfd *virqfd = container_of(pt, struct virqfd, pt);
+	add_wait_queue(wqh, &virqfd->wait);
+}
+
+static void virqfd_shutdown(struct work_struct *work)
+{
+	struct virqfd *virqfd = container_of(work, struct virqfd, shutdown);
+	u64 cnt;
+
+	eventfd_ctx_remove_wait_queue(virqfd->eventfd, &virqfd->wait, &cnt);
+	flush_work(&virqfd->inject);
+	eventfd_ctx_put(virqfd->eventfd);
+
+	kfree(virqfd);
+}
+
+static void virqfd_inject(struct work_struct *work)
+{
+	struct virqfd *virqfd = container_of(work, struct virqfd, inject);
+	if (virqfd->thread)
+		virqfd->thread(virqfd->opaque, virqfd->data);
+}
+
+int vfio_virqfd_enable(void *opaque,
+		       int (*handler)(void *, void *),
+		       void (*thread)(void *, void *),
+		       void *data, struct virqfd **pvirqfd, int fd)
+{
+	struct fd irqfd;
+	struct eventfd_ctx *ctx;
+	struct virqfd *virqfd;
+	int ret = 0;
+	unsigned int events;
+
+	virqfd = kzalloc(sizeof(*virqfd), GFP_KERNEL);
+	if (!virqfd)
+		return -ENOMEM;
+
+	virqfd->pvirqfd = pvirqfd;
+	virqfd->opaque = opaque;
+	virqfd->handler = handler;
+	virqfd->thread = thread;
+	virqfd->data = data;
+
+	INIT_WORK(&virqfd->shutdown, virqfd_shutdown);
+	INIT_WORK(&virqfd->inject, virqfd_inject);
+
+	irqfd = fdget(fd);
+	if (!irqfd.file) {
+		ret = -EBADF;
+		goto err_fd;
+	}
+
+	ctx = eventfd_ctx_fileget(irqfd.file);
+	if (IS_ERR(ctx)) {
+		ret = PTR_ERR(ctx);
+		goto err_ctx;
+	}
+
+	virqfd->eventfd = ctx;
+
+	/*
+	 * virqfds can be released by closing the eventfd or directly
+	 * through ioctl.  These are both done through a workqueue, so
+	 * we update the pointer to the virqfd under lock to avoid
+	 * pushing multiple jobs to release the same virqfd.
+	 */
+	spin_lock_irq(&virqfd_lock);
+
+	if (*pvirqfd) {
+		spin_unlock_irq(&virqfd_lock);
+		ret = -EBUSY;
+		goto err_busy;
+	}
+	*pvirqfd = virqfd;
+
+	spin_unlock_irq(&virqfd_lock);
+
+	/*
+	 * Install our own custom wake-up handling so we are notified via
+	 * a callback whenever someone signals the underlying eventfd.
+	 */
+	init_waitqueue_func_entry(&virqfd->wait, virqfd_wakeup);
+	init_poll_funcptr(&virqfd->pt, virqfd_ptable_queue_proc);
+
+	events = irqfd.file->f_op->poll(irqfd.file, &virqfd->pt);
+
+	/*
+	 * Check if there was an event already pending on the eventfd
+	 * before we registered and trigger it as if we didn't miss it.
+	 */
+	if (events & POLLIN) {
+		if ((!handler || handler(opaque, data)) && thread)
+			schedule_work(&virqfd->inject);
+	}
+
+	/*
+	 * Do not drop the file until the irqfd is fully initialized,
+	 * otherwise we might race against the POLLHUP.
+	 */
+	fdput(irqfd);
+
+	return 0;
+err_busy:
+	eventfd_ctx_put(ctx);
+err_ctx:
+	fdput(irqfd);
+err_fd:
+	kfree(virqfd);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vfio_virqfd_enable);
+
+void vfio_virqfd_disable(struct virqfd **pvirqfd)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&virqfd_lock, flags);
+
+	if (*pvirqfd) {
+		virqfd_deactivate(*pvirqfd);
+		*pvirqfd = NULL;
+	}
+
+	spin_unlock_irqrestore(&virqfd_lock, flags);
+
+	/*
+	 * Block until we know all outstanding shutdown jobs have completed.
+	 * Even if we don't queue the job, flush the wq to be sure it's
+	 * been released.
+	 */
+	flush_workqueue(vfio_irqfd_cleanup_wq);
+}
+EXPORT_SYMBOL_GPL(vfio_virqfd_disable);
+
+module_init(vfio_virqfd_init);
+module_exit(vfio_virqfd_exit);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index 2bbfc25e582c..7d137a43cc86 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -357,13 +357,13 @@ static void handle_tx(struct vhost_net *net)
 		iov_iter_init(&msg.msg_iter, WRITE, vq->iov, out, len);
 		iov_iter_advance(&msg.msg_iter, hdr_size);
 		/* Sanity check */
-		if (!iov_iter_count(&msg.msg_iter)) {
+		if (!msg_data_left(&msg)) {
 			vq_err(vq, "Unexpected header len for TX: "
 			       "%zd expected %zd\n",
 			       len, hdr_size);
 			break;
 		}
-		len = iov_iter_count(&msg.msg_iter);
+		len = msg_data_left(&msg);
 
 		zcopy_used = zcopy && len >= VHOST_GOODCOPY_LEN
 				   && (nvq->upend_idx + 1) % UIO_MAXIOV !=
@@ -390,7 +390,7 @@ static void handle_tx(struct vhost_net *net)
 			ubufs = NULL;
 		}
 		/* TODO: Check specific error and bomb out unless ENOBUFS? */
-		err = sock->ops->sendmsg(NULL, sock, &msg, len);
+		err = sock->ops->sendmsg(sock, &msg, len);
 		if (unlikely(err < 0)) {
 			if (zcopy_used) {
 				vhost_net_ubuf_put(ubufs);
@@ -566,7 +566,7 @@ static void handle_rx(struct vhost_net *net)
 		/* On overrun, truncate and discard */
 		if (unlikely(headcount > UIO_MAXIOV)) {
 			iov_iter_init(&msg.msg_iter, READ, vq->iov, 1, 1);
-			err = sock->ops->recvmsg(NULL, sock, &msg,
+			err = sock->ops->recvmsg(sock, &msg,
 						 1, MSG_DONTWAIT | MSG_TRUNC);
 			pr_debug("Discarded rx packet: len %zd\n", sock_len);
 			continue;
@@ -592,7 +592,7 @@ static void handle_rx(struct vhost_net *net)
 			 */
 			iov_iter_advance(&msg.msg_iter, vhost_hlen);
 		}
-		err = sock->ops->recvmsg(NULL, sock, &msg,
+		err = sock->ops->recvmsg(sock, &msg,
 					 sock_len, MSG_DONTWAIT | MSG_TRUNC);
 		/* Userspace might have consumed the packet meanwhile:
 		 * it's not supposed to do this usually, but might be hard
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/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index 9a23698b6fe8..2da5862876d1 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -168,10 +168,7 @@ static int pm860x_backlight_dt_init(struct platform_device *pdev,
 	struct device_node *nproot, *np;
 	int iset = 0;
 
-	nproot = of_node_get(pdev->dev.parent->of_node);
-	if (!nproot)
-		return -ENODEV;
-	nproot = of_find_node_by_name(nproot, "backlights");
+	nproot = of_get_child_by_name(pdev->dev.parent->of_node, "backlights");
 	if (!nproot) {
 		dev_err(&pdev->dev, "failed to find backlights node\n");
 		return -ENODEV;
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index efb09046a8cf..2d9923a60076 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -408,6 +408,16 @@ config BACKLIGHT_PANDORA
 	  If you have a Pandora console, say Y to enable the
 	  backlight driver.
 
+config BACKLIGHT_SKY81452
+	tristate "Backlight driver for SKY81452"
+	depends on BACKLIGHT_CLASS_DEVICE && MFD_SKY81452
+	help
+	  If you have a Skyworks SKY81452, say Y to enable the
+	  backlight driver.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called sky81452-backlight
+
 config BACKLIGHT_TPS65217
 	tristate "TPS65217 Backlight"
 	depends on BACKLIGHT_CLASS_DEVICE && MFD_TPS65217
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index fcd50b732165..d67073f9d421 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -50,6 +50,7 @@ obj-$(CONFIG_BACKLIGHT_PANDORA)		+= pandora_bl.o
 obj-$(CONFIG_BACKLIGHT_PCF50633)	+= pcf50633-backlight.o
 obj-$(CONFIG_BACKLIGHT_PWM)		+= pwm_bl.o
 obj-$(CONFIG_BACKLIGHT_SAHARA)		+= kb3886_bl.o
+obj-$(CONFIG_BACKLIGHT_SKY81452)	+= sky81452-backlight.o
 obj-$(CONFIG_BACKLIGHT_TOSA)		+= tosa_bl.o
 obj-$(CONFIG_BACKLIGHT_TPS65217)	+= tps65217_bl.o
 obj-$(CONFIG_BACKLIGHT_WM831X)		+= wm831x_bl.o
diff --git a/drivers/video/backlight/da9052_bl.c b/drivers/video/backlight/da9052_bl.c
index d4bd74bd5070..b1943e7735a1 100644
--- a/drivers/video/backlight/da9052_bl.c
+++ b/drivers/video/backlight/da9052_bl.c
@@ -165,6 +165,7 @@ static struct platform_device_id da9052_wled_ids[] = {
 		.name		= "da9052-wled3",
 		.driver_data	= DA9052_TYPE_WLED3,
 	},
+	{ },
 };
 
 static struct platform_driver da9052_wled_driver = {
diff --git a/drivers/video/backlight/sky81452-backlight.c b/drivers/video/backlight/sky81452-backlight.c
new file mode 100644
index 000000000000..052fa1bac03d
--- /dev/null
+++ b/drivers/video/backlight/sky81452-backlight.c
@@ -0,0 +1,353 @@
+/*
+ * sky81452-backlight.c	SKY81452 backlight driver
+ *
+ * Copyright 2014 Skyworks Solutions Inc.
+ * Author : Gyungoh Yoo <jack.yoo@skyworksinc.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/backlight.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/platform_data/sky81452-backlight.h>
+#include <linux/slab.h>
+
+/* registers */
+#define SKY81452_REG0	0x00
+#define SKY81452_REG1	0x01
+#define SKY81452_REG2	0x02
+#define SKY81452_REG4	0x04
+#define SKY81452_REG5	0x05
+
+/* bit mask */
+#define SKY81452_CS	0xFF
+#define SKY81452_EN	0x3F
+#define SKY81452_IGPW	0x20
+#define SKY81452_PWMMD	0x10
+#define SKY81452_PHASE	0x08
+#define SKY81452_ILIM	0x04
+#define SKY81452_VSHRT	0x03
+#define SKY81452_OCP	0x80
+#define SKY81452_OTMP	0x40
+#define SKY81452_SHRT	0x3F
+#define SKY81452_OPN	0x3F
+
+#define SKY81452_DEFAULT_NAME "lcd-backlight"
+#define SKY81452_MAX_BRIGHTNESS	(SKY81452_CS + 1)
+
+#define CTZ(b) __builtin_ctz(b)
+
+static int sky81452_bl_update_status(struct backlight_device *bd)
+{
+	const struct sky81452_bl_platform_data *pdata =
+			dev_get_platdata(bd->dev.parent);
+	const unsigned int brightness = (unsigned int)bd->props.brightness;
+	struct regmap *regmap = bl_get_data(bd);
+	int ret;
+
+	if (brightness > 0) {
+		ret = regmap_write(regmap, SKY81452_REG0, brightness - 1);
+		if (IS_ERR_VALUE(ret))
+			return ret;
+
+		return regmap_update_bits(regmap, SKY81452_REG1, SKY81452_EN,
+					pdata->enable << CTZ(SKY81452_EN));
+	}
+
+	return regmap_update_bits(regmap, SKY81452_REG1, SKY81452_EN, 0);
+}
+
+static const struct backlight_ops sky81452_bl_ops = {
+	.update_status = sky81452_bl_update_status,
+};
+
+static ssize_t sky81452_bl_store_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct regmap *regmap = bl_get_data(to_backlight_device(dev));
+	unsigned long value;
+	int ret;
+
+	ret = kstrtoul(buf, 16, &value);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	ret = regmap_update_bits(regmap, SKY81452_REG1, SKY81452_EN,
+					value << CTZ(SKY81452_EN));
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	return count;
+}
+
+static ssize_t sky81452_bl_show_open_short(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct regmap *regmap = bl_get_data(to_backlight_device(dev));
+	unsigned int reg, value = 0;
+	char tmp[3];
+	int i, ret;
+
+	reg = !strcmp(attr->attr.name, "open") ? SKY81452_REG5 : SKY81452_REG4;
+	ret = regmap_read(regmap, reg, &value);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	if (value & SKY81452_SHRT) {
+		*buf = 0;
+		for (i = 0; i < 6; i++) {
+			if (value & 0x01) {
+				sprintf(tmp, "%d ", i + 1);
+				strcat(buf, tmp);
+			}
+			value >>= 1;
+		}
+		strcat(buf, "\n");
+	} else {
+		strcpy(buf, "none\n");
+	}
+
+	return strlen(buf);
+}
+
+static ssize_t sky81452_bl_show_fault(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct regmap *regmap = bl_get_data(to_backlight_device(dev));
+	unsigned int value = 0;
+	int ret;
+
+	ret = regmap_read(regmap, SKY81452_REG4, &value);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	*buf = 0;
+
+	if (value & SKY81452_OCP)
+		strcat(buf, "over-current ");
+
+	if (value & SKY81452_OTMP)
+		strcat(buf, "over-temperature");
+
+	strcat(buf, "\n");
+	return strlen(buf);
+}
+
+static DEVICE_ATTR(enable, S_IWGRP | S_IWUSR, NULL, sky81452_bl_store_enable);
+static DEVICE_ATTR(open, S_IRUGO, sky81452_bl_show_open_short, NULL);
+static DEVICE_ATTR(short, S_IRUGO, sky81452_bl_show_open_short, NULL);
+static DEVICE_ATTR(fault, S_IRUGO, sky81452_bl_show_fault, NULL);
+
+static struct attribute *sky81452_bl_attribute[] = {
+	&dev_attr_enable.attr,
+	&dev_attr_open.attr,
+	&dev_attr_short.attr,
+	&dev_attr_fault.attr,
+	NULL
+};
+
+static const struct attribute_group sky81452_bl_attr_group = {
+	.attrs = sky81452_bl_attribute,
+};
+
+#ifdef CONFIG_OF
+static struct sky81452_bl_platform_data *sky81452_bl_parse_dt(
+							struct device *dev)
+{
+	struct device_node *np = of_node_get(dev->of_node);
+	struct sky81452_bl_platform_data *pdata;
+	int num_entry;
+	unsigned int sources[6];
+	int ret;
+
+	if (!np) {
+		dev_err(dev, "backlight node not found.\n");
+		return ERR_PTR(-ENODATA);
+	}
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		of_node_put(np);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	of_property_read_string(np, "name", &pdata->name);
+	pdata->ignore_pwm = of_property_read_bool(np, "skyworks,ignore-pwm");
+	pdata->dpwm_mode = of_property_read_bool(np, "skyworks,dpwm-mode");
+	pdata->phase_shift = of_property_read_bool(np, "skyworks,phase-shift");
+	pdata->gpio_enable = of_get_gpio(np, 0);
+
+	ret = of_property_count_u32_elems(np, "led-sources");
+	if (IS_ERR_VALUE(ret)) {
+		pdata->enable = SKY81452_EN >> CTZ(SKY81452_EN);
+	} else {
+		num_entry = ret;
+		if (num_entry > 6)
+			num_entry = 6;
+
+		ret = of_property_read_u32_array(np, "led-sources", sources,
+					num_entry);
+		if (IS_ERR_VALUE(ret)) {
+			dev_err(dev, "led-sources node is invalid.\n");
+			return ERR_PTR(-EINVAL);
+		}
+
+		pdata->enable = 0;
+		while (--num_entry)
+			pdata->enable |= (1 << sources[num_entry]);
+	}
+
+	ret = of_property_read_u32(np,
+			"skyworks,short-detection-threshold-volt",
+			&pdata->short_detection_threshold);
+	if (IS_ERR_VALUE(ret))
+		pdata->short_detection_threshold = 7;
+
+	ret = of_property_read_u32(np, "skyworks,current-limit-mA",
+			&pdata->boost_current_limit);
+	if (IS_ERR_VALUE(ret))
+		pdata->boost_current_limit = 2750;
+
+	of_node_put(np);
+	return pdata;
+}
+#else
+static struct sky81452_bl_platform_data *sky81452_bl_parse_dt(
+							struct device *dev)
+{
+	return ERR_PTR(-EINVAL);
+}
+#endif
+
+static int sky81452_bl_init_device(struct regmap *regmap,
+		struct sky81452_bl_platform_data *pdata)
+{
+	unsigned int value;
+
+	value = pdata->ignore_pwm ? SKY81452_IGPW : 0;
+	value |= pdata->dpwm_mode ? SKY81452_PWMMD : 0;
+	value |= pdata->phase_shift ? 0 : SKY81452_PHASE;
+
+	if (pdata->boost_current_limit == 2300)
+		value |= SKY81452_ILIM;
+	else if (pdata->boost_current_limit != 2750)
+		return -EINVAL;
+
+	if (pdata->short_detection_threshold < 4 ||
+				pdata->short_detection_threshold > 7)
+		return -EINVAL;
+	value |= (7 - pdata->short_detection_threshold) << CTZ(SKY81452_VSHRT);
+
+	return regmap_write(regmap, SKY81452_REG2, value);
+}
+
+static int sky81452_bl_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct regmap *regmap = dev_get_drvdata(dev->parent);
+	struct sky81452_bl_platform_data *pdata = dev_get_platdata(dev);
+	struct backlight_device *bd;
+	struct backlight_properties props;
+	const char *name;
+	int ret;
+
+	if (!pdata) {
+		pdata = sky81452_bl_parse_dt(dev);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+	}
+
+	if (gpio_is_valid(pdata->gpio_enable)) {
+		ret = devm_gpio_request_one(dev, pdata->gpio_enable,
+					GPIOF_OUT_INIT_HIGH, "sky81452-en");
+		if (IS_ERR_VALUE(ret)) {
+			dev_err(dev, "failed to request GPIO. err=%d\n", ret);
+			return ret;
+		}
+	}
+
+	ret = sky81452_bl_init_device(regmap, pdata);
+	if (IS_ERR_VALUE(ret)) {
+		dev_err(dev, "failed to initialize. err=%d\n", ret);
+		return ret;
+	}
+
+	memset(&props, 0, sizeof(props));
+	props.max_brightness = SKY81452_MAX_BRIGHTNESS,
+	name = pdata->name ? pdata->name : SKY81452_DEFAULT_NAME;
+	bd = devm_backlight_device_register(dev, name, dev, regmap,
+						&sky81452_bl_ops, &props);
+	if (IS_ERR(bd)) {
+		dev_err(dev, "failed to register. err=%ld\n", PTR_ERR(bd));
+		return PTR_ERR(bd);
+	}
+
+	platform_set_drvdata(pdev, bd);
+
+	ret  = sysfs_create_group(&bd->dev.kobj, &sky81452_bl_attr_group);
+	if (IS_ERR_VALUE(ret)) {
+		dev_err(dev, "failed to create attribute. err=%d\n", ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int sky81452_bl_remove(struct platform_device *pdev)
+{
+	const struct sky81452_bl_platform_data *pdata =
+						dev_get_platdata(&pdev->dev);
+	struct backlight_device *bd = platform_get_drvdata(pdev);
+
+	sysfs_remove_group(&bd->dev.kobj, &sky81452_bl_attr_group);
+
+	bd->props.power = FB_BLANK_UNBLANK;
+	bd->props.brightness = 0;
+	backlight_update_status(bd);
+
+	if (gpio_is_valid(pdata->gpio_enable))
+		gpio_set_value_cansleep(pdata->gpio_enable, 0);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id sky81452_bl_of_match[] = {
+	{ .compatible = "skyworks,sky81452-backlight", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sky81452_bl_of_match);
+#endif
+
+static struct platform_driver sky81452_bl_driver = {
+	.driver = {
+		.name = "sky81452-backlight",
+		.of_match_table = of_match_ptr(sky81452_bl_of_match),
+	},
+	.probe = sky81452_bl_probe,
+	.remove = sky81452_bl_remove,
+};
+
+module_platform_driver(sky81452_bl_driver);
+
+MODULE_DESCRIPTION("Skyworks SKY81452 backlight driver");
+MODULE_AUTHOR("Gyungoh Yoo <jack.yoo@skyworksinc.com>");
+MODULE_LICENSE("GPL v2");
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/diag288_wdt.c b/drivers/watchdog/diag288_wdt.c
index 429494b6c822..a9a5210143ae 100644
--- a/drivers/watchdog/diag288_wdt.c
+++ b/drivers/watchdog/diag288_wdt.c
@@ -125,9 +125,7 @@ static int wdt_start(struct watchdog_device *dev)
 		ret = __diag288_vm(func, dev->timeout, ebc_cmd, len);
 		WARN_ON(ret != 0);
 		kfree(ebc_cmd);
-	}
-
-	if (MACHINE_IS_LPAR) {
+	} else {
 		ret = __diag288_lpar(WDT_FUNC_INIT,
 				     dev->timeout, LPARWDT_RESTART);
 	}
@@ -136,7 +134,6 @@ static int wdt_start(struct watchdog_device *dev)
 		pr_err("The watchdog cannot be activated\n");
 		return ret;
 	}
-	pr_info("The watchdog was activated\n");
 	return 0;
 }
 
@@ -145,7 +142,6 @@ static int wdt_stop(struct watchdog_device *dev)
 	int ret;
 
 	ret = __diag288(WDT_FUNC_CANCEL, 0, 0, 0);
-	pr_info("The watchdog was deactivated\n");
 	return ret;
 }
 
@@ -177,10 +173,9 @@ static int wdt_ping(struct watchdog_device *dev)
 		ret = __diag288_vm(func, dev->timeout, ebc_cmd, len);
 		WARN_ON(ret != 0);
 		kfree(ebc_cmd);
-	}
-
-	if (MACHINE_IS_LPAR)
+	} else {
 		ret = __diag288_lpar(WDT_FUNC_CHANGE, dev->timeout, 0);
+	}
 
 	if (ret)
 		pr_err("The watchdog timer cannot be started or reset\n");
@@ -202,7 +197,7 @@ static struct watchdog_ops wdt_ops = {
 };
 
 static struct watchdog_info wdt_info = {
-	.options = WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE,
+	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
 	.firmware_version = 0,
 	.identity = "z Watchdog",
 };
@@ -273,21 +268,16 @@ static int __init diag288_init(void)
 	watchdog_set_nowayout(&wdt_dev, nowayout_info);
 
 	if (MACHINE_IS_VM) {
-		pr_info("The watchdog device driver detected a z/VM environment\n");
 		if (__diag288_vm(WDT_FUNC_INIT, 15,
 				 ebc_begin, sizeof(ebc_begin)) != 0) {
 			pr_err("The watchdog cannot be initialized\n");
 			return -EINVAL;
 		}
-	} else if (MACHINE_IS_LPAR) {
-		pr_info("The watchdog device driver detected an LPAR environment\n");
+	} else {
 		if (__diag288_lpar(WDT_FUNC_INIT, 30, LPARWDT_RESTART)) {
 			pr_err("The watchdog cannot be initialized\n");
 			return -EINVAL;
 		}
-	} else {
-		pr_err("Linux runs in an environment that does not support the diag288 watchdog\n");
-		return -ENODEV;
 	}
 
 	if (__diag288_lpar(WDT_FUNC_CANCEL, 0, 0)) {
diff --git a/drivers/watchdog/iTCO_wdt.c b/drivers/watchdog/iTCO_wdt.c
index 05ee0bf88ce9..3c3fd417ddeb 100644
--- a/drivers/watchdog/iTCO_wdt.c
+++ b/drivers/watchdog/iTCO_wdt.c
@@ -51,6 +51,7 @@
 #define DRV_VERSION	"1.11"
 
 /* Includes */
+#include <linux/acpi.h>			/* For ACPI support */
 #include <linux/module.h>		/* For module specific items */
 #include <linux/moduleparam.h>		/* For new moduleparam's */
 #include <linux/types.h>		/* For standard types (like size_t) */
@@ -103,6 +104,8 @@ static struct {		/* this is private data for the iTCO_wdt device */
 	struct platform_device *dev;
 	/* the PCI-device */
 	struct pci_dev *pdev;
+	/* whether or not the watchdog has been suspended */
+	bool suspended;
 } iTCO_wdt_private;
 
 /* module parameters */
@@ -571,12 +574,60 @@ static void iTCO_wdt_shutdown(struct platform_device *dev)
 	iTCO_wdt_stop(NULL);
 }
 
+#ifdef CONFIG_PM_SLEEP
+/*
+ * Suspend-to-idle requires this, because it stops the ticks and timekeeping, so
+ * the watchdog cannot be pinged while in that state.  In ACPI sleep states the
+ * watchdog is stopped by the platform firmware.
+ */
+
+#ifdef CONFIG_ACPI
+static inline bool need_suspend(void)
+{
+	return acpi_target_system_state() == ACPI_STATE_S0;
+}
+#else
+static inline bool need_suspend(void) { return true; }
+#endif
+
+static int iTCO_wdt_suspend_noirq(struct device *dev)
+{
+	int ret = 0;
+
+	iTCO_wdt_private.suspended = false;
+	if (watchdog_active(&iTCO_wdt_watchdog_dev) && need_suspend()) {
+		ret = iTCO_wdt_stop(&iTCO_wdt_watchdog_dev);
+		if (!ret)
+			iTCO_wdt_private.suspended = true;
+	}
+	return ret;
+}
+
+static int iTCO_wdt_resume_noirq(struct device *dev)
+{
+	if (iTCO_wdt_private.suspended)
+		iTCO_wdt_start(&iTCO_wdt_watchdog_dev);
+
+	return 0;
+}
+
+static struct dev_pm_ops iTCO_wdt_pm = {
+	.suspend_noirq = iTCO_wdt_suspend_noirq,
+	.resume_noirq = iTCO_wdt_resume_noirq,
+};
+
+#define ITCO_WDT_PM_OPS	(&iTCO_wdt_pm)
+#else
+#define ITCO_WDT_PM_OPS	NULL
+#endif /* CONFIG_PM_SLEEP */
+
 static struct platform_driver iTCO_wdt_driver = {
 	.probe          = iTCO_wdt_probe,
 	.remove         = iTCO_wdt_remove,
 	.shutdown       = iTCO_wdt_shutdown,
 	.driver         = {
 		.name   = DRV_NAME,
+		.pm     = ITCO_WDT_PM_OPS,
 	},
 };
 
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/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/tmem.c b/drivers/xen/tmem.c
index 8a65423bc696..c4211a31612d 100644
--- a/drivers/xen/tmem.c
+++ b/drivers/xen/tmem.c
@@ -397,13 +397,15 @@ static int __init xen_tmem_init(void)
 #ifdef CONFIG_CLEANCACHE
 	BUG_ON(sizeof(struct cleancache_filekey) != sizeof(struct tmem_oid));
 	if (tmem_enabled && cleancache) {
-		char *s = "";
-		struct cleancache_ops *old_ops =
-			cleancache_register_ops(&tmem_cleancache_ops);
-		if (old_ops)
-			s = " (WARNING: cleancache_ops overridden)";
-		pr_info("cleancache enabled, RAM provided by Xen Transcendent Memory%s\n",
-			s);
+		int err;
+
+		err = cleancache_register_ops(&tmem_cleancache_ops);
+		if (err)
+			pr_warn("xen-tmem: failed to enable cleancache: %d\n",
+				err);
+		else
+			pr_info("cleancache enabled, RAM provided by "
+				"Xen Transcendent Memory\n");
 	}
 #endif
 #ifdef CONFIG_XEN_SELFBALLOONING
diff --git a/drivers/xen/xen-scsiback.c b/drivers/xen/xen-scsiback.c
index 9faca6a60bb0..42bd55a6c237 100644
--- a/drivers/xen/xen-scsiback.c
+++ b/drivers/xen/xen-scsiback.c
@@ -1659,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;